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
}
/* 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)
{
*phi_meas = int32_atan2(-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);
*theta_meas = int32_atan2(-cphi_ax, -accel->z);
*phi_meas *= F_UPDATE;
*theta_meas *= F_UPDATE;
}
void nav_circle(struct EnuCoor_i * wp_center, int32_t radius) {
if (radius == 0) {
VECT2_COPY(navigation_target, *wp_center);
dist2_to_wp = get_dist2_to_point(wp_center);
}
else {
struct Int32Vect2 pos_diff;
VECT2_DIFF(pos_diff, *stateGetPositionEnu_i(), *wp_center);
// go back to half metric precision or values are too large
//INT32_VECT2_RSHIFT(pos_diff,pos_diff,INT32_POS_FRAC/2);
// store last qdr
int32_t last_qdr = nav_circle_qdr;
// compute qdr
nav_circle_qdr = int32_atan2(pos_diff.y, pos_diff.x);
// increment circle radians
if (nav_circle_radians != 0) {
int32_t angle_diff = nav_circle_qdr - last_qdr;
INT32_ANGLE_NORMALIZE(angle_diff);
nav_circle_radians += angle_diff;
}
else {
// Smallest angle to increment at next step
nav_circle_radians = 1;
}
// direction of rotation
int8_t sign_radius = radius > 0 ? 1 : -1;
// absolute radius
int32_t abs_radius = abs(radius);
// carrot_angle
int32_t carrot_angle = ((CARROT_DIST<<INT32_ANGLE_FRAC) / abs_radius);
Bound(carrot_angle, (INT32_ANGLE_PI / 16), INT32_ANGLE_PI_4);
carrot_angle = nav_circle_qdr - sign_radius * carrot_angle;
int32_t s_carrot, c_carrot;
PPRZ_ITRIG_SIN(s_carrot, carrot_angle);
PPRZ_ITRIG_COS(c_carrot, carrot_angle);
// compute setpoint
VECT2_ASSIGN(pos_diff, abs_radius * c_carrot, abs_radius * s_carrot);
INT32_VECT2_RSHIFT(pos_diff, pos_diff, INT32_TRIG_FRAC);
VECT2_SUM(navigation_target, *wp_center, pos_diff);
}
nav_circle_center = *wp_center;
nav_circle_radius = radius;
horizontal_mode = HORIZONTAL_MODE_CIRCLE;
}
