WebSVN - NaviCtrl - Diff - Rev 908 and 916


#include <stdio.h>
#include <string.h>
#include "91x_lib.h"
#include "CamCtrl.h"
#include "i2c.h"
#include "timer1.h"
#include "timer2.h"
#include "led.h"
#include "main.h"
#include "uart1.h"
#include "compass.h"
#include "spi_slave.h"
#include "triggerlog.h"
 
FromCamCtrl_t FromCamCtrl;
ToCamCtrl_t ToCamCtrl;
FromLaserCtrl_t FromLaserCtrl;
ToLaserCtrl_t ToLaserCtrl;
FromGimbalCtrl_t FromGimbalCtrl;
ToGimbalCtrl_t ToGimbalCtrl;
 
s32 FromMenuGimbalYaw = 0,FromMenuServoNickControl = 0, MenuNickGimbalOffset = 0;
u16 CamCtrlTimeout = 31000;
u16 LaserCtrlTimeout = 5000;
u16 GimbalCtrlTimeout = 8000;
u16 I2C0_Timeout = 0;
u8      ResetNickServoValue = 1;
 
void InitCamCtrl(void)
{
 ToCamCtrl.CamCommand = 0;
 ToCamCtrl.ZoomInput = 128; // Middle -> no zoom
}
 
void CamCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)
{       // if crc is ok and number of byte are matching
 static u8 speak_rec_on = 1;
 static u8 speak_rec_off = 0;
 
  memcpy((u8 *)&FromCamCtrl, pRxBuffer, sizeof(FromCamCtrl));
//if(FromCamCtrl.CamStatus & CAM_STATE_PIC_CAPTURED) 
        if(FromCamCtrl.CamStatus & CAM_STATE_REC_ACTIVE)                CamCtrlCharacter = 'R';
        else if(FromCamCtrl.CamStatus & CAM_STATE_PHOTO_MODE)   CamCtrlCharacter = 'P';
        else if(FromCamCtrl.CamStatus & CAM_STATE_CAM_DISCONN)  CamCtrlCharacter = '?';
        else if(FromCamCtrl.CamStatus & CAM_STATE_OFF)                  CamCtrlCharacter = '!';
        else if(TrigLogging.CountExternal)                                              CamCtrlCharacter = TrigLogging.CountExternal % 10 + '0';
        else if(FromCamCtrl.PhotoCount)                                                 CamCtrlCharacter = FromCamCtrl.PhotoCount % 10 + '0';
        else if(FromCamCtrl.CamStatus & CAM_STATE_RDY)                  CamCtrlCharacter = 'c';
        else  CamCtrlCharacter = ' ';
 
// c = camera is ready
// R = Record active
// P = Cam in Photo mode (LANC Camcorders)
// ! = Camera is off but connected
// ? = disconnected
// 0-9 = Photo releases
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Speak
//+++++++++++++++++++++++++++++++++++++++++++++++++++
 if(!SpeakHoTT)
  {
        if(FromCamCtrl.CamStatus & CAM_STATE_REC_ACTIVE)        
         {
          if(speak_rec_on) SpeakHoTT = SPEAK_REC_START;
          speak_rec_on = 0;
          speak_rec_off = 1;
         }
         else
         {
          if(speak_rec_off)  SpeakHoTT = SPEAK_REC_STOP;
          speak_rec_on = 1;
          speak_rec_off = 0;
         }
  }
//+++++++++++++++++++++++++++++++++++++++++++++++++++
 
  if(FromCamCtrl.CamStatus & CAM_STATE_I2C_OK) CamCtrlTimeout = 19000;
  I2C0_Timeout = 31000;
}
 
void CamCtrl_GetData(u8 timeout)
{
 static u8 timing = 250, p63 = 0;
 static u8 force1photo = 0, delay ;             // makes one photo when switching from Off to Photo
 
 if(timing)
   {
    timing--;
   }
   else
        // try to catch the I2C buffer within timeout ms
   if(I2CBus_LockBuffer(I2C0, timeout))
        {
                u8 RxBytes = sizeof(FromCamCtrl);
                // initiate transmission
                ToCamCtrl.ZoomInput = 128 + PPM_In[EE_Parameter.CamCtrlZoomChannel];
                if(EE_Parameter.CamCtrlModeChannel)
                 {
                        if(PPM_In[EE_Parameter.CamCtrlModeChannel] > 50)        // max
                           {
                        ToCamCtrl.CamCommand &= ~CAM_CMD_REC_OFF;
                            ToCamCtrl.CamCommand |= CAM_CMD_REC_ON;
                                force1photo = 0;
                           }
                        else
                        if(PPM_In[EE_Parameter.CamCtrlModeChannel] < -50)        // min
                           {
                        ToCamCtrl.CamCommand &= ~CAM_CMD_REC_ON;
                            ToCamCtrl.CamCommand |= CAM_CMD_REC_OFF;
                                force1photo = 1;
                                p63 = 0;
                                delay = 5;
                                }
                    else         // Middle
                          {
                           if(delay) delay--;
                                else
                           {
                        if(force1photo || ((UART_VersionInfo.HWMajor >= 30) && TRIGGER_PP_INTERN) || ((UART_VersionInfo.HWMajor < 30) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE))) // internal Portpin or Flag
                                 {
                                  if(!p63) ToCamCtrl.CamCommand |= CAM_CMD_SHUTTER; // combined with the FC trigger output
                                  p63 = 1;
                                  force1photo = 0;
                                 }
                                 else p63 = 0;
                           }
                          }
                }
 
        I2CBus_Transmission(I2C0, CAM_SLAVE_ADDRESS, &ToCamCtrl, sizeof(ToCamCtrl)+1, &CamCtrl_UpdateData, RxBytes);
 
        if(!EE_Parameter.CamCtrlModeChannel) ToCamCtrl.CamCommand = 0; // delete REC_OFF
        ToCamCtrl.CamCommand = CAM_CMD_SWITCH_ON | (ToCamCtrl.CamCommand & (CAM_CMD_REC_ON | CAM_CMD_REC_OFF));
        timing = 66; // 66ms = 15Hz
   }
   else timing = 11; // try again in 11ms
}
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Laser
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void LaserCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)
{       // if crc is ok and number of byte are matching
  memcpy((u8 *)&FromLaserCtrl, pRxBuffer, sizeof(FromLaserCtrl));
  DebugOut.Analog[13] = FromLaserCtrl.Distance;
  if((FromLaserCtrl.LaserStatus & LASER_DATA_OK) && (FromLaserCtrl.Distance == 0)) FromLaserCtrl.Distance = 1; // just to mark that the value is active (0 -> not connected)
  LaserCtrlTimeout = 30000;
  I2C0_Timeout = 31000;
}
 
 
void LaserCtrl_GetData(u8 timeout)
{
 static u8 timing = 250;
 
 if(timing)
   {
    timing--;
   }
   else
        // try to catch the I2C buffer within timeout ms
   if(I2CBus_LockBuffer(I2C0, timeout))
        {
                u8 RxBytes = 3;//sizeof(FromLaserCtrl);
                // initiate transmission
        I2CBus_Transmission(I2C0, LASER_SLAVE_ADDRESS, &ToLaserCtrl, sizeof(ToLaserCtrl)+1, &LaserCtrl_UpdateData, RxBytes);
        timing = 66; // 66ms = 15Hz
   }
   else timing = 11; // try again in 11ms
}
 
 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
//+ Gimbal
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 
void GimbalCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)
{       // if crc is ok and number of byte are matching
  memcpy((u8 *)&FromGimbalCtrl, pRxBuffer, sizeof(FromGimbalCtrl));
  GimbalCtrlTimeout = 30000;
  I2C0_Timeout = 31000;
}
 
 
void GimbalCtrl_GetData(u8 timeout)
{
 static u32 timing = 500, start_idle = 4000;
 static s16 old_nick;
 s16 tmp, NickGimbal;
#define GIMBAL_DATARATE  66       // 66ms = 15Hz bzw. 30Hz 
 
 tmp = ((signed int)NickServoValue - 8192) / 7;
 if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) NickGimbal = -tmp; else NickGimbal = tmp;
 NickGimbal += CAM_Orientation.Elevation; // Add Poi Nick
 NickGimbal += MenuNickGimbalOffset / 50;
 if((old_nick != NickGimbal) && (timing < GIMBAL_DATARATE/2)) timing = 0;  // Schneller, wenn sich die Daten ändern
 
 if(timing)
   {
    timing--;
    if(start_idle) start_idle--;// nach dem Start einige Sekunden warten
   }
  else
  {
        // try to catch the I2C buffer within timeout ms
//DebugOut.Analog[]++;
   if(I2CBus_LockBuffer(I2C0, timeout))
        {
                u8 i, RxBytes = 1+6; // Status + 3 * Angle
                u8 *dat;
                s32 min_n, max_n, min_r, max_r;
                // initiate transmission
                tmp = (s16) (FromFC_ServoRollControl - 128) * 4;
                if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) ToGimbalCtrl.Roll = tmp; else ToGimbalCtrl.Roll = -tmp;
//              ToGimbalCtrl.Yaw = (s32) SerialChannel.Ch[0] * 3;
                ToGimbalCtrl.Yaw = (s32) (PPM_In[EE_Parameter.GimbalYawChannel]) * -1; // war bis 1.21 * -2 -> das Gimbal nimmt aber seltsamerweise manchmal nur +-200 an
 
// ++++++++++++++++++++++++++++++++++++++++++++++
// digital switching outputs 
            if(PPM_In[EE_Parameter.GimbalOut1Channel] > 10) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT1;
        if(PPM_In[EE_Parameter.GimbalOut2Channel] > 10) ToGimbalCtrl.BitCmd |= GIMBAL_CMD_OUT2;                
// flags will be reset after transmitting
// ++++++++++++++++++++++++++++++++++++++++++++++
 
                ToGimbalCtrl.NRY_Speed = 100;
                ToGimbalCtrl.Filter = 5;
                ToGimbalCtrl.Nick = NickGimbal;
                ToGimbalCtrl.Compass = GyroCompassCorrected;
// ++++++++++++++++++++++++++++++++++++++++++++++
// + limit angles (0 = -128°  255 = +127°)
// ++++++++++++++++++++++++++++++++++++++++++++++
                if(FC.RC_Quality < 50) // Failsafe-Positions
                 {
                  if(EE_Parameter.ServoFS_Pos[0]) ToGimbalCtrl.Nick = (s32)((u32)EE_Parameter.ServoFS_Pos[0] * 10) - 1280; //else ToGimbalCtrl.Nick = 0;
                  if(EE_Parameter.ServoFS_Pos[1]) ToGimbalCtrl.Roll = (s32)((u32)EE_Parameter.ServoFS_Pos[1] * 10) - 1280; //else ToGimbalCtrl.Roll = 0;
                  ToGimbalCtrl.Yaw = 0;
                 }
                if(FromMenuGimbalYaw) ToGimbalCtrl.Yaw = FromMenuGimbalYaw;
 
                min_n = (s32)((u32)EE_Parameter.ServoNickMin * 10) - 1280;
                max_n = (s32)((u32)EE_Parameter.ServoNickMax * 10) - 1280;
        if(ToGimbalCtrl.Nick > max_n) ToGimbalCtrl.Nick = max_n;
        if(ToGimbalCtrl.Nick < min_n) ToGimbalCtrl.Nick = min_n;
                min_r = (s32)((u32)EE_Parameter.ServoRollMin * 10) - 1280;
                max_r = (s32)((u32)EE_Parameter.ServoRollMax * 10) - 1280;
        if(ToGimbalCtrl.Roll > max_r) ToGimbalCtrl.Roll = max_r;
        if(ToGimbalCtrl.Roll < min_r) ToGimbalCtrl.Roll = min_r;
// ++++++++++++++++++++++++++++++++++++++++++++++
        if(start_idle) // nach dem Start einige Sekunden warten
          {
           ToGimbalCtrl.Nick = 0;
           ToGimbalCtrl.Roll = 0;
           ToGimbalCtrl.Yaw = 0;
           ToGimbalCtrl.Compass = -1;
          }
// ++++++++++++++++++++++++++++++++++++++++++++++
// + Crc
// ++++++++++++++++++++++++++++++++++++++++++++++
                ToGimbalCtrl.Crc = 0x55; // initialize 
                dat = &ToGimbalCtrl; // pointer 
                for(i=0; i< sizeof(ToGimbalCtrl) - 1; i++) ToGimbalCtrl.Crc += dat[i];
// ++++++++++++++++++++++++++++++++++++++++++++++
RxBytes = sizeof(FromGimbalCtrl);
 
                I2CBus_Transmission(I2C0, GIMBAL_SLAVE_ADDRESS, &ToGimbalCtrl, sizeof(ToGimbalCtrl)+1, &GimbalCtrl_UpdateData, RxBytes);
                timing = GIMBAL_DATARATE;
                ToGimbalCtrl.BitCmd = GIMBAL_CMD_SPEED; // reset the Flags
                old_nick = NickGimbal;
 
   }
   else  timing = 11; // try again in 11ms 
  }
}
 
void CalcNickServoValue(void)
{
 signed int max, min;
#define MULTIPLYER 4
 if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) // relative moving of the servo value
  {
        max = ((unsigned int) EE_Parameter.ServoNickMax * MULTIPLYER * 15);
        min = ((unsigned int) EE_Parameter.ServoNickMin * MULTIPLYER * 20);
        NickServoValue -= ((signed char) (FromFC_ServoNickControl - 128) / 4) * 2; // * 6
        if(ResetNickServoValue) NickServoValue = 8192; //  middle Position
        ResetNickServoValue = 0;
        LIMIT_MIN_MAX(NickServoValue,min, max);
  }
  else NickServoValue = (int16_t)FromFC_ServoNickControl * (MULTIPLYER*16);  // direct poti control
  MenuNickGimbalOffset += FromMenuServoNickControl;
  LIMIT_MIN_MAX(MenuNickGimbalOffset,-50000,50000);
}
 
 
 
 
 
 
 

Rev 908	Rev 916
1	#include <stdio.h>	1	#include <stdio.h>
2	#include <string.h>	2	#include <string.h>
3	#include "91x_lib.h"	3	#include "91x_lib.h"
4	#include "CamCtrl.h"	4	#include "CamCtrl.h"
5	#include "i2c.h"	5	#include "i2c.h"
6	#include "timer1.h"	6	#include "timer1.h"
7	#include "timer2.h"	7	#include "timer2.h"
8	#include "led.h"	8	#include "led.h"
9	#include "main.h"	9	#include "main.h"
10	#include "uart1.h"	10	#include "uart1.h"
11	#include "compass.h"	11	#include "compass.h"
12	#include "spi_slave.h"	12	#include "spi_slave.h"
13	#include "triggerlog.h"	13	#include "triggerlog.h"
14		14
15	FromCamCtrl_t FromCamCtrl;	15	FromCamCtrl_t FromCamCtrl;
16	ToCamCtrl_t ToCamCtrl;	16	ToCamCtrl_t ToCamCtrl;
17	FromLaserCtrl_t FromLaserCtrl;	17	FromLaserCtrl_t FromLaserCtrl;
18	ToLaserCtrl_t ToLaserCtrl;	18	ToLaserCtrl_t ToLaserCtrl;
19	FromGimbalCtrl_t FromGimbalCtrl;	19	FromGimbalCtrl_t FromGimbalCtrl;
20	ToGimbalCtrl_t ToGimbalCtrl;	20	ToGimbalCtrl_t ToGimbalCtrl;
21		21
22	s32 FromMenuGimbalYaw = 0,FromMenuServoNickControl = 0, MenuNickGimbalOffset = 0;	22	s32 FromMenuGimbalYaw = 0,FromMenuServoNickControl = 0, MenuNickGimbalOffset = 0;
23	u16 CamCtrlTimeout = 31000;	23	u16 CamCtrlTimeout = 31000;
24	u16 LaserCtrlTimeout = 5000;	24	u16 LaserCtrlTimeout = 5000;
25	u16 GimbalCtrlTimeout = 8000;	25	u16 GimbalCtrlTimeout = 8000;
26	u16 I2C0_Timeout = 0;	26	u16 I2C0_Timeout = 0;
27	u8 ResetNickServoValue = 1;	27	u8 ResetNickServoValue = 1;
28		28
29	void InitCamCtrl(void)	29	void InitCamCtrl(void)
30	{	30	{
31	ToCamCtrl.CamCommand = 0;	31	ToCamCtrl.CamCommand = 0;
32	ToCamCtrl.ZoomInput = 128; // Middle -> no zoom	32	ToCamCtrl.ZoomInput = 128; // Middle -> no zoom
33	}	33	}
34		34
35	void CamCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)	35	void CamCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)
36	{ // if crc is ok and number of byte are matching	36	{ // if crc is ok and number of byte are matching
37	static u8 speak_rec_on = 1;	37	static u8 speak_rec_on = 1;
38	static u8 speak_rec_off = 0;	38	static u8 speak_rec_off = 0;
39		39
40	memcpy((u8 *)&FromCamCtrl, pRxBuffer, sizeof(FromCamCtrl));	40	memcpy((u8 *)&FromCamCtrl, pRxBuffer, sizeof(FromCamCtrl));
41	//if(FromCamCtrl.CamStatus & CAM_STATE_PIC_CAPTURED)	41	//if(FromCamCtrl.CamStatus & CAM_STATE_PIC_CAPTURED)
42	if(FromCamCtrl.CamStatus & CAM_STATE_REC_ACTIVE) CamCtrlCharacter = 'R';	42	if(FromCamCtrl.CamStatus & CAM_STATE_REC_ACTIVE) CamCtrlCharacter = 'R';
43	else if(FromCamCtrl.CamStatus & CAM_STATE_PHOTO_MODE) CamCtrlCharacter = 'P';	43	else if(FromCamCtrl.CamStatus & CAM_STATE_PHOTO_MODE) CamCtrlCharacter = 'P';
44	else if(FromCamCtrl.CamStatus & CAM_STATE_CAM_DISCONN) CamCtrlCharacter = '?';	44	else if(FromCamCtrl.CamStatus & CAM_STATE_CAM_DISCONN) CamCtrlCharacter = '?';
45	else if(FromCamCtrl.CamStatus & CAM_STATE_OFF) CamCtrlCharacter = '!';	45	else if(FromCamCtrl.CamStatus & CAM_STATE_OFF) CamCtrlCharacter = '!';
46	else if(TrigLogging.CountExternal) CamCtrlCharacter = TrigLogging.CountExternal % 10 + '0';	46	else if(TrigLogging.CountExternal) CamCtrlCharacter = TrigLogging.CountExternal % 10 + '0';
47	else if(FromCamCtrl.PhotoCount) CamCtrlCharacter = FromCamCtrl.PhotoCount % 10 + '0';	47	else if(FromCamCtrl.PhotoCount) CamCtrlCharacter = FromCamCtrl.PhotoCount % 10 + '0';
48	else if(FromCamCtrl.CamStatus & CAM_STATE_RDY) CamCtrlCharacter = 'c';	48	else if(FromCamCtrl.CamStatus & CAM_STATE_RDY) CamCtrlCharacter = 'c';
49	else CamCtrlCharacter = ' ';	49	else CamCtrlCharacter = ' ';
50		50
51	// c = camera is ready	51	// c = camera is ready
52	// R = Record active	52	// R = Record active
53	// P = Cam in Photo mode (LANC Camcorders)	53	// P = Cam in Photo mode (LANC Camcorders)
54	// ! = Camera is off but connected	54	// ! = Camera is off but connected
55	// ? = disconnected	55	// ? = disconnected
56	// 0-9 = Photo releases	56	// 0-9 = Photo releases
57		57
58	//+++++++++++++++++++++++++++++++++++++++++++++++++++	58	//+++++++++++++++++++++++++++++++++++++++++++++++++++
59	//+ Speak	59	//+ Speak
60	//+++++++++++++++++++++++++++++++++++++++++++++++++++	60	//+++++++++++++++++++++++++++++++++++++++++++++++++++
61	if(!SpeakHoTT)	61	if(!SpeakHoTT)
62	{	62	{
63	if(FromCamCtrl.CamStatus & CAM_STATE_REC_ACTIVE)	63	if(FromCamCtrl.CamStatus & CAM_STATE_REC_ACTIVE)
64	{	64	{
65	if(speak_rec_on) SpeakHoTT = SPEAK_REC_START;	65	if(speak_rec_on) SpeakHoTT = SPEAK_REC_START;
66	speak_rec_on = 0;	66	speak_rec_on = 0;
67	speak_rec_off = 1;	67	speak_rec_off = 1;
68	}	68	}
69	else	69	else
70	{	70	{
71	if(speak_rec_off) SpeakHoTT = SPEAK_REC_STOP;	71	if(speak_rec_off) SpeakHoTT = SPEAK_REC_STOP;
72	speak_rec_on = 1;	72	speak_rec_on = 1;
73	speak_rec_off = 0;	73	speak_rec_off = 0;
74	}	74	}
75	}	75	}
76	//+++++++++++++++++++++++++++++++++++++++++++++++++++	76	//+++++++++++++++++++++++++++++++++++++++++++++++++++
77		77
78	if(FromCamCtrl.CamStatus & CAM_STATE_I2C_OK) CamCtrlTimeout = 19000;	78	if(FromCamCtrl.CamStatus & CAM_STATE_I2C_OK) CamCtrlTimeout = 19000;
79	I2C0_Timeout = 31000;	79	I2C0_Timeout = 31000;
80	}	80	}
81		81
82	void CamCtrl_GetData(u8 timeout)	82	void CamCtrl_GetData(u8 timeout)
83	{	83	{
84	static u8 timing = 250, p63 = 0;	84	static u8 timing = 250, p63 = 0;
85	static u8 force1photo = 0, delay ; // makes one photo when switching from Off to Photo	85	static u8 force1photo = 0, delay ; // makes one photo when switching from Off to Photo
86		86
87	if(timing)	87	if(timing)
88	{	88	{
89	timing--;	89	timing--;
90	}	90	}
91	else	91	else
92	// try to catch the I2C buffer within timeout ms	92	// try to catch the I2C buffer within timeout ms
93	if(I2CBus_LockBuffer(I2C0, timeout))	93	if(I2CBus_LockBuffer(I2C0, timeout))
94	{	94	{
95	u8 RxBytes = sizeof(FromCamCtrl);	95	u8 RxBytes = sizeof(FromCamCtrl);
96	// initiate transmission	96	// initiate transmission
97	ToCamCtrl.ZoomInput = 128 + PPM_In[EE_Parameter.CamCtrlZoomChannel];	97	ToCamCtrl.ZoomInput = 128 + PPM_In[EE_Parameter.CamCtrlZoomChannel];
98	if(EE_Parameter.CamCtrlModeChannel)	98	if(EE_Parameter.CamCtrlModeChannel)
99	{	99	{
100	if(PPM_In[EE_Parameter.CamCtrlModeChannel] > 50) // max	100	if(PPM_In[EE_Parameter.CamCtrlModeChannel] > 50) // max
101	{	101	{
102	ToCamCtrl.CamCommand &= ~CAM_CMD_REC_OFF;	102	ToCamCtrl.CamCommand &= ~CAM_CMD_REC_OFF;
103	ToCamCtrl.CamCommand \|= CAM_CMD_REC_ON;	103	ToCamCtrl.CamCommand \|= CAM_CMD_REC_ON;
104	force1photo = 0;	104	force1photo = 0;
105	}	105	}
106	else	106	else
107	if(PPM_In[EE_Parameter.CamCtrlModeChannel] < -50) // min	107	if(PPM_In[EE_Parameter.CamCtrlModeChannel] < -50) // min
108	{	108	{
109	ToCamCtrl.CamCommand &= ~CAM_CMD_REC_ON;	109	ToCamCtrl.CamCommand &= ~CAM_CMD_REC_ON;
110	ToCamCtrl.CamCommand \|= CAM_CMD_REC_OFF;	110	ToCamCtrl.CamCommand \|= CAM_CMD_REC_OFF;
111	force1photo = 1;	111	force1photo = 1;
112	p63 = 0;	112	p63 = 0;
113	delay = 5;	113	delay = 5;
114	}	114	}
115	else // Middle	115	else // Middle
116	{	116	{
117	if(delay) delay--;	117	if(delay) delay--;
118	else	118	else
119	{	119	{
120	if(force1photo \|\| ((UART_VersionInfo.HWMajor >= 30) && TRIGGER_PP_INTERN) \|\| ((UART_VersionInfo.HWMajor < 30) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE))) // internal Portpin or Flag	120	if(force1photo \|\| ((UART_VersionInfo.HWMajor >= 30) && TRIGGER_PP_INTERN) \|\| ((UART_VersionInfo.HWMajor < 30) && (FC.StatusFlags2 & FC_STATUS2_OUT1_ACTIVE))) // internal Portpin or Flag
121	{	121	{
122	if(!p63) ToCamCtrl.CamCommand \|= CAM_CMD_SHUTTER; // combined with the FC trigger output	122	if(!p63) ToCamCtrl.CamCommand \|= CAM_CMD_SHUTTER; // combined with the FC trigger output
123	p63 = 1;	123	p63 = 1;
124	force1photo = 0;	124	force1photo = 0;
125	}	125	}
126	else p63 = 0;	126	else p63 = 0;
127	}	127	}
128	}	128	}
129	}	129	}
130		130
131	I2CBus_Transmission(I2C0, CAM_SLAVE_ADDRESS, &ToCamCtrl, sizeof(ToCamCtrl)+1, &CamCtrl_UpdateData, RxBytes);	131	I2CBus_Transmission(I2C0, CAM_SLAVE_ADDRESS, &ToCamCtrl, sizeof(ToCamCtrl)+1, &CamCtrl_UpdateData, RxBytes);
132		132
133	if(!EE_Parameter.CamCtrlModeChannel) ToCamCtrl.CamCommand = 0; // delete REC_OFF	133	if(!EE_Parameter.CamCtrlModeChannel) ToCamCtrl.CamCommand = 0; // delete REC_OFF
134	ToCamCtrl.CamCommand = CAM_CMD_SWITCH_ON \| (ToCamCtrl.CamCommand & (CAM_CMD_REC_ON \| CAM_CMD_REC_OFF));	134	ToCamCtrl.CamCommand = CAM_CMD_SWITCH_ON \| (ToCamCtrl.CamCommand & (CAM_CMD_REC_ON \| CAM_CMD_REC_OFF));
135	timing = 66; // 66ms = 15Hz	135	timing = 66; // 66ms = 15Hz
136	}	136	}
137	else timing = 11; // try again in 11ms	137	else timing = 11; // try again in 11ms
138	}	138	}
139		139
140	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	140	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
141	//+ Laser	141	//+ Laser
142	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	142	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
143	void LaserCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)	143	void LaserCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)
144	{ // if crc is ok and number of byte are matching	144	{ // if crc is ok and number of byte are matching
145	memcpy((u8 *)&FromLaserCtrl, pRxBuffer, sizeof(FromLaserCtrl));	145	memcpy((u8 *)&FromLaserCtrl, pRxBuffer, sizeof(FromLaserCtrl));
146	DebugOut.Analog[13] = FromLaserCtrl.Distance;	146	DebugOut.Analog[13] = FromLaserCtrl.Distance;
147	if((FromLaserCtrl.LaserStatus & LASER_DATA_OK) && (FromLaserCtrl.Distance == 0)) FromLaserCtrl.Distance = 1; // just to mark that the value is active (0 -> not connected)	147	if((FromLaserCtrl.LaserStatus & LASER_DATA_OK) && (FromLaserCtrl.Distance == 0)) FromLaserCtrl.Distance = 1; // just to mark that the value is active (0 -> not connected)
148	LaserCtrlTimeout = 30000;	148	LaserCtrlTimeout = 30000;
149	I2C0_Timeout = 31000;	149	I2C0_Timeout = 31000;
150	}	150	}
151		151
152		152
153	void LaserCtrl_GetData(u8 timeout)	153	void LaserCtrl_GetData(u8 timeout)
154	{	154	{
155	static u8 timing = 250;	155	static u8 timing = 250;
156		156
157	if(timing)	157	if(timing)
158	{	158	{
159	timing--;	159	timing--;
160	}	160	}
161	else	161	else
162	// try to catch the I2C buffer within timeout ms	162	// try to catch the I2C buffer within timeout ms
163	if(I2CBus_LockBuffer(I2C0, timeout))	163	if(I2CBus_LockBuffer(I2C0, timeout))
164	{	164	{
165	u8 RxBytes = 3;//sizeof(FromLaserCtrl);	165	u8 RxBytes = 3;//sizeof(FromLaserCtrl);
166	// initiate transmission	166	// initiate transmission
167	I2CBus_Transmission(I2C0, LASER_SLAVE_ADDRESS, &ToLaserCtrl, sizeof(ToLaserCtrl)+1, &LaserCtrl_UpdateData, RxBytes);	167	I2CBus_Transmission(I2C0, LASER_SLAVE_ADDRESS, &ToLaserCtrl, sizeof(ToLaserCtrl)+1, &LaserCtrl_UpdateData, RxBytes);
168	timing = 66; // 66ms = 15Hz	168	timing = 66; // 66ms = 15Hz
169	}	169	}
170	else timing = 11; // try again in 11ms	170	else timing = 11; // try again in 11ms
171	}	171	}
172		172
173		173
174	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	174	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
175	//+ Gimbal	175	//+ Gimbal
176	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	176	//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
177		177
178	void GimbalCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)	178	void GimbalCtrl_UpdateData(u8* pRxBuffer, u8 RxBufferSize)
179	{ // if crc is ok and number of byte are matching	179	{ // if crc is ok and number of byte are matching
180	memcpy((u8 *)&FromGimbalCtrl, pRxBuffer, sizeof(FromGimbalCtrl));	180	memcpy((u8 *)&FromGimbalCtrl, pRxBuffer, sizeof(FromGimbalCtrl));
181	GimbalCtrlTimeout = 30000;	181	GimbalCtrlTimeout = 30000;
182	I2C0_Timeout = 31000;	182	I2C0_Timeout = 31000;
183	}	183	}
184		184
185		185
186	void GimbalCtrl_GetData(u8 timeout)	186	void GimbalCtrl_GetData(u8 timeout)
187	{	187	{
188	static u32 timing = 500, start_idle = 4000;	188	static u32 timing = 500, start_idle = 4000;
189	static s16 old_nick;	189	static s16 old_nick;
190	s16 tmp, NickGimbal;	190	s16 tmp, NickGimbal;
191	#define GIMBAL_DATARATE 66 // 66ms = 15Hz bzw. 30Hz	191	#define GIMBAL_DATARATE 66 // 66ms = 15Hz bzw. 30Hz
192		192
193	tmp = ((signed int)NickServoValue - 8192) / 7;	193	tmp = ((signed int)NickServoValue - 8192) / 7;
194	if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) NickGimbal = -tmp; else NickGimbal = tmp;	194	if(EE_Parameter.ServoCompInvert & SERVO_NICK_INV) NickGimbal = -tmp; else NickGimbal = tmp;
195	NickGimbal += CAM_Orientation.Elevation; // Add Poi Nick	195	NickGimbal += CAM_Orientation.Elevation; // Add Poi Nick
196	NickGimbal += MenuNickGimbalOffset / 50;	196	NickGimbal += MenuNickGimbalOffset / 50;
197	if((old_nick != NickGimbal) && (timing < GIMBAL_DATARATE/2)) timing = 0; // Schneller, wenn sich die Daten ändern	197	if((old_nick != NickGimbal) && (timing < GIMBAL_DATARATE/2)) timing = 0; // Schneller, wenn sich die Daten ändern
198		198
199	if(timing)	199	if(timing)
200	{	200	{
201	timing--;	201	timing--;
202	if(start_idle) start_idle--;// nach dem Start einige Sekunden warten	202	if(start_idle) start_idle--;// nach dem Start einige Sekunden warten
203	}	203	}
204	else	204	else
205	{	205	{
206	// try to catch the I2C buffer within timeout ms	206	// try to catch the I2C buffer within timeout ms
207	//DebugOut.Analog[]++;	207	//DebugOut.Analog[]++;
208	if(I2CBus_LockBuffer(I2C0, timeout))	208	if(I2CBus_LockBuffer(I2C0, timeout))
209	{	209	{
210	u8 i, RxBytes = 1+6; // Status + 3 * Angle	210	u8 i, RxBytes = 1+6; // Status + 3 * Angle
211	u8 *dat;	211	u8 *dat;
212	s32 min_n, max_n, min_r, max_r;	212	s32 min_n, max_n, min_r, max_r;
213	// initiate transmission	213	// initiate transmission
214	tmp = (s16) (FromFC_ServoRollControl - 128) * 4;	214	tmp = (s16) (FromFC_ServoRollControl - 128) * 4;
215	if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) ToGimbalCtrl.Roll = tmp; else ToGimbalCtrl.Roll = -tmp;	215	if(EE_Parameter.ServoCompInvert & SERVO_ROLL_INV) ToGimbalCtrl.Roll = tmp; else ToGimbalCtrl.Roll = -tmp;
216	// ToGimbalCtrl.Yaw = (s32) SerialChannel.Ch[0] * 3;	216	// ToGimbalCtrl.Yaw = (s32) SerialChannel.Ch[0] * 3;
217	ToGimbalCtrl.Yaw = (s32) (PPM_In[EE_Parameter.GimbalYawChannel]) * -1; // war bis 1.21 * -2 -> das Gimbal nimmt aber seltsamerweise manchmal nur +-200 an	217	ToGimbalCtrl.Yaw = (s32) (PPM_In[EE_Parameter.GimbalYawChannel]) * -1; // war bis 1.21 * -2 -> das Gimbal nimmt aber seltsamerweise manchmal nur +-200 an
218		218
219	// ++++++++++++++++++++++++++++++++++++++++++++++	219	// ++++++++++++++++++++++++++++++++++++++++++++++
220	// digital switching outputs	220	// digital switching outputs
221	if(PPM_In[EE_Parameter.GimbalOut1Channel] > 10) ToGimbalCtrl.BitCmd \|= GIMBAL_CMD_OUT1;	221	if(PPM_In[EE_Parameter.GimbalOut1Channel] > 10) ToGimbalCtrl.BitCmd \|= GIMBAL_CMD_OUT1;
222	if(PPM_In[EE_Parameter.GimbalOut2Channel] > 10) ToGimbalCtrl.BitCmd \|= GIMBAL_CMD_OUT2;	222	if(PPM_In[EE_Parameter.GimbalOut2Channel] > 10) ToGimbalCtrl.BitCmd \|= GIMBAL_CMD_OUT2;
223	// flags will be reset after transmitting	223	// flags will be reset after transmitting
224	// ++++++++++++++++++++++++++++++++++++++++++++++	224	// ++++++++++++++++++++++++++++++++++++++++++++++
225		225
226	ToGimbalCtrl.NRY_Speed = 100;	226	ToGimbalCtrl.NRY_Speed = 100;
227	ToGimbalCtrl.Filter = 5;	227	ToGimbalCtrl.Filter = 5;
228	ToGimbalCtrl.Nick = NickGimbal;	228	ToGimbalCtrl.Nick = NickGimbal;
229	ToGimbalCtrl.Compass = GyroCompassCorrected;	229	ToGimbalCtrl.Compass = GyroCompassCorrected;
230	// ++++++++++++++++++++++++++++++++++++++++++++++	230	// ++++++++++++++++++++++++++++++++++++++++++++++
231	// + limit angles (0 = -128° 255 = +127°)	231	// + limit angles (0 = -128° 255 = +127°)
232	// ++++++++++++++++++++++++++++++++++++++++++++++	232	// ++++++++++++++++++++++++++++++++++++++++++++++
233	if(FC.RC_Quality < 50) // Failsafe-Positions	233	if(FC.RC_Quality < 50) // Failsafe-Positions
234	{	234	{
235	if(EE_Parameter.ServoFS_Pos[0]) (s32)((u32)EE_Parameter.ServoFS_Pos[0] * 10) - 1280; //else ToGimbalCtrl.Nick = 0;	235	if(EE_Parameter.ServoFS_Pos[0]) ToGimbalCtrl.Nick = (s32)((u32)EE_Parameter.ServoFS_Pos[0] * 10) - 1280; //else ToGimbalCtrl.Nick = 0;
236	if(EE_Parameter.ServoFS_Pos[1]) (s32)((u32)EE_Parameter.ServoFS_Pos[1] * 10) - 1280; //else ToGimbalCtrl.Roll = 0;	236	if(EE_Parameter.ServoFS_Pos[1]) ToGimbalCtrl.Roll = (s32)((u32)EE_Parameter.ServoFS_Pos[1] * 10) - 1280; //else ToGimbalCtrl.Roll = 0;
237	ToGimbalCtrl.Yaw = 0;	237	ToGimbalCtrl.Yaw = 0;
238	}	238	}
239	if(FromMenuGimbalYaw) ToGimbalCtrl.Yaw = FromMenuGimbalYaw;	239	if(FromMenuGimbalYaw) ToGimbalCtrl.Yaw = FromMenuGimbalYaw;
240		240
241	min_n = (s32)((u32)EE_Parameter.ServoNickMin * 10) - 1280;	241	min_n = (s32)((u32)EE_Parameter.ServoNickMin * 10) - 1280;
242	max_n = (s32)((u32)EE_Parameter.ServoNickMax * 10) - 1280;	242	max_n = (s32)((u32)EE_Parameter.ServoNickMax * 10) - 1280;
243	if(ToGimbalCtrl.Nick > max_n) ToGimbalCtrl.Nick = max_n;	243	if(ToGimbalCtrl.Nick > max_n) ToGimbalCtrl.Nick = max_n;
244	if(ToGimbalCtrl.Nick < min_n) ToGimbalCtrl.Nick = min_n;	244	if(ToGimbalCtrl.Nick < min_n) ToGimbalCtrl.Nick = min_n;
245	min_r = (s32)((u32)EE_Parameter.ServoRollMin * 10) - 1280;	245	min_r = (s32)((u32)EE_Parameter.ServoRollMin * 10) - 1280;
246	max_r = (s32)((u32)EE_Parameter.ServoRollMax * 10) - 1280;	246	max_r = (s32)((u32)EE_Parameter.ServoRollMax * 10) - 1280;
247	if(ToGimbalCtrl.Roll > max_r) ToGimbalCtrl.Roll = max_r;	247	if(ToGimbalCtrl.Roll > max_r) ToGimbalCtrl.Roll = max_r;
248	if(ToGimbalCtrl.Roll < min_r) ToGimbalCtrl.Roll = min_r;	248	if(ToGimbalCtrl.Roll < min_r) ToGimbalCtrl.Roll = min_r;
249	// ++++++++++++++++++++++++++++++++++++++++++++++	249	// ++++++++++++++++++++++++++++++++++++++++++++++
250	if(start_idle) // nach dem Start einige Sekunden warten	250	if(start_idle) // nach dem Start einige Sekunden warten
251	{	251	{
252	ToGimbalCtrl.Nick = 0;	252	ToGimbalCtrl.Nick = 0;
253	ToGimbalCtrl.Roll = 0;	253	ToGimbalCtrl.Roll = 0;
254	ToGimbalCtrl.Yaw = 0;	254	ToGimbalCtrl.Yaw = 0;
255	ToGimbalCtrl.Compass = -1;	255	ToGimbalCtrl.Compass = -1;
256	}	256	}
257	// ++++++++++++++++++++++++++++++++++++++++++++++	257	// ++++++++++++++++++++++++++++++++++++++++++++++
258	// + Crc	258	// + Crc
259	// ++++++++++++++++++++++++++++++++++++++++++++++	259	// ++++++++++++++++++++++++++++++++++++++++++++++
260	ToGimbalCtrl.Crc = 0x55; // initialize	260	ToGimbalCtrl.Crc = 0x55; // initialize
261	dat = &ToGimbalCtrl; // pointer	261	dat = &ToGimbalCtrl; // pointer
262	for(i=0; i< sizeof(ToGimbalCtrl) - 1; i++) ToGimbalCtrl.Crc += dat[i];	262	for(i=0; i< sizeof(ToGimbalCtrl) - 1; i++) ToGimbalCtrl.Crc += dat[i];
263	// ++++++++++++++++++++++++++++++++++++++++++++++	263	// ++++++++++++++++++++++++++++++++++++++++++++++
264	RxBytes = sizeof(FromGimbalCtrl);	264	RxBytes = sizeof(FromGimbalCtrl);
265		265
266	I2CBus_Transmission(I2C0, GIMBAL_SLAVE_ADDRESS, &ToGimbalCtrl, sizeof(ToGimbalCtrl)+1, &GimbalCtrl_UpdateData, RxBytes);	266	I2CBus_Transmission(I2C0, GIMBAL_SLAVE_ADDRESS, &ToGimbalCtrl, sizeof(ToGimbalCtrl)+1, &GimbalCtrl_UpdateData, RxBytes);
267	timing = GIMBAL_DATARATE;	267	timing = GIMBAL_DATARATE;
268	ToGimbalCtrl.BitCmd = GIMBAL_CMD_SPEED; // reset the Flags	268	ToGimbalCtrl.BitCmd = GIMBAL_CMD_SPEED; // reset the Flags
269	old_nick = NickGimbal;	269	old_nick = NickGimbal;
270		270
271	}	271	}
272	else timing = 11; // try again in 11ms	272	else timing = 11; // try again in 11ms
273	}	273	}
274	}	274	}
275		275
276	void CalcNickServoValue(void)	276	void CalcNickServoValue(void)
277	{	277	{
278	signed int max, min;	278	signed int max, min;
279	#define MULTIPLYER 4	279	#define MULTIPLYER 4
280	if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) // relative moving of the servo value	280	if(EE_Parameter.ServoCompInvert & SERVO_RELATIVE) // relative moving of the servo value
281	{	281	{
282	max = ((unsigned int) EE_Parameter.ServoNickMax * MULTIPLYER * 15);	282	max = ((unsigned int) EE_Parameter.ServoNickMax * MULTIPLYER * 15);
283	min = ((unsigned int) EE_Parameter.ServoNickMin * MULTIPLYER * 20);	283	min = ((unsigned int) EE_Parameter.ServoNickMin * MULTIPLYER * 20);
284	NickServoValue -= ((signed char) (FromFC_ServoNickControl - 128) / 4) * 2; // * 6	284	NickServoValue -= ((signed char) (FromFC_ServoNickControl - 128) / 4) * 2; // * 6
285	if(ResetNickServoValue) NickServoValue = 8192; // middle Position	285	if(ResetNickServoValue) NickServoValue = 8192; // middle Position
286	ResetNickServoValue = 0;	286	ResetNickServoValue = 0;
287	LIMIT_MIN_MAX(NickServoValue,min, max);	287	LIMIT_MIN_MAX(NickServoValue,min, max);
288	}	288	}
289	else NickServoValue = (int16_t)FromFC_ServoNickControl * (MULTIPLYER*16); // direct poti control	289	else NickServoValue = (int16_t)FromFC_ServoNickControl * (MULTIPLYER*16); // direct poti control
290	MenuNickGimbalOffset += FromMenuServoNickControl;	290	MenuNickGimbalOffset += FromMenuServoNickControl;
291	LIMIT_MIN_MAX(MenuNickGimbalOffset,-50000,50000);	291	LIMIT_MIN_MAX(MenuNickGimbalOffset,-50000,50000);
292	}	292	}
293		293
294		294
295		295
296		296
297		297
298		298
299		299

Subversion Repositories NaviCtrl

(root)/tags/V2.22a/CamCtrl.c @ 935 - Rev 908 → 916