/** Computes the servo values from cam_pan_c and cam_tilt_c */
void cam_angles( void ) {
float cam_pan = 0;
float cam_tilt = 0;
#ifdef CAM_PAN_NEUTRAL
float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
if (pan_diff > 0)
cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
else
cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
#endif
#ifdef CAM_TILT_NEUTRAL
float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
if (tilt_diff > 0)
cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
else
cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
#endif
cam_pan = TRIM_PPRZ(cam_pan);
cam_tilt = TRIM_PPRZ(cam_tilt);
cam_phi_c = cam_pan_c;
cam_theta_c = cam_tilt_c;
#ifdef COMMAND_CAM_PAN
ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
#endif
#ifdef COMMAND_CAM_TILT
ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
#endif
}
void pid_slew_gaz( void ) {
static pprz_t last_gaz=TRIM_PPRZ(CLIMB_LEVEL_GAZ*MAX_PPRZ);
pprz_t diff_gaz = desired_gaz - last_gaz;
Bound(diff_gaz, -TRIM_PPRZ(CLIMB_MAX_DIFF_GAZ*MAX_PPRZ), TRIM_PPRZ(CLIMB_MAX_DIFF_GAZ*MAX_PPRZ));
desired_gaz = last_gaz + diff_gaz;
last_gaz = desired_gaz;
}
/** \brief Computes ::desired_aileron and ::desired_elevator from attitude estimation and expected attitude. */
void roll_pitch_pid_run( void ) {
float err = estimator_phi - desired_roll;
desired_aileron = TRIM_PPRZ(roll_pgain * err);
if (pitch_of_roll <0.)
pitch_of_roll = 0.;
err = -(estimator_theta - desired_pitch - pitch_of_roll * fabs(estimator_phi));
desired_elevator = TRIM_PPRZ(pitch_pgain * err);
}
/** Computes the servo values from cam_pan_c and cam_tilt_c */
void cam_angles(void)
{
float cam_pan = 0;
float cam_tilt = 0;
if (cam_pan_c > RadOfDeg(CAM_PAN_MAX)) {
cam_pan_c = RadOfDeg(CAM_PAN_MAX);
} else {
if (cam_pan_c < RadOfDeg(CAM_PAN_MIN)) {
cam_pan_c = RadOfDeg(CAM_PAN_MIN);
}
}
if (cam_tilt_c > RadOfDeg(CAM_TILT_MAX)) {
cam_tilt_c = RadOfDeg(CAM_TILT_MAX);
} else {
if (cam_tilt_c < RadOfDeg(CAM_TILT_MIN)) {
cam_tilt_c = RadOfDeg(CAM_TILT_MIN);
}
}
#ifdef CAM_PAN_NEUTRAL
float pan_diff = cam_pan_c - RadOfDeg(CAM_PAN_NEUTRAL);
if (pan_diff > 0) {
cam_pan = MAX_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MAX - CAM_PAN_NEUTRAL)));
} else {
cam_pan = MIN_PPRZ * (pan_diff / (RadOfDeg(CAM_PAN_MIN - CAM_PAN_NEUTRAL)));
}
#else
cam_pan = ((float)RadOfDeg(cam_pan_c - CAM_PAN_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(CAM_PAN_MAX - CAM_PAN_MIN));
#endif
#ifdef CAM_TILT_NEUTRAL
float tilt_diff = cam_tilt_c - RadOfDeg(CAM_TILT_NEUTRAL);
if (tilt_diff > 0) {
cam_tilt = MAX_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MAX - CAM_TILT_NEUTRAL)));
} else {
cam_tilt = MIN_PPRZ * (tilt_diff / (RadOfDeg(CAM_TILT_MIN - CAM_TILT_NEUTRAL)));
}
#else
cam_tilt = ((float)RadOfDeg(cam_tilt_c - CAM_TILT_MIN)) * ((float)MAX_PPRZ / (float)RadOfDeg(
CAM_TILT_MAX - CAM_TILT_MIN));
#endif
cam_pan = TRIM_PPRZ(cam_pan);
cam_tilt = TRIM_PPRZ(cam_tilt);
cam_phi_c = cam_pan_c;
cam_theta_c = cam_tilt_c;
#ifdef COMMAND_CAM_PAN
ap_state->commands[COMMAND_CAM_PAN] = cam_pan;
#endif
#ifdef COMMAND_CAM_TILT
ap_state->commands[COMMAND_CAM_TILT] = cam_tilt;
#endif
}
void stabilisation_task(void)
{
/* ---- inlined below: ir_update(); ---- */
// #ifndef SIMUL
int16_t x1_mean = buf_ir1.sum/AV_NB_SAMPLE;
int16_t x2_mean = buf_ir2.sum/AV_NB_SAMPLE;
/* simplesclar cannot have type decls in the middle of the func */
float rad_of_ir, err, tmp_sanjit;
ir_roll = IR_RollOfIrs(x1_mean, x2_mean) - ir_roll_neutral;
ir_pitch = IR_PitchOfIrs(x1_mean, x2_mean) - ir_pitch_neutral;
/*
#else
extern volatile int16_t simul_ir_roll, simul_ir_pitch;
ir_roll = simul_ir_roll - ir_roll_neutral;
ir_pitch = simul_ir_pitch - ir_pitch_neutral;
#endif
*/
/* ---- inlined below estimator_update_state_infrared(); ---- */
rad_of_ir = (ir_estim_mode == IR_ESTIM_MODE_ON && EstimatorIrGainIsCorrect()) ?
estimator_rad_of_ir : ir_rad_of_ir;
estimator_phi = rad_of_ir * ir_roll;
estimator_theta = rad_of_ir * ir_pitch;
/* --- inlined below roll_pitch_pid_run(); // Set desired_aileron & desired_elevator ---- */
err = estimator_phi - desired_roll;
desired_aileron = TRIM_PPRZ(roll_pgain * err);
if (pitch_of_roll <0.)
pitch_of_roll = 0.;
/* line below commented out by sanjit, to avoid use of fabs
err = -(estimator_theta - desired_pitch - pitch_of_roll * fabs(estimator_phi));
2 replacement lines are below
*/
tmp_sanjit = (estimator_phi < 0) ? -estimator_phi : estimator_phi;
err = -(estimator_theta - desired_pitch - pitch_of_roll * tmp_sanjit);
desired_elevator = TRIM_PPRZ(pitch_pgain * err);
/* --- end inline ---- */
to_fbw.channels[RADIO_THROTTLE] = desired_gaz; // desired_gaz is set upon GPS message reception
to_fbw.channels[RADIO_ROLL] = desired_aileron;
#ifndef ANTON_T7
to_fbw.channels[RADIO_PITCH] = desired_elevator;
#endif
// Code for camera stabilization, FIXME put that elsewhere
to_fbw.channels[RADIO_GAIN1] = TRIM_PPRZ(MAX_PPRZ/0.75*(-estimator_phi));
}
static inline void h_ctl_roll_rate_loop()
{
float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;
/* I term calculation */
static float roll_rate_sum_err = 0.;
static uint8_t roll_rate_sum_idx = 0;
static float roll_rate_sum_values[H_CTL_ROLL_RATE_SUM_NB_SAMPLES];
roll_rate_sum_err -= roll_rate_sum_values[roll_rate_sum_idx];
roll_rate_sum_values[roll_rate_sum_idx] = err;
roll_rate_sum_err += err;
roll_rate_sum_idx++;
if (roll_rate_sum_idx >= H_CTL_ROLL_RATE_SUM_NB_SAMPLES) { roll_rate_sum_idx = 0; }
/* D term calculations */
static float last_err = 0;
float d_err = err - last_err;
last_err = err;
float throttle_dep_pgain =
Blend(h_ctl_hi_throttle_roll_rate_pgain, h_ctl_lo_throttle_roll_rate_pgain,
v_ctl_throttle_setpoint / ((float)MAX_PPRZ));
float cmd = throttle_dep_pgain * (err + h_ctl_roll_rate_igain * roll_rate_sum_err / H_CTL_ROLL_RATE_SUM_NB_SAMPLES +
h_ctl_roll_rate_dgain * d_err);
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
inline static void h_ctl_pitch_loop( void ) {
static float last_err;
/* sanity check */
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi);
#ifdef USE_PITCH_TRIM
loiter();
#endif
#ifdef USE_ANGLE_REF
// Update reference setpoints for pitch
h_ctl_ref_pitch_angle += h_ctl_ref_pitch_rate * H_CTL_REF_DT;
h_ctl_ref_pitch_rate += h_ctl_ref_pitch_accel * H_CTL_REF_DT;
h_ctl_ref_pitch_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_pitch_loop_setpoint - h_ctl_ref_pitch_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_pitch_rate;
// Saturation on references
BoundAbs(h_ctl_ref_pitch_accel, H_CTL_REF_MAX_Q_DOT);
if (h_ctl_ref_pitch_rate > H_CTL_REF_MAX_Q) {
h_ctl_ref_pitch_rate = H_CTL_REF_MAX_Q;
if (h_ctl_ref_pitch_accel > 0.) h_ctl_ref_pitch_accel = 0.;
}
else if (h_ctl_ref_pitch_rate < - H_CTL_REF_MAX_Q) {
h_ctl_ref_pitch_rate = - H_CTL_REF_MAX_Q;
if (h_ctl_ref_pitch_accel < 0.) h_ctl_ref_pitch_accel = 0.;
}
#else
h_ctl_ref_pitch_angle = h_ctl_pitch_loop_setpoint;
h_ctl_ref_pitch_rate = 0.;
h_ctl_ref_pitch_accel = 0.;
#endif
// Compute errors
float err = estimator_theta - h_ctl_ref_pitch_angle;
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
last_err = err;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_pitch_sum_err = 0.;
}
else {
if (h_ctl_pitch_igain < 0.) {
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, (- H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain));
} else {
h_ctl_pitch_sum_err = 0.;
}
}
float cmd = h_ctl_pitch_Kffa * h_ctl_ref_pitch_accel
+ h_ctl_pitch_Kffd * h_ctl_ref_pitch_rate
+ h_ctl_pitch_pgain * err
+ h_ctl_pitch_dgain * d_err
+ h_ctl_pitch_igain * h_ctl_pitch_sum_err;
cmd /= airspeed_ratio2;
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
/** Pitch loop */
inline static void pid_pitch_loop_run(void) {
/* sanity check */
if (pitch_of_roll <0.)
pitch_of_roll = 0.;
float err = -(estimator_theta - desired_pitch - pitch_of_roll*fabs(estimator_phi));
Bound(err, -M_PI/2, M_PI/2);
desired_elevator = TRIM_PPRZ(pitch_pgain * err);
}
inline static void h_ctl_cl_loop(void)
{
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
#if (!defined SITL || defined USE_NPS)
struct Int32Vect3 accel_meas_body, accel_ned;
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
VECT3_COPY(accel_ned, (*accel_tmp));
accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
float nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f;
// max load factor to be taken into acount
// to prevent negative flap movement du to negative acceleration
Bound(nz, 1.f, 2.f);
#else
float nz = 1.f;
#endif
#endif
// Compute a corrected airspeed corresponding to the current load factor nz
// with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V,
// the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2,
// thus Vn = V / sqrt(nz)
#if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT
float corrected_airspeed = v_ctl_auto_airspeed_setpoint;
#else
float corrected_airspeed = stateGetAirspeed_f();
#endif
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
corrected_airspeed /= sqrtf(nz);
#endif
Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED);
float cmd = 0.f;
// deadband around NOMINAL_AIRSPEED, rest linear
if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) {
cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED);
}
else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) {
cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED);
}
// no control in manual mode
if (pprz_mode == PPRZ_MODE_MANUAL) {
cmd = 0.f;
}
// bound max flap angle
Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL);
// from percent to pprz
cmd = cmd * MAX_PPRZ;
h_ctl_flaps_setpoint = TRIM_PPRZ(cmd);
}
inline static void h_ctl_yaw_loop(void)
{
#if H_CTL_YAW_TRIM_NY
// Actual Acceleration from IMU:
#if (!defined SITL || defined USE_NPS)
struct Int32Vect3 accel_meas_body, accel_ned;
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
struct NedCoor_i *accel_tmp = stateGetAccelNed_i();
VECT3_COPY(accel_ned, (*accel_tmp));
accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f);
int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned);
float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g)
#else
float ny = 0.f;
#endif
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_yaw_ny_sum_err = 0.;
} else {
if (h_ctl_yaw_ny_igain > 0.) {
// only update when: phi<60degrees and ny<2g
if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) {
h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT;
// max half rudder deflection for trim
BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain));
}
} else {
h_ctl_yaw_ny_sum_err = 0.;
}
}
#endif
#ifdef USE_AIRSPEED
float Vo = stateGetAirspeed_f();
Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED);
#else
float Vo = NOMINAL_AIRSPEED;
#endif
h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC
+ 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns
float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r;
float cmd = + h_ctl_yaw_dgain * d_err
#if H_CTL_YAW_TRIM_NY
+ h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err
#endif
;
cmd /= airspeed_ratio2;
h_ctl_rudder_setpoint = TRIM_PPRZ(cmd);
}
inline static void h_ctl_roll_loop( void ) {
float err = estimator_phi - h_ctl_roll_setpoint;
#ifdef SITL
static float last_err = 0;
estimator_p = (err - last_err)/(1/60.);
last_err = err;
#endif
float cmd = - h_ctl_roll_attitude_gain * err
- h_ctl_roll_rate_gain * estimator_p
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
void cam_periodic( void ) {
switch (cam_roll_mode) {
case MODE_STABILIZED:
phi_c = cam_roll_phi + estimator_phi;
break;
case MODE_MANUAL:
phi_c = cam_roll_phi;
break;
default:
phi_c = 0;
}
ap_state->commands[COMMAND_CAM_ROLL] = TRIM_PPRZ(phi_c * MAX_PPRZ / RadOfDeg(CAM_PHI_MAX_DEG));
}
void cam_periodic(void)
{
switch (cam_roll_mode) {
case MODE_STABILIZED:
phi_c = cam_roll_phi + stateGetNedToBodyEulers_f()->phi;
break;
case MODE_MANUAL:
phi_c = cam_roll_phi;
break;
default:
phi_c = 0;
}
ap_state->commands[COMMAND_CAM_ROLL] = TRIM_PPRZ(phi_c * MAX_PPRZ / CAM_PHI_MAX);
}
inline static void h_ctl_roll_loop( void ) {
float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint;
struct FloatRates* body_rate = stateGetBodyRates_f();
#ifdef SITL
static float last_err = 0;
body_rate->p = (err - last_err)/(1/60.);
last_err = err;
#endif
float cmd = h_ctl_roll_attitude_gain * err
+ h_ctl_roll_rate_gain * body_rate->p
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
inline static void pid_roll_loop_run(void) {
/** Attitude PID */
float err = estimator_phi - desired_roll;
Bound(err, -M_PI/2., M_PI/2);
float std_desired_aileron = TRIM_PPRZ(roll_pgain * err + desired_gaz * aileron_of_gaz);
#ifndef PID_RATE_LOOP
desired_aileron = std_desired_aileron;
#else
float desired_roll_rate = -alt_roll_pgain*err;
Bound(desired_roll_rate, -GYRO_MAX_RATE, GYRO_MAX_RATE);
/** Roll rate pid */
err = roll_rate - desired_roll_rate;
Bound(err, -GYRO_MAX_RATE, GYRO_MAX_RATE);
static float rollrate_sum_err = 0.;
static uint8_t rollratesum_idx = 0;
static float rollratesum_values[ROLLRATESUM_NB_SAMPLES];
rollrate_sum_err -= rollratesum_values[rollratesum_idx];
rollratesum_values[rollratesum_idx] = err;
rollrate_sum_err += err;
rollratesum_idx++;
if (rollratesum_idx >= ROLLRATESUM_NB_SAMPLES) rollratesum_idx = 0;
/* D term calculations */
static float last_roll_rate;
float d_err = roll_rate - last_roll_rate;
last_roll_rate = roll_rate;
float rate_desired_aileron = TRIM_PPRZ(roll_rate_pgain*(err + roll_rate_igain*(rollrate_sum_err/ROLLRATESUM_NB_SAMPLES) + roll_rate_dgain*d_err));
desired_aileron = rate_mode*rate_desired_aileron+(1-rate_mode)*std_desired_aileron;
#endif
}
void attitude_loop( void ) {
#if USE_INFRARED
ahrs_update_infrared();
#endif /* USE_INFRARED */
if (pprz_mode >= PPRZ_MODE_AUTO2)
{
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE) {
v_ctl_throttle_setpoint = nav_throttle_setpoint;
v_ctl_pitch_setpoint = nav_pitch;
}
else if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
{
v_ctl_climb_loop();
}
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ);
#endif
#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
if (vsupply > 0.) {
v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
}
#endif
h_ctl_pitch_setpoint = v_ctl_pitch_setpoint; // Copy the pitch setpoint from the guidance to the stabilization control
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
if (kill_throttle || (!estimator_flight_time && !launch))
v_ctl_throttle_setpoint = 0;
}
h_ctl_attitude_loop(); /* Set h_ctl_aileron_setpoint & h_ctl_elevator_setpoint */
v_ctl_throttle_slew();
ap_state->commands[COMMAND_THROTTLE] = v_ctl_throttle_slewed;
ap_state->commands[COMMAND_ROLL] = -h_ctl_aileron_setpoint;
ap_state->commands[COMMAND_PITCH] = h_ctl_elevator_setpoint;
#if defined MCU_SPI_LINK
link_mcu_send();
#elif defined INTER_MCU && defined SINGLE_MCU
/**Directly set the flag indicating to FBW that shared buffer is available*/
inter_mcu_received_ap = TRUE;
#endif
}
inline static void h_ctl_pitch_loop(void)
{
static float last_err;
struct FloatEulers *att = stateGetNedToBodyEulers_f();
/* sanity check */
if (h_ctl_elevator_of_roll < 0.) {
h_ctl_elevator_of_roll = 0.;
}
if (v_ctl_mode == V_CTL_MODE_LANDING) {
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint;
}
else {
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi);
}
float err = 0;
#ifdef USE_AOA
switch (h_ctl_pitch_mode) {
case H_CTL_PITCH_MODE_THETA:
err = att->theta - h_ctl_pitch_loop_setpoint;
break;
case H_CTL_PITCH_MODE_AOA:
err = stateGetAngleOfAttack_f() - h_ctl_pitch_loop_setpoint;
break;
default:
err = att->theta - h_ctl_pitch_loop_setpoint;
break;
}
#else //NO_AOA
err = att->theta - h_ctl_pitch_loop_setpoint;
#endif
float d_err = err - last_err;
last_err = err;
float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
#ifdef LOITER_TRIM
cmd += loiter();
#endif
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
inline static void h_ctl_pitch_loop( void ) {
static float last_err;
/* sanity check */
if (h_ctl_elevator_of_roll <0.)
h_ctl_elevator_of_roll = 0.;
h_ctl_pitch_loop_setpoint =
h_ctl_pitch_setpoint
- h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi);
float err = estimator_theta - h_ctl_pitch_loop_setpoint;
float d_err = err - last_err;
last_err = err;
float cmd = h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
#ifdef LOITER_TRIM
cmd += loiter();
#endif
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
/** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */
inline static void h_ctl_roll_loop( void ) {
float err = estimator_phi - h_ctl_roll_setpoint;
float cmd = h_ctl_roll_pgain * err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
#ifdef H_CTL_RATE_LOOP
if (h_ctl_auto1_rate) {
/** Runs only the roll rate loop */
h_ctl_roll_rate_setpoint = h_ctl_roll_setpoint * 10.;
h_ctl_roll_rate_loop();
} else {
h_ctl_roll_rate_setpoint = h_ctl_roll_rate_setpoint_pgain * err;
BoundAbs(h_ctl_roll_rate_setpoint, H_CTL_ROLL_RATE_MAX_SETPOINT);
float saved_aileron_setpoint = h_ctl_aileron_setpoint;
h_ctl_roll_rate_loop();
h_ctl_aileron_setpoint = Blend(h_ctl_aileron_setpoint, saved_aileron_setpoint, h_ctl_roll_rate_mode) ;
}
#endif
}
void airborne_ant_point_periodic(void)
{
float airborne_ant_pan_servo = 0;
static VECTOR svPlanePosition,
Home_Position,
Home_PositionForPlane,
Home_PositionForPlane2;
static MATRIX smRotation;
svPlanePosition.fx = stateGetPositionEnu_f()->y;
svPlanePosition.fy = stateGetPositionEnu_f()->x;
svPlanePosition.fz = stateGetPositionUtm_f()->alt;
Home_Position.fx = WaypointY(WP_HOME);
Home_Position.fy = WaypointX(WP_HOME);
Home_Position.fz = waypoints[WP_HOME].a;
/* distance between plane and object */
vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
/* yaw */
smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
smRotation.fy3 = 0.;
smRotation.fz1 = 0.;
smRotation.fz2 = 0.;
smRotation.fz3 = 1.;
vMultiplyMatrixByVector(&Home_PositionForPlane2, smRotation, Home_PositionForPlane);
/*
* This is for one axis pan antenna mechanisms. The default is to
* circle clockwise so view is right. The pan servo neutral makes
* the antenna look to the right with 0� given, 90� is to the back and
* -90� is to the front.
*
*
*
* plane front
*
* 90
^
* I
* 135 I 45�
* \ I /
* \I/
* 180-------I------- 0�
* /I\
* / I \
* -135 I -45�
* I
* -90
* plane back
*
*
*/
/* fPan = 0 -> antenna looks along the wing
90 -> antenna looks in flight direction
-90 -> antenna looks backwards
*/
/* fixed to the plane*/
airborne_ant_pan = (float)(atan2(Home_PositionForPlane2.fx, (Home_PositionForPlane2.fy)));
// I need to avoid oscillations around the 180 degree mark.
if (airborne_ant_pan > 0 && airborne_ant_pan <= RadOfDeg(175)) { ant_pan_positive = 1; }
if (airborne_ant_pan < 0 && airborne_ant_pan >= RadOfDeg(-175)) { ant_pan_positive = 0; }
if (airborne_ant_pan > RadOfDeg(175) && ant_pan_positive == 0) {
airborne_ant_pan = RadOfDeg(-180);
} else if (airborne_ant_pan < RadOfDeg(-175) && ant_pan_positive) {
airborne_ant_pan = RadOfDeg(180);
ant_pan_positive = 0;
}
#ifdef ANT_PAN_NEUTRAL
airborne_ant_pan = airborne_ant_pan - RadOfDeg(ANT_PAN_NEUTRAL);
if (airborne_ant_pan > 0) {
airborne_ant_pan_servo = MAX_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MAX - ANT_PAN_NEUTRAL)));
} else {
airborne_ant_pan_servo = MIN_PPRZ * (airborne_ant_pan / (RadOfDeg(ANT_PAN_MIN - ANT_PAN_NEUTRAL)));
}
#endif
airborne_ant_pan_servo = TRIM_PPRZ(airborne_ant_pan_servo);
#ifdef COMMAND_ANT_PAN
ap_state->commands[COMMAND_ANT_PAN] = airborne_ant_pan_servo;
#endif
return;
}
/** \brief Computes slewed throttle from throttle setpoint
called at 20Hz
*/
void v_ctl_throttle_slew( void ) {
pprz_t diff_throttle = v_ctl_throttle_setpoint - v_ctl_throttle_slewed;
BoundAbs(diff_throttle, TRIM_PPRZ(V_CTL_THROTTLE_SLEW*MAX_PPRZ));
v_ctl_throttle_slewed += diff_throttle;
}
inline static void h_ctl_roll_loop( void ) {
static float cmd_fb = 0.;
#ifdef USE_ANGLE_REF
// Update reference setpoints for roll
h_ctl_ref_roll_angle += h_ctl_ref_roll_rate * H_CTL_REF_DT;
h_ctl_ref_roll_rate += h_ctl_ref_roll_accel * H_CTL_REF_DT;
h_ctl_ref_roll_accel = H_CTL_REF_W*H_CTL_REF_W * (h_ctl_roll_setpoint - h_ctl_ref_roll_angle) - 2*H_CTL_REF_XI*H_CTL_REF_W * h_ctl_ref_roll_rate;
// Saturation on references
BoundAbs(h_ctl_ref_roll_accel, H_CTL_REF_MAX_P_DOT);
if (h_ctl_ref_roll_rate > H_CTL_REF_MAX_P) {
h_ctl_ref_roll_rate = H_CTL_REF_MAX_P;
if (h_ctl_ref_roll_accel > 0.) h_ctl_ref_roll_accel = 0.;
}
else if (h_ctl_ref_roll_rate < - H_CTL_REF_MAX_P) {
h_ctl_ref_roll_rate = - H_CTL_REF_MAX_P;
if (h_ctl_ref_roll_accel < 0.) h_ctl_ref_roll_accel = 0.;
}
#else
h_ctl_ref_roll_angle = h_ctl_roll_setpoint;
h_ctl_ref_roll_rate = 0.;
h_ctl_ref_roll_accel = 0.;
#endif
#ifdef USE_KFF_UPDATE
// update Kff gains
h_ctl_roll_Kffa -= KFFA_UPDATE * h_ctl_ref_roll_accel * cmd_fb / (H_CTL_REF_MAX_P_DOT*H_CTL_REF_MAX_P_DOT);
h_ctl_roll_Kffd -= KFFD_UPDATE * h_ctl_ref_roll_rate * cmd_fb / (H_CTL_REF_MAX_P*H_CTL_REF_MAX_P);
#ifdef SITL
printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
#endif
h_ctl_roll_Kffa = Min(h_ctl_roll_Kffa, 0);
h_ctl_roll_Kffd = Min(h_ctl_roll_Kffd, 0);
#endif
// Compute errors
float err = estimator_phi - h_ctl_ref_roll_angle;
#ifdef SITL
static float last_err = 0;
estimator_p = (err - last_err)/(1/60.);
last_err = err;
#endif
float d_err = estimator_p - h_ctl_ref_roll_rate;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_roll_sum_err = 0.;
}
else {
if (h_ctl_roll_igain < 0.) {
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, (- H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain));
} else {
h_ctl_roll_sum_err = 0.;
}
}
cmd_fb = h_ctl_roll_attitude_gain * err;// + h_ctl_roll_rate_gain * d_err;
float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
+ h_ctl_roll_Kffd * h_ctl_ref_roll_rate
- cmd_fb
- h_ctl_roll_rate_gain * d_err
- h_ctl_roll_igain * h_ctl_roll_sum_err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
// float cmd = h_ctl_roll_Kffa * h_ctl_ref_roll_accel
// + h_ctl_roll_Kffd * h_ctl_ref_roll_rate
// - h_ctl_roll_attitude_gain * err
// - h_ctl_roll_rate_gain * d_err
// - h_ctl_roll_igain * h_ctl_roll_sum_err
// + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
//
cmd /= airspeed_ratio2;
// Set aileron commands
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
void v_ctl_climb_loop ( void )
{
// Integrators
static float v_ctl_throttle_trim = 0;
static float v_ctl_pitch_trim = 0;
static float v_ctl_last_e_tot = 0;
static float v_ctl_last_e_dis = 0;
// Local
float v_ctl_vz_error = 0.;
float v_ctl_gamma_error = 0.;
float throttle = 0.;
// Energy: Potential and Kinetic
float v_ctl_energy_error_pot = 0;
float v_ctl_energy_error_kin = 0;
// Energy: Total and distribution
float v_ctl_energy_error_tot = 0;
float v_ctl_energy_error_dis = 0;
static float v_ctl_energy_rate_tot = 0;
static float v_ctl_energy_rate_dis = 0;
// Altitude Error
v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
// Commanded Climb Rate
v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error
+ v_ctl_altitude_pre_climb;
BoundAbs(v_ctl_climb_setpoint, V_CTL_ALTITUDE_MAX_CLIMB);
// Actual Climb Rate
v_ctl_vz_error = v_ctl_climb_setpoint + estimator_z_dot;
BoundAbs(v_ctl_vz_error, V_CTL_ALTITUDE_MAX_CLIMB);
// Commanded Flight Path
v_ctl_gamma_error = atan2(v_ctl_vz_error, estimator_hspeed_mod);
BoundAbs(v_ctl_gamma_error, 0.5);
// Massless Energy
v_ctl_energy_error_pot = (v_ctl_altitude_error * 9.81) / 1000.;
v_ctl_energy_error_kin = (v_ctl_auto_airspeed_setpoint * v_ctl_auto_airspeed_setpoint/2.
- estimator_hspeed_mod * estimator_hspeed_mod/2.) / 1000.;
BoundAbs( v_ctl_energy_error_pot, v_ctl_pot_energy_bounds * 9.81);
BoundAbs( v_ctl_energy_error_kin, v_ctl_kin_energy_bounds * v_ctl_kin_energy_bounds / 2.);
// Total en Distribution
v_ctl_energy_error_tot = v_ctl_energy_error_pot + v_ctl_energy_error_kin;
v_ctl_energy_error_dis = v_ctl_energy_error_pot - v_ctl_energy_error_kin;
// Energy Rate
v_ctl_energy_rate_tot = ((v_ctl_energy_error_tot - v_ctl_last_e_tot) - v_ctl_energy_rate_tot) / 5.;
v_ctl_last_e_tot = v_ctl_energy_error_tot;
v_ctl_energy_rate_dis = ((v_ctl_energy_error_dis - v_ctl_last_e_dis) - v_ctl_energy_rate_dis) / 5.;
v_ctl_last_e_dis = v_ctl_energy_error_dis;
// Actual Control
// integrators
v_ctl_pitch_trim += -v_ctl_energy_error_kin * v_ctl_energy_dis_i;
if (v_ctl_pitch_trim >= v_ctl_pitch_max)
{
v_ctl_pitch_trim = v_ctl_pitch_max;
}
else if (v_ctl_pitch_trim < v_ctl_pitch_min)
{
v_ctl_pitch_trim = v_ctl_pitch_min;
}
// proportional
throttle = v_ctl_throttle_trim + v_ctl_energy_error_tot * v_ctl_energy_tot_p
- v_ctl_energy_rate_tot * v_ctl_energy_tot_d;
nav_pitch = v_ctl_pitch_trim + v_ctl_energy_error_dis * v_ctl_energy_dis_p
- v_ctl_energy_rate_dis * v_ctl_energy_dis_d;
if (nav_pitch >= v_ctl_pitch_max)
{
nav_pitch = v_ctl_pitch_max;
}
else if (nav_pitch < v_ctl_pitch_min)
{
nav_pitch = v_ctl_pitch_min;
}
if (throttle >= v_ctl_throttle_max)
{
throttle = v_ctl_throttle_max;
}
else if (throttle < v_ctl_throttle_min)
{
throttle = v_ctl_throttle_min;
}
else
{
v_ctl_throttle_trim += v_ctl_energy_error_tot * v_ctl_energy_tot_i;
// too little energy and not too much speed
/* if ((nav_pitch < H_CTL_PITCH_MAX_SETPOINT) && (v_ctl_energy_error_tot > 0))
{
v_ctl_throttle_trim += v_ctl_energy_error_tot * v_ctl_energy_tot_i;
}
else if ((nav_pitch > H_CTL_PITCH_MIN_SETPOINT) && (v_ctl_energy_error_tot < 0))
{
v_ctl_throttle_trim += v_ctl_energy_error_tot * v_ctl_energy_tot_i;
}
*/
if (v_ctl_throttle_trim >= v_ctl_throttle_max)
{
v_ctl_throttle_trim = v_ctl_throttle_max;
}
else if (v_ctl_throttle_trim < v_ctl_throttle_min)
{
v_ctl_throttle_trim = v_ctl_throttle_min;
}
}
v_ctl_throttle_setpoint = TRIM_PPRZ(throttle * MAX_PPRZ);
DOWNLINK_SEND_VERTICAL_ENERGY(DefaultChannel, &v_ctl_energy_error_tot, &v_ctl_throttle_trim, &v_ctl_energy_error_kin, &v_ctl_pitch_trim, &throttle, &nav_pitch, &v_ctl_auto_airspeed_setpoint);
}
inline static void h_ctl_roll_loop( void ) {
static float cmd_fb = 0.;
#if USE_ANGLE_REF
// Update reference setpoints for roll
h_ctl_ref.roll_angle += h_ctl_ref.roll_rate * H_CTL_REF_DT;
h_ctl_ref.roll_rate += h_ctl_ref.roll_accel * H_CTL_REF_DT;
h_ctl_ref.roll_accel = H_CTL_REF_W_P*H_CTL_REF_W_P * (h_ctl_roll_setpoint - h_ctl_ref.roll_angle) - 2*H_CTL_REF_XI_P*H_CTL_REF_W_P * h_ctl_ref.roll_rate;
// Saturation on references
BoundAbs(h_ctl_ref.roll_accel, h_ctl_ref.max_p_dot);
if (h_ctl_ref.roll_rate > h_ctl_ref.max_p) {
h_ctl_ref.roll_rate = h_ctl_ref.max_p;
if (h_ctl_ref.roll_accel > 0.) h_ctl_ref.roll_accel = 0.;
}
else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) {
h_ctl_ref.roll_rate = - h_ctl_ref.max_p;
if (h_ctl_ref.roll_accel < 0.) h_ctl_ref.roll_accel = 0.;
}
#else
h_ctl_ref.roll_angle = h_ctl_roll_setpoint;
h_ctl_ref.roll_rate = 0.;
h_ctl_ref.roll_accel = 0.;
#endif
#ifdef USE_KFF_UPDATE
// update Kff gains
h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot*h_ctl_ref.max_p_dot);
h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate * cmd_fb / (h_ctl_ref.max_p*h_ctl_ref.max_p);
#ifdef SITL
printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
#endif
h_ctl_roll_Kffa = Max(h_ctl_roll_Kffa, 0);
h_ctl_roll_Kffd = Max(h_ctl_roll_Kffd, 0);
#endif
// Compute errors
float err = h_ctl_ref.roll_angle - stateGetNedToBodyEulers_f()->phi;
struct FloatRates* body_rate = stateGetBodyRates_f();
float d_err = h_ctl_ref.roll_rate - body_rate->p;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_roll_sum_err = 0.;
}
else {
if (h_ctl_roll_igain > 0.) {
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);
} else {
h_ctl_roll_sum_err = 0.;
}
}
cmd_fb = h_ctl_roll_attitude_gain * err;
float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel
- h_ctl_roll_Kffd * h_ctl_ref.roll_rate
- cmd_fb
- h_ctl_roll_rate_gain * d_err
- h_ctl_roll_igain * h_ctl_roll_sum_err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
cmd /= airspeed_ratio2;
// Set aileron commands
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
/** \fn void navigation_task( void )
* \brief Compute desired_course
*/
static void navigation_task( void ) {
#if defined FAILSAFE_DELAY_WITHOUT_GPS
/** This section is used for the failsafe of GPS */
static uint8_t last_pprz_mode;
/** If aircraft is launched and is in autonomus mode, go into
PPRZ_MODE_GPS_OUT_OF_ORDER mode (Failsafe) if we lost the GPS */
if (launch) {
if (GpsTimeoutError) {
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME) {
last_pprz_mode = pprz_mode;
pprz_mode = PPRZ_MODE_GPS_OUT_OF_ORDER;
PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
gps_lost = TRUE;
}
} else /* GPS is ok */ if (gps_lost) {
/** If aircraft was in failsafe mode, come back in previous mode */
pprz_mode = last_pprz_mode;
gps_lost = FALSE;
PERIODIC_SEND_PPRZ_MODE(DefaultChannel);
}
}
#endif /* GPS && FAILSAFE_DELAY_WITHOUT_GPS */
common_nav_periodic_task_4Hz();
if (pprz_mode == PPRZ_MODE_HOME)
nav_home();
else if (pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER)
nav_without_gps();
else
nav_periodic_task();
#ifdef TCAS
CallTCAS();
#endif
#ifndef PERIOD_NAVIGATION_DefaultChannel_0 // If not sent periodically (in default 0 mode)
SEND_NAVIGATION(DefaultChannel);
#endif
SEND_CAM(DefaultChannel);
/* The nav task computes only nav_altitude. However, we are interested
by desired_altitude (= nav_alt+alt_shift) in any case.
So we always run the altitude control loop */
if (v_ctl_mode == V_CTL_MODE_AUTO_ALT)
v_ctl_altitude_loop();
if (pprz_mode == PPRZ_MODE_AUTO2 || pprz_mode == PPRZ_MODE_HOME
|| pprz_mode == PPRZ_MODE_GPS_OUT_OF_ORDER) {
#ifdef H_CTL_RATE_LOOP
/* Be sure to be in attitude mode, not roll */
h_ctl_auto1_rate = FALSE;
#endif
if (lateral_mode >=LATERAL_MODE_COURSE)
h_ctl_course_loop(); /* aka compute nav_desired_roll */
if (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)
v_ctl_climb_loop();
if (v_ctl_mode == V_CTL_MODE_AUTO_THROTTLE)
v_ctl_throttle_setpoint = nav_throttle_setpoint;
#if defined V_CTL_THROTTLE_IDLE
Bound(v_ctl_throttle_setpoint, TRIM_PPRZ(V_CTL_THROTTLE_IDLE*MAX_PPRZ), MAX_PPRZ);
#endif
#ifdef V_CTL_POWER_CTL_BAT_NOMINAL
if (vsupply > 0.) {
v_ctl_throttle_setpoint *= 10. * V_CTL_POWER_CTL_BAT_NOMINAL / (float)vsupply;
v_ctl_throttle_setpoint = TRIM_UPPRZ(v_ctl_throttle_setpoint);
}
#endif
h_ctl_pitch_setpoint = nav_pitch;
Bound(h_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
if (kill_throttle || (!estimator_flight_time && !launch))
v_ctl_throttle_setpoint = 0;
}
energy += ((float)current) / 3600.0f * 0.25f; // mAh = mA * dt (4Hz -> hours)
}
inline static void h_ctl_pitch_loop( void ) {
#if !USE_GYRO_PITCH_RATE
static float last_err;
#endif
/* sanity check */
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);
#if USE_PITCH_TRIM
loiter();
#endif
#if USE_ANGLE_REF
// Update reference setpoints for pitch
h_ctl_ref.pitch_angle += h_ctl_ref.pitch_rate * H_CTL_REF_DT;
h_ctl_ref.pitch_rate += h_ctl_ref.pitch_accel * H_CTL_REF_DT;
h_ctl_ref.pitch_accel = H_CTL_REF_W_Q*H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2*H_CTL_REF_XI_Q*H_CTL_REF_W_Q * h_ctl_ref.pitch_rate;
// Saturation on references
BoundAbs(h_ctl_ref.pitch_accel, h_ctl_ref.max_q_dot);
if (h_ctl_ref.pitch_rate > h_ctl_ref.max_q) {
h_ctl_ref.pitch_rate = h_ctl_ref.max_q;
if (h_ctl_ref.pitch_accel > 0.) h_ctl_ref.pitch_accel = 0.;
}
else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) {
h_ctl_ref.pitch_rate = - h_ctl_ref.max_q;
if (h_ctl_ref.pitch_accel < 0.) h_ctl_ref.pitch_accel = 0.;
}
#else
h_ctl_ref.pitch_angle = h_ctl_pitch_loop_setpoint;
h_ctl_ref.pitch_rate = 0.;
h_ctl_ref.pitch_accel = 0.;
#endif
// Compute errors
float err = h_ctl_ref.pitch_angle - stateGetNedToBodyEulers_f()->theta;
#if USE_GYRO_PITCH_RATE
float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q;
#else // soft derivation
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate;
last_err = err;
#endif
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_pitch_sum_err = 0.;
}
else {
if (h_ctl_pitch_igain > 0.) {
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);
} else {
h_ctl_pitch_sum_err = 0.;
}
}
float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel
- h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate
+ h_ctl_pitch_pgain * err
+ h_ctl_pitch_dgain * d_err
+ h_ctl_pitch_igain * h_ctl_pitch_sum_err;
cmd /= airspeed_ratio2;
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}