void stateCalcHorizontalSpeedDir_i(void) {
if (bit_is_set(state.speed_status, SPEED_HDIR_I))
return;
if (bit_is_set(state.speed_status, SPEED_HDIR_F)){
state.h_speed_dir_i = SPEED_BFP_OF_REAL(state.h_speed_dir_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x);
INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y);
INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
SPEEDS_BFP_OF_REAL(state.ned_speed_i, state.ned_speed_f);
SetBit(state.speed_status, SPEED_NED_I);
INT32_ATAN2(state.h_speed_dir_i, state.ned_speed_i.y, state.ned_speed_i.x);
INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
SPEEDS_BFP_OF_REAL(state.enu_speed_i, state.enu_speed_f);
SetBit(state.speed_status, SPEED_ENU_I);
INT32_ATAN2(state.h_speed_dir_i, state.enu_speed_i.x, state.enu_speed_i.y);
INT32_COURSE_NORMALIZE(state.h_speed_dir_i);
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_HDIR_I);
}
/* measures phi and theta assuming no dynamic acceleration ?!! */
__attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) {
INT32_ATAN2(*phi_meas, -accel.y, -accel.z);
int32_t cphi;
PPRZ_ITRIG_COS(cphi, *phi_meas);
int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel.x, INT32_TRIG_FRAC);
INT32_ATAN2(*theta_meas, -cphi_ax, -accel.z);
*phi_meas *= F_UPDATE;
*theta_meas *= F_UPDATE;
}
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
}
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);
INT32_ATAN2(rotorcraft_cam_pan,diff.x,diff.y);
nav_heading = rotorcraft_cam_pan;
#if ROTORCRAFT_CAM_USE_TILT_ANGLES
int32_t dist, height;
INT32_VECT2_NORM(dist, diff);
height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
INT32_ATAN2(rotorcraft_cam_tilt, 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 gh_update_ref_from_pos_sp(struct Int32Vect2 pos_sp) {
VECT2_ADD(gh_pos_ref, gh_speed_ref);
VECT2_ADD(gh_speed_ref, gh_accel_ref);
// compute the "speed part" of accel = -2*zeta*omega*speed -omega^2(pos - pos_sp)
struct Int32Vect2 speed;
INT32_VECT2_RSHIFT(speed, gh_speed_ref, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
VECT2_SMUL(speed, speed, -2*GH_ZETA_OMEGA);
INT32_VECT2_RSHIFT(speed, speed, GH_ZETA_OMEGA_FRAC);
// compute pos error in pos_sp resolution
struct Int32Vect2 pos_err;
INT32_VECT2_RSHIFT(pos_err, gh_pos_ref, (GH_POS_REF_FRAC - INT32_POS_FRAC));
VECT2_DIFF(pos_err, pos_err, pos_sp);
// convert to accel resolution
INT32_VECT2_RSHIFT(pos_err, pos_err, (INT32_POS_FRAC - GH_ACCEL_REF_FRAC));
// compute the "pos part" of accel
struct Int32Vect2 pos;
VECT2_SMUL(pos, pos_err, (-GH_OMEGA_2));
INT32_VECT2_RSHIFT(pos, pos, GH_OMEGA_2_FRAC);
// sum accel
VECT2_SUM(gh_accel_ref, speed, pos);
/* Compute route reference before saturation */
// use metric precision or values are too large
INT32_ATAN2(route_ref, -pos_err.y, -pos_err.x);
/* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum speed*/
gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
/* restore gh_speed_ref range (Q14.17) */
INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
/* Saturate accelerations */
if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
gh_accel_ref.x = -gh_max_accel_ref.x;
}
else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
gh_accel_ref.x = gh_max_accel_ref.x;
}
if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
gh_accel_ref.y = -gh_max_accel_ref.y;
}
else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
gh_accel_ref.y = gh_max_accel_ref.y;
}
/* Saturate speed and adjust acceleration accordingly */
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
gh_speed_ref.x = -gh_max_speed_ref.x;
if (gh_accel_ref.x < 0)
gh_accel_ref.x = 0;
}
else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
gh_speed_ref.x = gh_max_speed_ref.x;
if (gh_accel_ref.x > 0)
gh_accel_ref.x = 0;
}
if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
gh_speed_ref.y = -gh_max_speed_ref.y;
if (gh_accel_ref.y < 0)
gh_accel_ref.y = 0;
}
else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
gh_speed_ref.y = gh_max_speed_ref.y;
if (gh_accel_ref.y > 0)
gh_accel_ref.y = 0;
}
}
void gh_update_ref_from_speed_sp(struct Int32Vect2 speed_sp) {
/* WARNING: SPEED SATURATION UNTESTED */
VECT2_ADD(gh_pos_ref, gh_speed_ref);
VECT2_ADD(gh_speed_ref, gh_accel_ref);
// compute speed error
struct Int32Vect2 speed_err;
INT32_VECT2_RSHIFT(speed_err, speed_sp, (INT32_SPEED_FRAC - GH_SPEED_REF_FRAC));
VECT2_DIFF(speed_err, gh_speed_ref, speed_err);
// convert to accel resolution
INT32_VECT2_RSHIFT(speed_err, speed_err, (GH_SPEED_REF_FRAC - GH_ACCEL_REF_FRAC));
// compute accel from speed_sp
VECT2_SMUL(gh_accel_ref, speed_err, -GH_REF_INV_THAU);
INT32_VECT2_RSHIFT(gh_accel_ref, gh_accel_ref, GH_REF_INV_THAU_FRAC);
/* Compute route reference before saturation */
// use metric precision or values are too large
INT32_ATAN2(route_ref, -speed_sp.y, -speed_sp.x);
/* Compute North and East route components */
PPRZ_ITRIG_SIN(s_route_ref, route_ref);
PPRZ_ITRIG_COS(c_route_ref, route_ref);
c_route_ref = abs(c_route_ref);
s_route_ref = abs(s_route_ref);
/* Compute maximum acceleration*/
gh_max_accel_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, c_route_ref, INT32_TRIG_FRAC);
gh_max_accel_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_ACCEL, s_route_ref, INT32_TRIG_FRAC);
/* Compute maximum speed*/
gh_max_speed_ref.x = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, c_route_ref, INT32_TRIG_FRAC);
gh_max_speed_ref.y = INT_MULT_RSHIFT((int32_t)GH_MAX_SPEED, s_route_ref, INT32_TRIG_FRAC);
/* restore gh_speed_ref range (Q14.17) */
INT32_VECT2_LSHIFT(gh_max_speed_ref, gh_max_speed_ref, (GH_SPEED_REF_FRAC - GH_MAX_SPEED_REF_FRAC));
/* Saturate accelerations */
if (gh_accel_ref.x <= -gh_max_accel_ref.x) {
gh_accel_ref.x = -gh_max_accel_ref.x;
}
else if (gh_accel_ref.x >= gh_max_accel_ref.x) {
gh_accel_ref.x = gh_max_accel_ref.x;
}
if (gh_accel_ref.y <= -gh_max_accel_ref.y) {
gh_accel_ref.y = -gh_max_accel_ref.y;
}
else if (gh_accel_ref.y >= gh_max_accel_ref.y) {
gh_accel_ref.y = gh_max_accel_ref.y;
}
/* Saturate speed and adjust acceleration accordingly */
if (gh_speed_ref.x <= -gh_max_speed_ref.x) {
gh_speed_ref.x = -gh_max_speed_ref.x;
if (gh_accel_ref.x < 0)
gh_accel_ref.x = 0;
}
else if (gh_speed_ref.x >= gh_max_speed_ref.x) {
gh_speed_ref.x = gh_max_speed_ref.x;
if (gh_accel_ref.x > 0)
gh_accel_ref.x = 0;
}
if (gh_speed_ref.y <= -gh_max_speed_ref.y) {
gh_speed_ref.y = -gh_max_speed_ref.y;
if (gh_accel_ref.y < 0)
gh_accel_ref.y = 0;
}
else if (gh_speed_ref.y >= gh_max_speed_ref.y) {
gh_speed_ref.y = gh_max_speed_ref.y;
if (gh_accel_ref.y > 0)
gh_accel_ref.y = 0;
}
}
