Rev 866 |
Blame |
Compare with Previous |
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);
}