예제 #1
0
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));
}
예제 #2
0
/** \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 */
}