ArduCopter/mode_brake.cpp

   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 bool ModeBrake::init(bool ignore_checks)
  11 {
  12     // initialise pos controller speed and acceleration
  13     pos_control->set_max_speed_accel_xy(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE);
  14     pos_control->set_correction_speed_accel_xy(inertial_nav.get_velocity_neu_cms().length(), BRAKE_MODE_DECEL_RATE);
  15
  16     // initialise position controller
  17     pos_control->init_xy_controller();
  18
  19     // set vertical speed and acceleration limits
  20     pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
  21     pos_control->set_correction_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
  22
  23     // initialise the vertical position controller
  24     if (!pos_control->is_active_z()) {
  25         pos_control->init_z_controller();
  26     }
  27
  28     _timeout_ms = 0;
  29
  30     return true;
  31 }
  32
  33 // brake_run - runs the brake controller
  34 // should be called at 100hz or more
  35 void ModeBrake::run()
  36 {
  37     // if not armed set throttle to zero and exit immediately
  38     if (is_disarmed_or_landed()) {
  39         make_safe_ground_handling();
  40         pos_control->relax_z_controller(0.0f);
  41         return;
  42     }
  43
  44     // set motors to full range
  45     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
  46
  47     // relax stop target if we might be landed
  48     if (copter.ap.land_complete_maybe) {
  49         pos_control->soften_for_landing_xy();
  50     }
  51
  52     // use position controller to stop
  53     Vector2f vel;
  54     Vector2f accel;
  55     pos_control->input_vel_accel_xy(vel, accel);
  56     pos_control->update_xy_controller();
  57
  58     // call attitude controller
  59     attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
  60
  61     pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
  62     pos_control->update_z_controller();
  63
  64     // MAV_CMD_SOLO_BTN_PAUSE_CLICK (Solo only) is used to set the timeout.
  65     if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) {
  66         if (!copter.set_mode(Mode::Number::LOITER, ModeReason::BRAKE_TIMEOUT)) {
  67             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 void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms)
  83 {
  84     _timeout_start = millis();
  85     _timeout_ms = timeout_ms;
  86 }
  87
  88 #endif