Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_BRAKE_ENABLED
       4             : 
       5             : /*
       6             :  * Init and run calls for brake flight mode
       7             :  */
       8             : 
       9             : // brake_init - initialise brake controller
      10           8 : bool ModeBrake::init(bool ignore_checks)
      11             : {
      12             :     // initialise pos controller speed and acceleration
      13           8 :     pos_control->NE_set_max_speed_accel_m(pos_control->get_vel_estimate_NED_ms().xy().length(), BRAKE_MODE_DECEL_RATE_MSS);
      14           8 :     pos_control->NE_set_correction_speed_accel_m(pos_control->get_vel_estimate_NED_ms().xy().length(), BRAKE_MODE_DECEL_RATE_MSS);
      15             : 
      16             :     // initialise position controller
      17           8 :     pos_control->NE_init_controller();
      18             : 
      19             :     // set vertical speed and acceleration limits
      20           8 :     pos_control->D_set_max_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS);
      21           8 :     pos_control->D_set_correction_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS);
      22             : 
      23             :     // initialise the vertical position controller
      24           8 :     if (!pos_control->D_is_active()) {
      25           2 :         pos_control->D_init_controller();
      26             :     }
      27             : 
      28           8 :     _timeout_ms = 0;
      29             : 
      30           8 :     return true;
      31             : }
      32             : 
      33             : // brake_run - runs the brake controller
      34             : // should be called at 100hz or more
      35       15862 : void ModeBrake::run()
      36             : {
      37             :     // if not armed set throttle to zero and exit immediately
      38       15862 :     if (is_disarmed_or_landed()) {
      39        2821 :         make_safe_ground_handling();
      40        2821 :         pos_control->D_relax_controller(0.0f);
      41        2821 :         return;
      42             :     }
      43             : 
      44             :     // set motors to full range
      45       13041 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      46             : 
      47             :     // relax stop target if we might be landed
      48       13041 :     if (copter.ap.land_complete_maybe) {
      49           0 :         pos_control->NE_soften_for_landing();
      50             :     }
      51             : 
      52             :     // use position controller to stop
      53       13041 :     Vector2f vel_ne_zero;
      54       13041 :     Vector2f accel_ne_zero;
      55       13041 :     pos_control->input_vel_accel_NE_m(vel_ne_zero, accel_ne_zero);
      56       13041 :     pos_control->NE_update_controller();
      57             : 
      58             :     // call attitude controller
      59       13041 :     attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0f);
      60             : 
      61       13041 :     pos_control->D_set_pos_target_from_climb_rate_ms(0.0f);
      62       13041 :     pos_control->D_update_controller();
      63             : 
      64             :     // MAV_CMD_SOLO_BTN_PAUSE_CLICK (Solo only) is used to set the timeout.
      65       13041 :     if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {
      66           0 :         if (!copter.set_mode(Mode::Number::LOITER, ModeReason::BRAKE_TIMEOUT)) {
      67           0 :             copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT);
      68             :         }
      69             :     }
      70             : }
      71             : 
      72             : /**
      73             :  * Set a timeout for the brake mode
      74             :  * 
      75             :  * @param timeout_ms [in] timeout in milliseconds
      76             :  * 
      77             :  * @note MAV_CMD_SOLO_BTN_PAUSE_CLICK (Solo only) is used to set the timeout.
      78             :  * If the timeout is reached, the mode will switch to loiter or alt hold depending on the current mode.
      79             :  * If timeout_ms is 0, the timeout is disabled.
      80             :  * 
      81             : */
      82           0 : void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms)
      83             : {
      84           0 :     _timeout_start = millis();
      85           0 :     _timeout_ms = timeout_ms;
      86           0 : }
      87             : 
      88             : #endif