ArduCopter/mode_throw.cpp

   1 #include "Copter.h"
   2
   3 #if MODE_THROW_ENABLED
   4
   5 // throw_init - initialise throw controller
   6 bool ModeThrow::init(bool ignore_checks)
   7 {
   8 #if FRAME_CONFIG == HELI_FRAME
   9     // do not allow helis to use throw to start
  10     return false;
  11 #endif
  12
  13     // do not enter the mode when already armed or when flying
  14     if (motors->armed()) {
  15         return false;
  16     }
  17
  18     // init state
  19     stage = Throw_Disarmed;
  20     nextmode_attempted = false;
  21
  22     // initialise pos controller speed and acceleration
  23     pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE);
  24     pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE);
  25
  26     // set vertical speed and acceleration limits
  27     pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
  28     pos_control->set_correction_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
  29
  30     return true;
  31 }
  32
  33 // runs the throw to start controller
  34 // should be called at 100hz or more
  35 void ModeThrow::run()
  36 {
  37     /* Throw State Machine
  38     Throw_Disarmed - motors are off
  39     Throw_Detecting -  motors are on and we are waiting for the throw
  40     Throw_Uprighting - the throw has been detected and the copter is being uprighted
  41     Throw_HgtStabilise - the copter is kept level and  height is stabilised about the target height
  42     Throw_PosHold - the copter is kept at a constant position and height
  43     */
  44
  45     if (!motors->armed()) {
  46         // state machine entry is always from a disarmed state
  47         stage = Throw_Disarmed;
  48
  49     } else if (stage == Throw_Disarmed && motors->armed()) {
  50         gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw");
  51         stage = Throw_Detecting;
  52
  53     } else if (stage == Throw_Detecting && throw_detected()){
  54         gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
  55         copter.set_land_complete(false);
  56         stage = Throw_Wait_Throttle_Unlimited;
  57
  58         // Cancel the waiting for throw tone sequence
  59         AP_Notify::flags.waiting_for_throw = false;
  60
  61     } else if (stage == Throw_Wait_Throttle_Unlimited &&
  62                motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
  63         gcs().send_text(MAV_SEVERITY_INFO,"throttle is unlimited - uprighting");
  64         stage = Throw_Uprighting;
  65     } else if (stage == Throw_Uprighting && throw_attitude_good()) {
  66         gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
  67         stage = Throw_HgtStabilise;
  68
  69         // initialise the z controller
  70         pos_control->init_z_controller_no_descent();
  71
  72         // initialise the demanded height to 3m above the throw height
  73         // we want to rapidly clear surrounding obstacles
  74         if (g2.throw_type == ThrowType::Drop) {
  75             pos_control->set_pos_desired_z_cm(inertial_nav.get_position_z_up_cm() - 100);
  76         } else {
  77             pos_control->set_pos_desired_z_cm(inertial_nav.get_position_z_up_cm() + 300);
  78         }
  79
  80         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
  81         copter.set_auto_armed(true);
  82
  83     } else if (stage == Throw_HgtStabilise && throw_height_good()) {
  84         gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
  85         stage = Throw_PosHold;
  86
  87         // initialise position controller
  88         pos_control->init_xy_controller();
  89
  90         // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
  91         copter.set_auto_armed(true);
  92     } else if (stage == Throw_PosHold && throw_position_good()) {
  93         if (!nextmode_attempted) {
  94             switch ((Mode::Number)g2.throw_nextmode.get()) {
  95                 case Mode::Number::AUTO:
  96                 case Mode::Number::GUIDED:
  97                 case Mode::Number::RTL:
  98                 case Mode::Number::LAND:
  99                 case Mode::Number::BRAKE:
 100                 case Mode::Number::LOITER:
 101                     set_mode((Mode::Number)g2.throw_nextmode.get(), ModeReason::THROW_COMPLETE);
 102                     break;
 103                 default:
 104                     // do nothing
 105                     break;
 106             }
 107             nextmode_attempted = true;
 108         }
 109     }
 110
 111     // Throw State Processing
 112     switch (stage) {
 113
 114     case Throw_Disarmed:
 115
 116         // prevent motors from rotating before the throw is detected unless enabled by the user
 117         if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
 118             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
 119         } else {
 120             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
 121         }
 122
 123         // demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
 124         attitude_control->reset_yaw_target_and_rate();
 125         attitude_control->reset_rate_controller_I_terms();
 126         attitude_control->set_throttle_out(0,true,g.throttle_filt);
 127         break;
 128
 129     case Throw_Detecting:
 130
 131         // prevent motors from rotating before the throw is detected unless enabled by the user
 132         if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
 133             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
 134         } else {
 135             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
 136         }
 137
 138         // Hold throttle at zero during the throw and continually reset the attitude controller
 139         attitude_control->reset_yaw_target_and_rate();
 140         attitude_control->reset_rate_controller_I_terms();
 141         attitude_control->set_throttle_out(0,true,g.throttle_filt);
 142
 143         // Play the waiting for throw tone sequence to alert the user
 144         AP_Notify::flags.waiting_for_throw = true;
 145
 146         break;
 147
 148     case Throw_Wait_Throttle_Unlimited:
 149
 150         // set motors to full range
 151         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 152
 153         break;
 154
 155     case Throw_Uprighting:
 156
 157         // set motors to full range
 158         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 159
 160         // demand a level roll/pitch attitude with zero yaw rate
 161         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
 162
 163         // output 50% throttle and turn off angle boost to maximise righting moment
 164         attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);
 165
 166         break;
 167
 168     case Throw_HgtStabilise:
 169
 170         // set motors to full range
 171         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 172
 173         // call attitude controller
 174         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
 175
 176         // call height controller
 177         pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
 178         pos_control->update_z_controller();
 179
 180         break;
 181
 182     case Throw_PosHold:
 183
 184         // set motors to full range
 185         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 186
 187         // use position controller to stop
 188         Vector2f vel;
 189         Vector2f accel;
 190         pos_control->input_vel_accel_xy(vel, accel);
 191         pos_control->update_xy_controller();
 192
 193         // call attitude controller
 194         attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
 195
 196         // call height controller
 197         pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
 198         pos_control->update_z_controller();
 199
 200         break;
 201     }
 202
 203 #if HAL_LOGGING_ENABLED
 204     // log at 10hz or if stage changes
 205     uint32_t now = AP_HAL::millis();
 206     if ((stage != prev_stage) || (now - last_log_ms) > 100) {
 207         prev_stage = stage;
 208         last_log_ms = now;
 209         const float velocity = inertial_nav.get_velocity_neu_cms().length();
 210         const float velocity_z = inertial_nav.get_velocity_z_up_cms();
 211         const float accel = copter.ins.get_accel().length();
 212         const float ef_accel_z = ahrs.get_accel_ef().z;
 213         const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
 214         const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
 215         const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
 216         const bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
 217
 218 // @LoggerMessage: THRO
 219 // @Description: Throw Mode messages
 220 // @URL: https://ardupilot.org/copter/docs/throw-mode.html
 221 // @Field: TimeUS: Time since system startup
 222 // @Field: Stage: Current stage of the Throw Mode
 223 // @Field: Vel: Magnitude of the velocity vector
 224 // @Field: VelZ: Vertical Velocity
 225 // @Field: Acc: Magnitude of the vector of the current acceleration
 226 // @Field: AccEfZ: Vertical earth frame accelerometer value
 227 // @Field: Throw: True if a throw has been detected since entering this mode
 228 // @Field: AttOk: True if the vehicle is upright
 229 // @Field: HgtOk: True if the vehicle is within 50cm of the demanded height
 230 // @Field: PosOk: True if the vehicle is within 50cm of the demanded horizontal position
 231
 232         AP::logger().WriteStreaming(
 233             "THRO",
 234             "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
 235             "s-nnoo----",
 236             "F-0000----",
 237             "QBffffbbbb",
 238             AP_HAL::micros64(),
 239             (uint8_t)stage,
 240             (double)velocity,
 241             (double)velocity_z,
 242             (double)accel,
 243             (double)ef_accel_z,
 244             throw_detect,
 245             attitude_ok,
 246             height_ok,
 247             pos_ok);
 248     }
 249 #endif  // HAL_LOGGING_ENABLED
 250 }
 251
 252 bool ModeThrow::throw_detected()
 253 {
 254     // Check that we have a valid navigation solution
 255     nav_filter_status filt_status = inertial_nav.get_filter_status();
 256     if (!filt_status.flags.attitude || !filt_status.flags.horiz_pos_abs || !filt_status.flags.vert_pos) {
 257         return false;
 258     }
 259
 260     // Check for high speed (>500 cm/s)
 261     bool high_speed = inertial_nav.get_velocity_neu_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED);
 262
 263     // check for upwards or downwards trajectory (airdrop) of 50cm/s
 264     bool changing_height;
 265     if (g2.throw_type == ThrowType::Drop) {
 266         changing_height = inertial_nav.get_velocity_z_up_cms() < -THROW_VERTICAL_SPEED;
 267     } else {
 268         changing_height = inertial_nav.get_velocity_z_up_cms() > THROW_VERTICAL_SPEED;
 269     }
 270
 271     // Check the vertical acceleraton is greater than 0.25g
 272     bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
 273
 274     // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
 275     bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
 276
 277     // fetch the altitude above home
 278     float altitude_above_home;  // Use altitude above home if it is set, otherwise relative to EKF origin
 279     if (ahrs.home_is_set()) {
 280         ahrs.get_relative_position_D_home(altitude_above_home);
 281         altitude_above_home = -altitude_above_home; // altitude above home is returned as negative
 282     } else {
 283         altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters
 284     }
 285
 286     // Check that the altitude is within user defined limits
 287     const bool height_within_params = (g.throw_altitude_min == 0 || altitude_above_home > g.throw_altitude_min) && (g.throw_altitude_max == 0 || (altitude_above_home < g.throw_altitude_max));
 288
 289     // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
 290     bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action && height_within_params;
 291
 292
 293     // Record time and vertical velocity when we detect the possible throw
 294     if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
 295         free_fall_start_ms = AP_HAL::millis();
 296         free_fall_start_velz = inertial_nav.get_velocity_z_up_cms();
 297     }
 298
 299     // Once a possible throw condition has been detected, we check for 2.5 m/s of downwards velocity change in less than 0.5 seconds to confirm
 300     bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity_z_up_cms() - free_fall_start_velz) < -250.0f));
 301
 302     // start motors and enter the control mode if we are in continuous freefall
 303     return throw_condition_confirmed;
 304 }
 305
 306 bool ModeThrow::throw_attitude_good() const
 307 {
 308     // Check that we have uprighted the copter
 309     const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
 310     return (rotMat.c.z > 0.866f); // is_upright
 311 }
 312
 313 bool ModeThrow::throw_height_good() const
 314 {
 315     // Check that we are within 0.5m of the demanded height
 316     return (pos_control->get_pos_error_z_cm() < 50.0f);
 317 }
 318
 319 bool ModeThrow::throw_position_good() const
 320 {
 321     // check that our horizontal position error is within 50cm
 322     return (pos_control->get_pos_error_xy_cm() < 50.0f);
 323 }
 324
 325 #endif