void stateCalcHorizontalSpeedNorm_i(void) {
if (bit_is_set(state.speed_status, SPEED_HNORM_I))
return;
if (bit_is_set(state.speed_status, SPEED_HNORM_F)){
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
/// @todo consider INT32_SPEED_FRAC
INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
/// @todo consider INT32_SPEED_FRAC
INT32_VECT2_NORM(state.h_speed_norm_i, state.enu_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_I)) {
/* transform ecef speed to ned, set status bit, then compute norm */
//foo
/// @todo consider INT32_SPEED_FRAC
//INT32_VECT2_NORM(state.h_speed_norm_i, state.ned_speed_i);
}
else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
SetBit(state.speed_status, SPEED_NED_F);
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
else {
//int32_t _norm_zero = 0;
//return _norm_zero;
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_HNORM_I);
}
void rotorcraft_cam_periodic(void)
{
switch (rotorcraft_cam_mode) {
case ROTORCRAFT_CAM_MODE_NONE:
#if ROTORCRAFT_CAM_USE_TILT
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
#endif
#if ROTORCRAFT_CAM_USE_PAN
rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
#endif
break;
case ROTORCRAFT_CAM_MODE_MANUAL:
// nothing to do here, just apply tilt pwm at the end
break;
case ROTORCRAFT_CAM_MODE_HEADING:
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
#if ROTORCRAFT_CAM_USE_PAN
INT32_COURSE_NORMALIZE(rotorcraft_cam_pan);
nav_heading = rotorcraft_cam_pan;
#endif
break;
case ROTORCRAFT_CAM_MODE_WP:
#ifdef ROTORCRAFT_CAM_TRACK_WP
{
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[ROTORCRAFT_CAM_TRACK_WP], *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC);
rotorcraft_cam_pan = int32_atan2(diff.x, diff.y);
nav_heading = rotorcraft_cam_pan;
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
int32_t dist, height;
dist = INT32_VECT2_NORM(diff);
height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
rotorcraft_cam_tilt = int32_atan2(height, dist);
Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
(CAM_TA_MAX - CAM_TA_MIN);
#endif
}
#endif
break;
default:
break;
}
#if ROTORCRAFT_CAM_USE_TILT
ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
#endif
}
void booz_cam_periodic(void) {
switch (booz_cam_mode) {
case BOOZ_CAM_MODE_NONE:
#ifdef BOOZ_CAM_USE_TILT
booz_cam_tilt_pwm = BOOZ_CAM_TILT_NEUTRAL;
#endif
#ifdef BOOZ_CAM_USE_PAN
booz_cam_pan = ahrs.ltp_to_body_euler.psi;
#endif
break;
case BOOZ_CAM_MODE_MANUAL:
#ifdef BOOZ_CAM_USE_TILT
Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
#endif
break;
case BOOZ_CAM_MODE_HEADING:
#ifdef BOOZ_CAM_USE_TILT_ANGLES
Bound(booz_cam_tilt,CAM_TA_MIN,CAM_TA_MAX);
booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
#endif
#ifdef BOOZ_CAM_USE_PAN
Bound(booz_cam_pan, CP_MIN, CP_MAX);
nav_heading = booz_cam_pan;
#endif
break;
case BOOZ_CAM_MODE_WP:
#ifdef WP_CAM
{
struct Int32Vect2 diff;
VECT2_DIFF(diff, waypoints[WP_CAM], ins_enu_pos);
INT32_VECT2_RSHIFT(diff,diff,INT32_POS_FRAC);
INT32_ATAN2(booz_cam_pan,diff.x,diff.y);
nav_heading = booz_cam_pan;
#ifdef BOOZ_CAM_USE_TILT_ANGLES
int32_t dist, height;
INT32_VECT2_NORM(dist, diff);
height = (waypoints[WP_CAM].z - ins_enu_pos.z) >> INT32_POS_FRAC;
INT32_ATAN2(booz_cam_tilt, height, dist);
Bound(booz_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
booz_cam_tilt_pwm = BOOZ_CAM_TILT_MIN + D_TILT * (booz_cam_tilt - CAM_TA_MIN) / (CAM_TA_MAX - CAM_TA_MIN);
Bound(booz_cam_tilt_pwm, CT_MIN, CT_MAX);
#endif
}
#endif
break;
}
#ifdef BOOZ_CAM_USE_TILT
BOOZ_CAM_SetPwm(booz_cam_tilt_pwm);
#endif
}
