2015-12-18 05:46:56 -04:00
|
|
|
#include "Copter.h"
|
|
|
|
|
2024-09-04 00:48:37 -03:00
|
|
|
#if MODE_THROW_ENABLED
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// throw_init - initialise throw controller
|
2019-05-09 23:18:49 -03:00
|
|
|
bool ModeThrow::init(bool ignore_checks)
|
2015-12-18 05:46:56 -04:00
|
|
|
{
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
|
|
// do not allow helis to use throw to start
|
|
|
|
return false;
|
|
|
|
#endif
|
2015-12-19 01:59:44 -04:00
|
|
|
|
2016-01-21 22:59:47 -04:00
|
|
|
// do not enter the mode when already armed or when flying
|
2017-01-09 03:31:26 -04:00
|
|
|
if (motors->armed()) {
|
2015-12-19 01:59:44 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2016-08-01 09:42:09 -03:00
|
|
|
// init state
|
2016-08-02 23:30:27 -03:00
|
|
|
stage = Throw_Disarmed;
|
|
|
|
nextmode_attempted = false;
|
2016-08-01 09:42:09 -03:00
|
|
|
|
2021-05-11 01:42:02 -03:00
|
|
|
// initialise pos controller speed and acceleration
|
|
|
|
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE);
|
2021-07-08 01:17:41 -03:00
|
|
|
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE);
|
2021-05-11 01:42:02 -03:00
|
|
|
|
2021-05-19 11:07:38 -03:00
|
|
|
// set vertical speed and acceleration limits
|
2021-05-11 01:42:02 -03:00
|
|
|
pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
|
2021-07-08 01:17:41 -03:00
|
|
|
pos_control->set_correction_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
|
2021-05-11 01:42:02 -03:00
|
|
|
|
2016-03-22 04:21:58 -03:00
|
|
|
return true;
|
2015-12-18 05:46:56 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// runs the throw to start controller
|
|
|
|
// should be called at 100hz or more
|
2019-05-09 23:18:49 -03:00
|
|
|
void ModeThrow::run()
|
2015-12-18 05:46:56 -04:00
|
|
|
{
|
|
|
|
/* Throw State Machine
|
|
|
|
Throw_Disarmed - motors are off
|
|
|
|
Throw_Detecting - motors are on and we are waiting for the throw
|
|
|
|
Throw_Uprighting - the throw has been detected and the copter is being uprighted
|
|
|
|
Throw_HgtStabilise - the copter is kept level and height is stabilised about the target height
|
|
|
|
Throw_PosHold - the copter is kept at a constant position and height
|
|
|
|
*/
|
|
|
|
|
2019-02-28 05:03:23 -04:00
|
|
|
if (!motors->armed()) {
|
2015-12-18 05:46:56 -04:00
|
|
|
// state machine entry is always from a disarmed state
|
2016-08-02 23:30:27 -03:00
|
|
|
stage = Throw_Disarmed;
|
2015-12-18 05:46:56 -04:00
|
|
|
|
2016-08-02 23:30:27 -03:00
|
|
|
} else if (stage == Throw_Disarmed && motors->armed()) {
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw");
|
2016-08-02 23:30:27 -03:00
|
|
|
stage = Throw_Detecting;
|
2015-12-18 05:46:56 -04:00
|
|
|
|
2016-08-02 23:30:27 -03:00
|
|
|
} else if (stage == Throw_Detecting && throw_detected()){
|
2021-07-02 03:37:03 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
|
2022-08-26 22:09:22 -03:00
|
|
|
copter.set_land_complete(false);
|
2021-07-02 03:37:03 -03:00
|
|
|
stage = Throw_Wait_Throttle_Unlimited;
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// Cancel the waiting for throw tone sequence
|
|
|
|
AP_Notify::flags.waiting_for_throw = false;
|
|
|
|
|
2021-07-13 22:49:15 -03:00
|
|
|
} else if (stage == Throw_Wait_Throttle_Unlimited &&
|
|
|
|
motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
|
2021-07-02 03:37:03 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"throttle is unlimited - uprighting");
|
|
|
|
stage = Throw_Uprighting;
|
2016-08-02 23:30:27 -03:00
|
|
|
} else if (stage == Throw_Uprighting && throw_attitude_good()) {
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
|
2016-08-02 23:30:27 -03:00
|
|
|
stage = Throw_HgtStabilise;
|
2015-12-18 05:46:56 -04:00
|
|
|
|
2021-05-11 01:42:02 -03:00
|
|
|
// initialise the z controller
|
|
|
|
pos_control->init_z_controller_no_descent();
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// initialise the demanded height to 3m above the throw height
|
|
|
|
// we want to rapidly clear surrounding obstacles
|
2020-12-05 03:10:00 -04:00
|
|
|
if (g2.throw_type == ThrowType::Drop) {
|
2021-10-20 05:29:57 -03:00
|
|
|
pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() - 100);
|
2016-08-01 23:26:48 -03:00
|
|
|
} else {
|
2021-10-20 05:29:57 -03:00
|
|
|
pos_control->set_pos_target_z_cm(inertial_nav.get_position_z_up_cm() + 300);
|
2016-08-01 23:26:48 -03:00
|
|
|
}
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.set_auto_armed(true);
|
2015-12-18 05:46:56 -04:00
|
|
|
|
2016-08-02 23:30:27 -03:00
|
|
|
} else if (stage == Throw_HgtStabilise && throw_height_good()) {
|
2017-07-08 21:56:49 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
|
2016-08-02 23:30:27 -03:00
|
|
|
stage = Throw_PosHold;
|
2015-12-18 05:46:56 -04:00
|
|
|
|
2021-05-11 01:42:02 -03:00
|
|
|
// initialise position controller
|
|
|
|
pos_control->init_xy_controller();
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
|
2018-02-07 22:21:09 -04:00
|
|
|
copter.set_auto_armed(true);
|
2016-08-02 23:30:27 -03:00
|
|
|
} else if (stage == Throw_PosHold && throw_position_good()) {
|
|
|
|
if (!nextmode_attempted) {
|
2019-09-07 20:33:56 -03:00
|
|
|
switch ((Mode::Number)g2.throw_nextmode.get()) {
|
|
|
|
case Mode::Number::AUTO:
|
|
|
|
case Mode::Number::GUIDED:
|
|
|
|
case Mode::Number::RTL:
|
|
|
|
case Mode::Number::LAND:
|
|
|
|
case Mode::Number::BRAKE:
|
|
|
|
case Mode::Number::LOITER:
|
2019-10-17 00:49:22 -03:00
|
|
|
set_mode((Mode::Number)g2.throw_nextmode.get(), ModeReason::THROW_COMPLETE);
|
2016-08-01 09:42:09 -03:00
|
|
|
break;
|
|
|
|
default:
|
|
|
|
// do nothing
|
|
|
|
break;
|
|
|
|
}
|
2016-08-02 23:30:27 -03:00
|
|
|
nextmode_attempted = true;
|
2016-08-01 09:42:09 -03:00
|
|
|
}
|
2015-12-18 05:46:56 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// Throw State Processing
|
2016-08-02 23:30:27 -03:00
|
|
|
switch (stage) {
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
case Throw_Disarmed:
|
|
|
|
|
2016-08-02 04:07:48 -03:00
|
|
|
// prevent motors from rotating before the throw is detected unless enabled by the user
|
2020-12-05 03:18:28 -04:00
|
|
|
if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
2016-08-02 04:07:48 -03:00
|
|
|
} else {
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
2016-08-02 04:07:48 -03:00
|
|
|
}
|
|
|
|
|
2015-12-18 05:46:56 -04:00
|
|
|
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
|
2021-05-24 10:42:19 -03:00
|
|
|
attitude_control->reset_yaw_target_and_rate();
|
2019-04-05 07:38:17 -03:00
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
2019-02-28 05:03:23 -04:00
|
|
|
attitude_control->set_throttle_out(0,true,g.throttle_filt);
|
2015-12-18 05:46:56 -04:00
|
|
|
break;
|
|
|
|
|
|
|
|
case Throw_Detecting:
|
|
|
|
|
2016-08-02 04:07:48 -03:00
|
|
|
// prevent motors from rotating before the throw is detected unless enabled by the user
|
2020-12-05 03:18:28 -04:00
|
|
|
if (g.throw_motor_start == PreThrowMotorState::RUNNING) {
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
2016-08-02 04:07:48 -03:00
|
|
|
} else {
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
|
2016-08-02 04:07:48 -03:00
|
|
|
}
|
|
|
|
|
2015-12-18 05:46:56 -04:00
|
|
|
// Hold throttle at zero during the throw and continually reset the attitude controller
|
2021-05-24 10:42:19 -03:00
|
|
|
attitude_control->reset_yaw_target_and_rate();
|
2019-04-05 07:38:17 -03:00
|
|
|
attitude_control->reset_rate_controller_I_terms();
|
2019-02-28 05:03:23 -04:00
|
|
|
attitude_control->set_throttle_out(0,true,g.throttle_filt);
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// Play the waiting for throw tone sequence to alert the user
|
|
|
|
AP_Notify::flags.waiting_for_throw = true;
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
2021-07-02 03:37:03 -03:00
|
|
|
case Throw_Wait_Throttle_Unlimited:
|
|
|
|
|
|
|
|
// set motors to full range
|
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
2015-12-18 05:46:56 -04:00
|
|
|
case Throw_Uprighting:
|
|
|
|
|
2016-08-02 04:07:48 -03:00
|
|
|
// set motors to full range
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2016-08-02 04:07:48 -03:00
|
|
|
|
2015-12-18 05:46:56 -04:00
|
|
|
// demand a level roll/pitch attitude with zero yaw rate
|
2017-06-26 05:48:04 -03:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// output 50% throttle and turn off angle boost to maximise righting moment
|
2017-01-09 03:31:26 -04:00
|
|
|
attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
case Throw_HgtStabilise:
|
|
|
|
|
2016-08-02 04:07:48 -03:00
|
|
|
// set motors to full range
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2016-08-02 04:07:48 -03:00
|
|
|
|
2015-12-18 05:46:56 -04:00
|
|
|
// call attitude controller
|
2017-06-26 05:48:04 -03:00
|
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// call height controller
|
2021-08-31 01:17:59 -03:00
|
|
|
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->update_z_controller();
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
case Throw_PosHold:
|
|
|
|
|
2016-08-02 04:07:48 -03:00
|
|
|
// set motors to full range
|
2019-04-09 09:16:58 -03:00
|
|
|
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
2016-08-02 04:07:48 -03:00
|
|
|
|
2021-05-11 01:42:02 -03:00
|
|
|
// use position controller to stop
|
2021-06-21 04:22:48 -03:00
|
|
|
Vector2f vel;
|
|
|
|
Vector2f accel;
|
2021-05-11 01:42:02 -03:00
|
|
|
pos_control->input_vel_accel_xy(vel, accel);
|
|
|
|
pos_control->update_xy_controller();
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// call attitude controller
|
2021-05-11 01:42:02 -03:00
|
|
|
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f);
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// call height controller
|
2021-08-31 01:17:59 -03:00
|
|
|
pos_control->set_pos_target_z_from_climb_rate_cm(0.0f);
|
2017-01-09 03:31:26 -04:00
|
|
|
pos_control->update_z_controller();
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
2016-08-02 02:41:39 -03:00
|
|
|
|
2023-07-13 21:58:09 -03:00
|
|
|
#if HAL_LOGGING_ENABLED
|
2016-08-02 02:41:39 -03:00
|
|
|
// log at 10hz or if stage changes
|
|
|
|
uint32_t now = AP_HAL::millis();
|
2016-08-02 23:30:27 -03:00
|
|
|
if ((stage != prev_stage) || (now - last_log_ms) > 100) {
|
|
|
|
prev_stage = stage;
|
|
|
|
last_log_ms = now;
|
2021-10-20 05:29:57 -03:00
|
|
|
const float velocity = inertial_nav.get_velocity_neu_cms().length();
|
|
|
|
const float velocity_z = inertial_nav.get_velocity_z_up_cms();
|
2018-04-17 12:38:47 -03:00
|
|
|
const float accel = copter.ins.get_accel().length();
|
|
|
|
const float ef_accel_z = ahrs.get_accel_ef().z;
|
|
|
|
const bool throw_detect = (stage > Throw_Detecting) || throw_detected();
|
|
|
|
const bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
|
|
|
|
const bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
|
|
|
|
const bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
|
2023-07-13 21:58:09 -03:00
|
|
|
|
2020-04-04 06:01:45 -03:00
|
|
|
// @LoggerMessage: THRO
|
|
|
|
// @Description: Throw Mode messages
|
|
|
|
// @URL: https://ardupilot.org/copter/docs/throw-mode.html
|
|
|
|
// @Field: TimeUS: Time since system startup
|
|
|
|
// @Field: Stage: Current stage of the Throw Mode
|
|
|
|
// @Field: Vel: Magnitude of the velocity vector
|
|
|
|
// @Field: VelZ: Vertical Velocity
|
|
|
|
// @Field: Acc: Magnitude of the vector of the current acceleration
|
|
|
|
// @Field: AccEfZ: Vertical earth frame accelerometer value
|
|
|
|
// @Field: Throw: True if a throw has been detected since entering this mode
|
|
|
|
// @Field: AttOk: True if the vehicle is upright
|
|
|
|
// @Field: HgtOk: True if the vehicle is within 50cm of the demanded height
|
|
|
|
// @Field: PosOk: True if the vehicle is within 50cm of the demanded horizontal position
|
2023-07-13 21:58:09 -03:00
|
|
|
|
2021-08-17 06:57:41 -03:00
|
|
|
AP::logger().WriteStreaming(
|
2017-12-13 05:57:38 -04:00
|
|
|
"THRO",
|
|
|
|
"TimeUS,Stage,Vel,VelZ,Acc,AccEfZ,Throw,AttOk,HgtOk,PosOk",
|
|
|
|
"s-nnoo----",
|
|
|
|
"F-0000----",
|
|
|
|
"QBffffbbbb",
|
|
|
|
AP_HAL::micros64(),
|
|
|
|
(uint8_t)stage,
|
2018-04-17 12:38:47 -03:00
|
|
|
(double)velocity,
|
|
|
|
(double)velocity_z,
|
|
|
|
(double)accel,
|
|
|
|
(double)ef_accel_z,
|
2017-12-13 05:57:38 -04:00
|
|
|
throw_detect,
|
|
|
|
attitude_ok,
|
|
|
|
height_ok,
|
|
|
|
pos_ok);
|
2016-08-02 02:41:39 -03:00
|
|
|
}
|
2023-07-13 21:58:09 -03:00
|
|
|
#endif // HAL_LOGGING_ENABLED
|
2015-12-18 05:46:56 -04:00
|
|
|
}
|
|
|
|
|
2019-05-09 23:18:49 -03:00
|
|
|
bool ModeThrow::throw_detected()
|
2015-12-18 05:46:56 -04:00
|
|
|
{
|
|
|
|
// Check that we have a valid navigation solution
|
|
|
|
nav_filter_status filt_status = inertial_nav.get_filter_status();
|
|
|
|
if (!filt_status.flags.attitude || !filt_status.flags.horiz_pos_abs || !filt_status.flags.vert_pos) {
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2016-08-02 02:30:32 -03:00
|
|
|
// Check for high speed (>500 cm/s)
|
2021-10-20 05:29:57 -03:00
|
|
|
bool high_speed = inertial_nav.get_velocity_neu_cms().length_squared() > (THROW_HIGH_SPEED * THROW_HIGH_SPEED);
|
2015-12-18 05:46:56 -04:00
|
|
|
|
2016-08-02 02:30:32 -03:00
|
|
|
// check for upwards or downwards trajectory (airdrop) of 50cm/s
|
2016-08-01 23:26:48 -03:00
|
|
|
bool changing_height;
|
2020-12-05 03:10:00 -04:00
|
|
|
if (g2.throw_type == ThrowType::Drop) {
|
2021-10-20 05:29:57 -03:00
|
|
|
changing_height = inertial_nav.get_velocity_z_up_cms() < -THROW_VERTICAL_SPEED;
|
2016-08-01 23:26:48 -03:00
|
|
|
} else {
|
2021-10-20 05:29:57 -03:00
|
|
|
changing_height = inertial_nav.get_velocity_z_up_cms() > THROW_VERTICAL_SPEED;
|
2016-08-01 23:26:48 -03:00
|
|
|
}
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// Check the vertical acceleraton is greater than 0.25g
|
|
|
|
bool free_falling = ahrs.get_accel_ef().z > -0.25 * GRAVITY_MSS;
|
|
|
|
|
|
|
|
// Check if the accel length is < 1.0g indicating that any throw action is complete and the copter has been released
|
2018-02-07 22:21:09 -04:00
|
|
|
bool no_throw_action = copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
|
2015-12-18 05:46:56 -04:00
|
|
|
|
2024-01-02 18:49:18 -04:00
|
|
|
// fetch the altitude above home
|
|
|
|
float altitude_above_home; // Use altitude above home if it is set, otherwise relative to EKF origin
|
|
|
|
if (ahrs.home_is_set()) {
|
|
|
|
ahrs.get_relative_position_D_home(altitude_above_home);
|
|
|
|
altitude_above_home = -altitude_above_home; // altitude above home is returned as negative
|
|
|
|
} else {
|
|
|
|
altitude_above_home = inertial_nav.get_position_z_up_cm() * 0.01f; // centimeters to meters
|
|
|
|
}
|
|
|
|
|
|
|
|
// Check that the altitude is within user defined limits
|
|
|
|
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));
|
|
|
|
|
|
|
|
// High velocity or free-fall combined with increasing height indicate a possible air-drop or throw release
|
|
|
|
bool possible_throw_detected = (free_falling || high_speed) && changing_height && no_throw_action && height_within_params;
|
|
|
|
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// Record time and vertical velocity when we detect the possible throw
|
2016-08-02 23:30:27 -03:00
|
|
|
if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
|
|
|
|
free_fall_start_ms = AP_HAL::millis();
|
2021-10-20 05:29:57 -03:00
|
|
|
free_fall_start_velz = inertial_nav.get_velocity_z_up_cms();
|
2015-12-18 05:46:56 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// 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
|
2021-10-20 05:29:57 -03:00
|
|
|
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));
|
2015-12-18 05:46:56 -04:00
|
|
|
|
|
|
|
// start motors and enter the control mode if we are in continuous freefall
|
2021-02-03 10:04:38 -04:00
|
|
|
return throw_condition_confirmed;
|
2015-12-18 05:46:56 -04:00
|
|
|
}
|
|
|
|
|
2021-07-02 03:06:42 -03:00
|
|
|
bool ModeThrow::throw_attitude_good() const
|
2015-12-18 05:46:56 -04:00
|
|
|
{
|
|
|
|
// Check that we have uprighted the copter
|
|
|
|
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
|
2016-11-02 21:23:26 -03:00
|
|
|
return (rotMat.c.z > 0.866f); // is_upright
|
2015-12-18 05:46:56 -04:00
|
|
|
}
|
|
|
|
|
2021-07-02 03:06:42 -03:00
|
|
|
bool ModeThrow::throw_height_good() const
|
2015-12-18 05:46:56 -04:00
|
|
|
{
|
2017-12-21 19:40:15 -04:00
|
|
|
// Check that we are within 0.5m of the demanded height
|
2021-05-11 01:42:02 -03:00
|
|
|
return (pos_control->get_pos_error_z_cm() < 50.0f);
|
2015-12-18 05:46:56 -04:00
|
|
|
}
|
|
|
|
|
2021-07-02 03:06:42 -03:00
|
|
|
bool ModeThrow::throw_position_good() const
|
2016-08-01 09:42:09 -03:00
|
|
|
{
|
|
|
|
// check that our horizontal position error is within 50cm
|
2021-05-11 01:42:02 -03:00
|
|
|
return (pos_control->get_pos_error_xy_cm() < 50.0f);
|
2016-08-01 09:42:09 -03:00
|
|
|
}
|
2021-07-02 03:37:03 -03:00
|
|
|
|
2018-03-14 17:16:16 -03:00
|
|
|
#endif
|