| 0,0 → 1,295 |
|---|
| #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]) * -2; |
| |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| // 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 |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| |
| if(FromMenuGimbalYaw) ToGimbalCtrl.Yaw = FromMenuGimbalYaw; |
| ToGimbalCtrl.NRY_Speed = 100; |
| ToGimbalCtrl.Filter = 5; |
| ToGimbalCtrl.Nick = NickGimbal; |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| // + limit angles (0 = -128° 255 = +127°) |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| if(FC.RC_Quality < 50) // Failsafe-Positions |
| { |
| if(EE_Parameter.ServoFS_Pos[0]) (s32)((u32)EE_Parameter.ServoFS_Pos[0] * 10) - 1280; //else ToGimbalCtrl.Nick = 0; |
| if(EE_Parameter.ServoFS_Pos[1]) (s32)((u32)EE_Parameter.ServoFS_Pos[1] * 10) - 1280; //else ToGimbalCtrl.Roll = 0; |
| } |
| |
| 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; |
| } |
| // ++++++++++++++++++++++++++++++++++++++++++++++ |
| // + 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); |
| } |
| |
| |
| |
| |
| |
| |