Subversion Repositories NaviCtrl

Rev

Rev 866 | Blame | Last modification | View Log | RSS feed

#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);
}