Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4        1380 : void ModeTraining::update()
       5             : {
       6        1380 :     plane.training_manual_roll = false;
       7        1380 :     plane.training_manual_pitch = false;
       8        1380 :     plane.update_load_factor();
       9             : 
      10             :     // if the roll is past the set roll limit, then
      11             :     // we set target roll to the limit
      12        1380 :     if (ahrs.roll_sensor >= plane.roll_limit_cd) {
      13        1204 :         plane.nav_roll_cd = plane.roll_limit_cd;
      14         176 :     } else if (ahrs.roll_sensor <= -plane.roll_limit_cd) {
      15           0 :         plane.nav_roll_cd = -plane.roll_limit_cd;
      16             :     } else {
      17         176 :         plane.training_manual_roll = true;
      18         176 :         plane.nav_roll_cd = 0;
      19             :     }
      20             : 
      21             :     // if the pitch is past the set pitch limits, then
      22             :     // we set target pitch to the limit
      23        1380 :     if (ahrs.pitch_sensor >= plane.aparm.pitch_limit_max*100) {
      24           0 :         plane.nav_pitch_cd = plane.aparm.pitch_limit_max*100;
      25        1380 :     } else if (ahrs.pitch_sensor <= plane.pitch_limit_min*100) {
      26           0 :         plane.nav_pitch_cd = plane.pitch_limit_min*100;
      27             :     } else {
      28        1380 :         plane.training_manual_pitch = true;
      29        1380 :         plane.nav_pitch_cd = 0;
      30             :     }
      31        1380 :     if (plane.fly_inverted()) {
      32           0 :         plane.nav_pitch_cd = -plane.nav_pitch_cd;
      33             :     }
      34        1380 : }
      35             : 
      36             : /*
      37             :   a special stabilization function for training mode
      38             :  */
      39        1380 : void ModeTraining::run()
      40             : {
      41        1380 :     const float rexpo = plane.roll_in_expo(false);
      42        1380 :     const float pexpo = plane.pitch_in_expo(false);
      43        1380 :     if (plane.training_manual_roll) {
      44         176 :         SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
      45         176 :         plane.rollController.reset();
      46             :     } else {
      47             :         // calculate what is needed to hold
      48        1204 :         plane.stabilize_roll();
      49        2408 :         if ((plane.nav_roll_cd > 0 && rexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_aileron)) ||
      50        1204 :             (plane.nav_roll_cd < 0 && rexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_aileron))) {
      51             :             // allow user to get out of the roll
      52           0 :             SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rexpo);
      53             :         }
      54             :     }
      55             : 
      56        1380 :     if (plane.training_manual_pitch) {
      57        1380 :         SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
      58        1380 :         plane.pitchController.reset();
      59             :     } else {
      60           0 :         plane.stabilize_pitch();
      61           0 :         if ((plane.nav_pitch_cd > 0 && pexpo < SRV_Channels::get_output_scaled(SRV_Channel::k_elevator)) ||
      62           0 :             (plane.nav_pitch_cd < 0 && pexpo > SRV_Channels::get_output_scaled(SRV_Channel::k_elevator))) {
      63             :             // allow user to get back to level
      64           0 :             SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pexpo);
      65             :         }
      66             :     }
      67             : 
      68             :     // Always manual rudder control
      69        1380 :     output_rudder_and_steering(plane.rudder_in_expo(false));
      70             : 
      71        1380 :     output_pilot_throttle();
      72             : 
      73        1380 : }