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));
}
/** \brief Update \a ir_roll and ir_pitch from ADCs or from simulator
* message in HITL and SITL modes
*/
void ir_update(void) {
#if ! (defined SITL || defined HITL)
int16_t x1_mean = buf_ir1.sum/buf_ir1.av_nb_sample;
int16_t x2_mean = buf_ir2.sum/buf_ir2.av_nb_sample;
ir_roll = IR_RollOfIrs(x1_mean, x2_mean);
ir_pitch = IR_PitchOfIrs(x1_mean, x2_mean);
#ifdef ADC_CHANNEL_IR_TOP
ir_top = IR_TopOfIr(buf_ir_top.sum/buf_ir_top.av_nb_sample - IR_ADC_TOP_NEUTRAL);
#endif
/** neutrals are not taken into account in SITL and HITL */
ir_roll -= IR_ADC_ROLL_NEUTRAL;
ir_pitch -= IR_ADC_PITCH_NEUTRAL;
#endif /* !SITL && !HITL */
/** #else ir_roll set by simulator in sim_ir.c */
}
