mirror of https://github.com/ArduPilot/ardupilot
Copter: FlightMode - convert THROW flight mode
This commit is contained in:
parent
1de99c737e
commit
cf423ce681
|
@ -415,16 +415,6 @@ private:
|
|||
int32_t nav_delay_time_max; // used for delaying the navigation commands (eg land,takeoff etc.)
|
||||
uint32_t nav_delay_time_start;
|
||||
|
||||
// throw mode state
|
||||
struct {
|
||||
ThrowModeStage stage;
|
||||
ThrowModeStage prev_stage;
|
||||
uint32_t last_log_ms;
|
||||
bool nextmode_attempted;
|
||||
uint32_t free_fall_start_ms; // system time free fall was detected
|
||||
float free_fall_start_velz; // vertical velocity when free fall was detected
|
||||
} throw_state = {Throw_Disarmed, Throw_Disarmed, 0, false, 0, 0.0f};
|
||||
|
||||
// Battery Sensors
|
||||
AP_BattMonitor battery = AP_BattMonitor::create();
|
||||
|
||||
|
@ -782,14 +772,6 @@ private:
|
|||
void set_mode_land_with_pause(mode_reason_t reason);
|
||||
bool landing_with_GPS();
|
||||
|
||||
// Throw to launch functionality
|
||||
bool throw_init(bool ignore_checks);
|
||||
void throw_run();
|
||||
bool throw_detected();
|
||||
bool throw_attitude_good();
|
||||
bool throw_height_good();
|
||||
bool throw_position_good();
|
||||
|
||||
bool smart_rtl_init(bool ignore_checks);
|
||||
void smart_rtl_exit();
|
||||
void smart_rtl_run();
|
||||
|
@ -1036,6 +1018,8 @@ private:
|
|||
|
||||
Copter::FlightMode_AVOID_ADSB flightmode_avoid_adsb{*this};
|
||||
|
||||
Copter::FlightMode_THROW flightmode_throw{*this};
|
||||
|
||||
public:
|
||||
void mavlink_delay_cb();
|
||||
void failsafe_check();
|
||||
|
|
|
@ -824,3 +824,43 @@ protected:
|
|||
private:
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class FlightMode_THROW : public FlightMode {
|
||||
|
||||
public:
|
||||
|
||||
FlightMode_THROW(Copter &copter) :
|
||||
Copter::FlightMode(copter)
|
||||
{ }
|
||||
|
||||
bool init(bool ignore_checks) override;
|
||||
void run() override; // should be called at 100hz or more
|
||||
|
||||
bool requires_GPS() const override { return true; }
|
||||
bool has_manual_throttle() const override { return false; }
|
||||
bool allows_arming(bool from_gcs) const override { return true; };
|
||||
bool is_autopilot() const override { return false; }
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
const char *name() const override { return "THROW"; }
|
||||
const char *name4() const override { return "THRW"; }
|
||||
|
||||
private:
|
||||
|
||||
bool throw_detected();
|
||||
bool throw_position_good();
|
||||
bool throw_height_good();
|
||||
bool throw_attitude_good();
|
||||
|
||||
ThrowModeStage stage = Throw_Disarmed;
|
||||
ThrowModeStage prev_stage = Throw_Disarmed;
|
||||
uint32_t last_log_ms;
|
||||
bool nextmode_attempted;
|
||||
uint32_t free_fall_start_ms; // system time free fall was detected
|
||||
float free_fall_start_velz; // vertical velocity when free fall was detected
|
||||
|
||||
};
|
||||
|
|
|
@ -2,7 +2,7 @@
|
|||
|
||||
|
||||
// throw_init - initialise throw controller
|
||||
bool Copter::throw_init(bool ignore_checks)
|
||||
bool Copter::FlightMode_THROW::init(bool ignore_checks)
|
||||
{
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// do not allow helis to use throw to start
|
||||
|
@ -15,15 +15,15 @@ bool Copter::throw_init(bool ignore_checks)
|
|||
}
|
||||
|
||||
// init state
|
||||
throw_state.stage = Throw_Disarmed;
|
||||
throw_state.nextmode_attempted = false;
|
||||
stage = Throw_Disarmed;
|
||||
nextmode_attempted = false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// runs the throw to start controller
|
||||
// should be called at 100hz or more
|
||||
void Copter::throw_run()
|
||||
void Copter::FlightMode_THROW::run()
|
||||
{
|
||||
/* Throw State Machine
|
||||
Throw_Disarmed - motors are off
|
||||
|
@ -36,22 +36,22 @@ void Copter::throw_run()
|
|||
// Don't enter THROW mode if interlock will prevent motors running
|
||||
if (!motors->armed() && motors->get_interlock()) {
|
||||
// state machine entry is always from a disarmed state
|
||||
throw_state.stage = Throw_Disarmed;
|
||||
stage = Throw_Disarmed;
|
||||
|
||||
} else if (throw_state.stage == Throw_Disarmed && motors->armed()) {
|
||||
} else if (stage == Throw_Disarmed && motors->armed()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"waiting for throw");
|
||||
throw_state.stage = Throw_Detecting;
|
||||
stage = Throw_Detecting;
|
||||
|
||||
} else if (throw_state.stage == Throw_Detecting && throw_detected()){
|
||||
} else if (stage == Throw_Detecting && throw_detected()){
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
|
||||
throw_state.stage = Throw_Uprighting;
|
||||
stage = Throw_Uprighting;
|
||||
|
||||
// Cancel the waiting for throw tone sequence
|
||||
AP_Notify::flags.waiting_for_throw = false;
|
||||
|
||||
} else if (throw_state.stage == Throw_Uprighting && throw_attitude_good()) {
|
||||
} else if (stage == Throw_Uprighting && throw_attitude_good()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
|
||||
throw_state.stage = Throw_HgtStabilise;
|
||||
stage = Throw_HgtStabilise;
|
||||
|
||||
// initialize vertical speed and acceleration limits
|
||||
// use brake mode values for rapid response
|
||||
|
@ -71,19 +71,19 @@ void Copter::throw_run()
|
|||
pos_control->set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
|
||||
|
||||
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
|
||||
set_auto_armed(true);
|
||||
_copter.set_auto_armed(true);
|
||||
|
||||
} else if (throw_state.stage == Throw_HgtStabilise && throw_height_good()) {
|
||||
} else if (stage == Throw_HgtStabilise && throw_height_good()) {
|
||||
gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position");
|
||||
throw_state.stage = Throw_PosHold;
|
||||
stage = Throw_PosHold;
|
||||
|
||||
// initialise the loiter target to the curent position and velocity
|
||||
wp_nav->init_loiter_target();
|
||||
|
||||
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
|
||||
set_auto_armed(true);
|
||||
} else if (throw_state.stage == Throw_PosHold && throw_position_good()) {
|
||||
if (!throw_state.nextmode_attempted) {
|
||||
_copter.set_auto_armed(true);
|
||||
} else if (stage == Throw_PosHold && throw_position_good()) {
|
||||
if (!nextmode_attempted) {
|
||||
switch (g2.throw_nextmode) {
|
||||
case AUTO:
|
||||
case GUIDED:
|
||||
|
@ -97,12 +97,12 @@ void Copter::throw_run()
|
|||
// do nothing
|
||||
break;
|
||||
}
|
||||
throw_state.nextmode_attempted = true;
|
||||
nextmode_attempted = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Throw State Processing
|
||||
switch (throw_state.stage) {
|
||||
switch (stage) {
|
||||
|
||||
case Throw_Disarmed:
|
||||
|
||||
|
@ -181,18 +181,18 @@ void Copter::throw_run()
|
|||
|
||||
// log at 10hz or if stage changes
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if ((throw_state.stage != throw_state.prev_stage) || (now - throw_state.last_log_ms) > 100) {
|
||||
throw_state.prev_stage = throw_state.stage;
|
||||
throw_state.last_log_ms = now;
|
||||
if ((stage != prev_stage) || (now - last_log_ms) > 100) {
|
||||
prev_stage = stage;
|
||||
last_log_ms = now;
|
||||
float velocity = inertial_nav.get_velocity().length();
|
||||
float velocity_z = inertial_nav.get_velocity().z;
|
||||
float accel = ins.get_accel().length();
|
||||
float accel = _copter.ins.get_accel().length();
|
||||
float ef_accel_z = ahrs.get_accel_ef().z;
|
||||
bool throw_detect = (throw_state.stage > Throw_Detecting) || throw_detected();
|
||||
bool attitude_ok = (throw_state.stage > Throw_Uprighting) || throw_attitude_good();
|
||||
bool height_ok = (throw_state.stage > Throw_HgtStabilise) || throw_height_good();
|
||||
bool pos_ok = (throw_state.stage > Throw_PosHold) || throw_position_good();
|
||||
Log_Write_Throw(throw_state.stage,
|
||||
bool throw_detect = (stage > Throw_Detecting) || throw_detected();
|
||||
bool attitude_ok = (stage > Throw_Uprighting) || throw_attitude_good();
|
||||
bool height_ok = (stage > Throw_HgtStabilise) || throw_height_good();
|
||||
bool pos_ok = (stage > Throw_PosHold) || throw_position_good();
|
||||
_copter.Log_Write_Throw(stage,
|
||||
velocity,
|
||||
velocity_z,
|
||||
accel,
|
||||
|
@ -204,7 +204,7 @@ void Copter::throw_run()
|
|||
}
|
||||
}
|
||||
|
||||
bool Copter::throw_detected()
|
||||
bool Copter::FlightMode_THROW::throw_detected()
|
||||
{
|
||||
// Check that we have a valid navigation solution
|
||||
nav_filter_status filt_status = inertial_nav.get_filter_status();
|
||||
|
@ -227,19 +227,19 @@ bool Copter::throw_detected()
|
|||
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
|
||||
bool no_throw_action = ins.get_accel().length() < 1.0f * GRAVITY_MSS;
|
||||
bool no_throw_action = _copter.ins.get_accel().length() < 1.0f * GRAVITY_MSS;
|
||||
|
||||
// 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;
|
||||
|
||||
// Record time and vertical velocity when we detect the possible throw
|
||||
if (possible_throw_detected && ((AP_HAL::millis() - throw_state.free_fall_start_ms) > 500)) {
|
||||
throw_state.free_fall_start_ms = AP_HAL::millis();
|
||||
throw_state.free_fall_start_velz = inertial_nav.get_velocity().z;
|
||||
if (possible_throw_detected && ((AP_HAL::millis() - free_fall_start_ms) > 500)) {
|
||||
free_fall_start_ms = AP_HAL::millis();
|
||||
free_fall_start_velz = inertial_nav.get_velocity().z;
|
||||
}
|
||||
|
||||
// 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
|
||||
bool throw_condition_confirmed = ((AP_HAL::millis() - throw_state.free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - throw_state.free_fall_start_velz) < -250.0f));
|
||||
bool throw_condition_confirmed = ((AP_HAL::millis() - free_fall_start_ms < 500) && ((inertial_nav.get_velocity().z - free_fall_start_velz) < -250.0f));
|
||||
|
||||
// start motors and enter the control mode if we are in continuous freefall
|
||||
if (throw_condition_confirmed) {
|
||||
|
@ -249,20 +249,20 @@ bool Copter::throw_detected()
|
|||
}
|
||||
}
|
||||
|
||||
bool Copter::throw_attitude_good()
|
||||
bool Copter::FlightMode_THROW::throw_attitude_good()
|
||||
{
|
||||
// Check that we have uprighted the copter
|
||||
const Matrix3f &rotMat = ahrs.get_rotation_body_to_ned();
|
||||
return (rotMat.c.z > 0.866f); // is_upright
|
||||
}
|
||||
|
||||
bool Copter::throw_height_good()
|
||||
bool Copter::FlightMode_THROW::throw_height_good()
|
||||
{
|
||||
// Check that we are no more than 0.5m below the demanded height
|
||||
return (pos_control->get_alt_error() < 50.0f);
|
||||
}
|
||||
|
||||
bool Copter::throw_position_good()
|
||||
bool Copter::FlightMode_THROW::throw_position_good()
|
||||
{
|
||||
// check that our horizontal position error is within 50cm
|
||||
return (pos_control->get_horizontal_error() < 50.0f);
|
||||
|
|
|
@ -149,7 +149,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
|
|||
break;
|
||||
|
||||
case THROW:
|
||||
success = throw_init(ignore_checks);
|
||||
success = flightmode_throw.init(ignore_checks);
|
||||
if (success) {
|
||||
flightmode = &flightmode_throw;
|
||||
}
|
||||
break;
|
||||
|
||||
case AVOID_ADSB:
|
||||
|
@ -233,10 +236,6 @@ void Copter::update_flight_mode()
|
|||
|
||||
switch (control_mode) {
|
||||
|
||||
case THROW:
|
||||
throw_run();
|
||||
break;
|
||||
|
||||
case GUIDED_NOGPS:
|
||||
guided_nogps_run();
|
||||
break;
|
||||
|
@ -312,7 +311,6 @@ bool Copter::mode_requires_GPS()
|
|||
}
|
||||
switch (control_mode) {
|
||||
case SMART_RTL:
|
||||
case THROW:
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
|
@ -369,9 +367,6 @@ void Copter::notify_flight_mode()
|
|||
|
||||
// set flight mode string
|
||||
switch (control_mode) {
|
||||
case THROW:
|
||||
notify.set_flight_mode_str("THRW");
|
||||
break;
|
||||
case GUIDED_NOGPS:
|
||||
notify.set_flight_mode_str("GNGP");
|
||||
break;
|
||||
|
|
Loading…
Reference in New Issue