void estimator_update_state_infrared( void ) { float 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; }
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)); }