Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_THROW_ENABLED
       4             : 
       5             : // throw_init - initialise throw controller
       6           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           0 :     return false;
      11             : #endif
      12             : 
      13             :     // do not enter the mode when already armed or when flying
      14           6 :     if (motors->armed()) {
      15           0 :         return false;
      16             :     }
      17             : 
      18             :     // init state
      19           6 :     stage = Throw_Disarmed;
      20           6 :     nextmode_attempted = false;
      21             : 
      22             :     // initialise pos controller speed and acceleration
      23           6 :     pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), BRAKE_MODE_DECEL_RATE_MSS);
      24           6 :     pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), BRAKE_MODE_DECEL_RATE_MSS);
      25             : 
      26             :     // set vertical speed and acceleration limits
      27           6 :     pos_control->D_set_max_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS);
      28           6 :     pos_control->D_set_correction_speed_accel_m(BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_SPEED_Z_MS, BRAKE_MODE_DECEL_RATE_MSS);
      29             : 
      30           6 :     return true;
      31             : }
      32             : 
      33             : // runs the throw to start controller
      34             : // should be called at 100hz or more
      35       56274 : 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       56274 :     if (!motors->armed()) {
      46             :         // state machine entry is always from a disarmed state
      47       23922 :         stage = Throw_Disarmed;
      48             : 
      49       32352 :     } else if (stage == Throw_Disarmed && motors->armed()) {
      50           6 :         gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw");
      51           6 :         stage = Throw_Detecting;
      52             : 
      53       32346 :     } else if (stage == Throw_Detecting && throw_detected()){
      54           5 :         gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
      55           5 :         copter.set_land_complete(false);
      56           5 :         stage = Throw_Wait_Throttle_Unlimited;
      57             : 
      58             :         // Cancel the waiting for throw tone sequence
      59           5 :         AP_Notify::flags.waiting_for_throw = false;
      60             : 
      61       34558 :     } else if (stage == Throw_Wait_Throttle_Unlimited &&
      62        2217 :                motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
      63           5 :         gcs().send_text(MAV_SEVERITY_INFO,"throttle is unlimited - uprighting");
      64           5 :         stage = Throw_Uprighting;
      65       32336 :     } else if (stage == Throw_Uprighting && throw_attitude_good()) {
      66           5 :         gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
      67           5 :         stage = Throw_HgtStabilise;
      68             : 
      69             :         // initialise the z controller
      70           5 :         pos_control->D_init_controller_no_descent();
      71             : 
      72             :         // initialise the demanded height below/above the throw height from user parameters
      73             :         // this allows for rapidly clearing surrounding obstacles
      74           5 :         if (g2.throw_type == ThrowType::Drop) {
      75           2 :             pos_control->set_pos_desired_U_m(pos_control->get_pos_estimate_U_m() - g.throw_altitude_descend);
      76             :         } else {
      77           3 :             pos_control->set_pos_desired_U_m(pos_control->get_pos_estimate_U_m() + g.throw_altitude_ascend);
      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           5 :         copter.set_auto_armed(true);
      82             : 
      83       32331 :     } else if (stage == Throw_HgtStabilise && throw_height_good()) {
      84           5 :         gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
      85           5 :         stage = Throw_PosHold;
      86             : 
      87             :         // initialise position controller
      88           5 :         pos_control->NE_init_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           5 :         copter.set_auto_armed(true);
      92       32326 :     } else if (stage == Throw_PosHold && throw_position_good()) {
      93        1125 :         if (!nextmode_attempted) {
      94           5 :             switch ((Mode::Number)g2.throw_nextmode.get()) {
      95           4 :                 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           4 :                     set_mode((Mode::Number)g2.throw_nextmode.get(), ModeReason::THROW_COMPLETE);
     102           4 :                     break;
     103           1 :                 default:
     104             :                     // do nothing
     105           1 :                     break;
     106             :             }
     107           5 :             nextmode_attempted = true;
     108             :         }
     109             :     }
     110             : 
     111             :     // Throw State Processing
     112       56274 :     switch (stage) {
     113             : 
     114       23922 :     case Throw_Disarmed:
     115             : 
     116             :         // prevent motors from rotating before the throw is detected unless enabled by the user
     117       23922 :         if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
     118           0 :             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
     119             :         } else {
     120       23922 :             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       23922 :         attitude_control->reset_yaw_target_and_rate();
     125       23922 :         attitude_control->reset_rate_controller_I_terms();
     126       23922 :         attitude_control->set_throttle_out(0,true,g.throttle_filt);
     127       23922 :         break;
     128             : 
     129       27442 :     case Throw_Detecting:
     130             : 
     131             :         // prevent motors from rotating before the throw is detected unless enabled by the user
     132       27442 :         if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
     133           0 :             motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
     134             :         } else {
     135       27442 :             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       27442 :         attitude_control->reset_yaw_target_and_rate();
     140       27442 :         attitude_control->reset_rate_controller_I_terms();
     141       27442 :         attitude_control->set_throttle_out(0,true,g.throttle_filt);
     142             : 
     143             :         // Play the waiting for throw tone sequence to alert the user
     144       27442 :         AP_Notify::flags.waiting_for_throw = true;
     145             : 
     146       27442 :         break;
     147             : 
     148        2217 :     case Throw_Wait_Throttle_Unlimited:
     149             : 
     150             :         // set motors to full range
     151        2217 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     152             :         // ignore motor checks
     153        2217 :         motors->set_spoolup_block(false);
     154             : 
     155        2217 :         break;
     156             : 
     157           5 :     case Throw_Uprighting:
     158             : 
     159             :         // set motors to full range
     160           5 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     161             : 
     162             :         // demand a level roll/pitch attitude with zero yaw rate
     163           5 :         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(0.0f, 0.0f, 0.0f);
     164             : 
     165             :         // output 50% throttle and turn off angle boost to maximise righting moment
     166           5 :         attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);
     167             : 
     168           5 :         break;
     169             : 
     170        1558 :     case Throw_HgtStabilise:
     171             : 
     172             :         // set motors to full range
     173        1558 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     174             : 
     175             :         // call attitude controller
     176        1558 :         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_rad(0.0f, 0.0f, 0.0f);
     177             : 
     178             :         // call height controller
     179        1558 :         pos_control->D_set_pos_target_from_climb_rate_ms(0.0f);
     180        1558 :         pos_control->D_update_controller();
     181             : 
     182        1558 :         break;
     183             : 
     184        1130 :     case Throw_PosHold:
     185             : 
     186             :         // set motors to full range
     187        1130 :         motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     188             : 
     189             :         // use position controller to stop
     190        1130 :         Vector2f vel_zero;
     191        1130 :         Vector2f accel_zero;
     192        1130 :         pos_control->input_vel_accel_NE_m(vel_zero, accel_zero);
     193        1130 :         pos_control->NE_update_controller();
     194             : 
     195             :         // call attitude controller
     196        1130 :         attitude_control->input_thrust_vector_rate_heading_rads(pos_control->get_thrust_vector(), 0.0f);
     197             : 
     198             :         // call height controller
     199        1130 :         pos_control->D_set_pos_target_from_climb_rate_ms(0.0f);
     200        1130 :         pos_control->D_update_controller();
     201             : 
     202        1130 :         break;
     203             :     }
     204             : 
     205             : #if HAL_LOGGING_ENABLED
     206             :     // log at 10hz or if stage changes
     207       56274 :     uint32_t now = AP_HAL::millis();
     208       56274 :     if ((stage != prev_stage) || (now - last_log_ms) > 100) {
     209        1392 :         prev_stage = stage;
     210        1392 :         last_log_ms = now;
     211        1392 :         const float velocity_ms = pos_control->get_vel_estimate_NED_ms().length();
     212        1392 :         const float velocity_z_ms = pos_control->get_vel_estimate_U_ms();
     213        1392 :         const float accel_mss = copter.ins.get_accel().length();
     214        1392 :         const float ef_accel_z_mss = ahrs.get_accel_ef().z;
     215        1392 :         const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
     216        1392 :         const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
     217        1392 :         const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
     218        1392 :         const bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
     219             : 
     220             : // @LoggerMessage: THRO
     221             : // @Description: Throw Mode messages
     222             : // @URL: https://ardupilot.club/copter/docs/throw-mode.html
     223             : // @Field: TimeUS: Time since system startup
     224             : // @Field: Stage: Current stage of the Throw Mode
     225             : // @Field: Vel: Magnitude of the velocity vector
     226             : // @Field: VelZ: Vertical Velocity
     227             : // @Field: Acc: Magnitude of the vector of the current acceleration
     228             : // @Field: AccEfZ: Vertical earth frame accelerometer value
     229             : // @Field: Throw: True if a throw has been detected since entering this mode
     230             : // @Field: AttOk: True if the vehicle is upright 
     231             : // @Field: HgtOk: True if the vehicle is within 0.5 m of the demanded height
     232             : // @Field: PosOk: True if the vehicle is within 0.5 m of the demanded horizontal position
     233             : 
     234        2784 :         AP::logger().WriteStreaming(
     235             :             "THRO",
     236             :             "TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
     237             :             "s-nnoo----",
     238             :             "F-0000----",
     239             :             "QBffffbbbb",
     240             :             AP_HAL::micros64(),
     241        1392 :             (uint8_t)stage,
     242             :             (double)velocity_ms,
     243             :             (double)velocity_z_ms,
     244             :             (double)accel_mss,
     245             :             (double)ef_accel_z_mss,
     246             :             throw_detect,
     247             :             attitude_ok,
     248             :             height_ok,
     249             :             pos_ok);
     250             :     }
     251             : #endif  // HAL_LOGGING_ENABLED
     252       56274 : }
     253             : 
     254       28700 : bool ModeThrow::throw_detected()
     255             : {
     256             :     // Check that the AHRS is healthy enough for us to be doing detection:
     257       28700 :     if (!ahrs.has_status(AP_AHRS::Status::ATTITUDE_VALID)) {
     258           0 :         return false;
     259             :     }
     260       28700 :     if (!ahrs.has_status(AP_AHRS::Status::HORIZ_POS_ABS)) {
     261         345 :         return false;
     262             :     }
     263       28355 :     if (!ahrs.has_status(AP_AHRS::Status::VERT_POS)) {
     264           0 :         return false;
     265             :     }
     266             : 
     267             :     // Check for high speed ( >5 m/s)
     268       28355 :     bool high_speed = pos_control->get_vel_estimate_NED_ms().length_squared() > (THROW_HIGH_SPEED_MS * THROW_HIGH_SPEED_MS);
     269             : 
     270             :     // check for upwards or downwards trajectory (airdrop) of 0.50 m/s
     271             :     bool changing_height;
     272       28355 :     if (g2.throw_type == ThrowType::Drop) {
     273       25750 :         changing_height = pos_control->get_vel_estimate_U_ms() < -THROW_VERTICAL_SPEED_MS;
     274             :     } else {
     275        2605 :         changing_height = pos_control->get_vel_estimate_U_ms() > THROW_VERTICAL_SPEED_MS;
     276             :     }
     277             : 
     278             :     // Check the vertical acceleration is greater than 0.25g
     279       28355 :     bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
     280             : 
     281             :     // Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
     282       28355 :     bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
     283             : 
     284             :     // fetch the altitude above home
     285             :     float altitude_above_home_m;  // Use altitude above home if it is set, otherwise relative to EKF origin
     286       28355 :     if (ahrs.home_is_set()) {
     287       28355 :         ahrs.get_relative_position_D_home(altitude_above_home_m);
     288       28355 :         altitude_above_home_m = -altitude_above_home_m; // altitude above home is returned as negative
     289             :     } else {
     290           0 :         altitude_above_home_m = pos_control->get_pos_estimate_U_m();
     291             :     }
     292             : 
     293             :     // Check that the altitude is within user defined limits
     294       28355 :     const bool height_within_params = (g.throw_altitude_min == 0 || altitude_above_home_m > g.throw_altitude_min) && (g.throw_altitude_max == 0 || (altitude_above_home_m < g.throw_altitude_max));
     295             : 
     296             :     // High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release  
     297       28355 :     bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action && height_within_params;
     298             : 
     299             : 
     300             :     // Record time and vertical velocity when we detect the possible throw
     301       28355 :     if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
     302           5 :         free_fall_start_ms = AP_HAL::millis();
     303           5 :         free_fall_start_vel_u_ms = pos_control->get_vel_estimate_U_ms();
     304             :     }
     305             : 
     306             :     // 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
     307       28355 :     bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((pos_control->get_vel_estimate_U_ms() - free_fall_start_vel_u_ms) < -2.5));
     308             : 
     309             :     // start motors and enter the control mode if we are in continuous freefall
     310       28355 :     return throw_condition_confirmed;
     311             : }
     312             : 
     313        1324 : bool ModeThrow::throw_attitude_good() const
     314             : {
     315             :     // Check that we have uprighted the copter
     316        1324 :     const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
     317        1324 :     return (rotMat.c.z > 0.866f); // is_upright
     318             : }
     319             : 
     320        2918 : bool ModeThrow::throw_height_good() const
     321             : {
     322             :     // Check that we are within 0.5m of the demanded height
     323        2918 :     return (fabsf(pos_control->get_pos_error_D_m()) < 0.5);
     324             : }
     325             : 
     326        2517 : bool ModeThrow::throw_position_good() const
     327             : {
     328             :     // check that our horizontal position error is within 0.5 m
     329        2517 :     return (pos_control->get_pos_error_NE_m() < 0.50);
     330             : }
     331             : 
     332             : #endif