Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: wait for motors to spool up before trying to upright vehicle 

This stops us from progressing through the whole throw mode if the
vehicle just happens to be in the right state - which is can be for a
drop.
This commit is contained in:
Peter Barker 2021-07-02 16:37:03 +10:00 committed by Andrew Tridgell
parent 952c8f7ff5
commit ce408ca58b
2 changed files with 30 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/mode.h
View File

@ -1468,11 +1468,13 @@ private:
bool throw_position_good() const;
bool throw_height_good() const;
bool throw_attitude_good() const;
bool throttle_is_unlimited() const;
// Throw stages
enum ThrowModeStage {
Throw_Disarmed,
Throw_Detecting,
Throw_Wait_Throttle_Unlimited,
Throw_Uprighting,
Throw_HgtStabilise,
Throw_PosHold

30
ArduCopter/mode_throw.cpp
View File

@ -51,12 +51,15 @@ void ModeThrow::run()
stage = Throw_Detecting;
} else if (stage == Throw_Detecting && throw_detected()){
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - uprighting");
stage = Throw_Uprighting;
gcs().send_text(MAV_SEVERITY_INFO,"throw detected - spooling motors");
stage = Throw_Wait_Throttle_Unlimited;
// Cancel the waiting for throw tone sequence
AP_Notify::flags.waiting_for_throw = false;
} else if (stage == Throw_Wait_Throttle_Unlimited && throttle_is_unlimited()) {
gcs().send_text(MAV_SEVERITY_INFO,"throttle is unlimited - uprighting");
stage = Throw_Uprighting;
} else if (stage == Throw_Uprighting && throw_attitude_good()) {
gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height");
stage = Throw_HgtStabilise;
@ -140,6 +143,13 @@ void ModeThrow::run()
break;
case Throw_Wait_Throttle_Unlimited:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
break;
case Throw_Uprighting:
// set motors to full range
@ -294,4 +304,20 @@ bool ModeThrow::throw_position_good() const
// check that our horizontal position error is within 50cm
return (pos_control->get_pos_error_xy_cm() < 50.0f);
}
bool ModeThrow::throttle_is_unlimited() const
{
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
case AP_Motors::SpoolState::GROUND_IDLE:
case AP_Motors::SpoolState::SPOOLING_UP:
return false;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
return true;
case AP_Motors::SpoolState::SPOOLING_DOWN:
return false;
}
// compiler ensures we never get here
return true;
}
#endif