aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: adjust for desired spool state and spool state renames

Browse Source
This commit is contained in:
Peter Barker 2019-04-09 22:16:58 +10:00 committed by Randy Mackay
parent 1e606cdc5b
commit 46a6f45e4a
29 changed files with 145 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/afs_copter.cpp
View File

@ -21,7 +21,7 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
copter.set_mode(LAND, MODE_REASON_TERMINATE);
} else {
// stop motors
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
copter.motors->output();
// disarm as well

2
ArduCopter/heli.cpp
View File

@ -23,7 +23,7 @@ void Copter::heli_init()
// should be called at 50hz
void Copter::check_dynamic_flight(void)
{
if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED ||
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED ||
control_mode == LAND || (control_mode==RTL && mode_rtl.state() == RTL_Land) || (control_mode == AUTO && mode_auto.mode() == Auto_Land)) {
heli_dynamic_flight_counter = 0;
heli_flags.dynamic_flight = false;

2
ArduCopter/land_detector.cpp
View File

@ -47,7 +47,7 @@ void Copter::update_land_detector()
} else if (ap.land_complete) {
#if FRAME_CONFIG == HELI_FRAME
// if rotor speed and collective pitch are high then clear landing flag
if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
#else
// if throttle output is high then clear landing flag
if (motors->get_throttle() > get_non_takeoff_throttle()) {

36
ArduCopter/mode.cpp
View File

@ -193,7 +193,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
#if FRAME_CONFIG == HELI_FRAME
// do not allow helis to enter a non-manual throttle mode if the
// rotor runup is not complete
if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_mode() == AP_Motors::SPOOL_UP || motors->get_spool_mode() == AP_Motors::SPOOL_DOWN)) {
if (!ignore_checks && !new_flightmode->has_manual_throttle() && (motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_UP || motors->get_spool_state() == AP_Motors::SpoolState::SPOOLING_DOWN)) {
gcs().send_text(MAV_SEVERITY_WARNING,"Flight mode change failed");
AP::logger().Write_Error(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(mode));
return false;
@ -382,7 +382,7 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const
return false;
}
if (copter.motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED) {
if (copter.motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
// hold aircraft on the ground until rotor speed runup has finished
return false;
}
@ -393,9 +393,9 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const
void Copter::Mode::zero_throttle_and_relax_ac(bool spool_up)
{
if (spool_up) {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
}
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt);
@ -411,17 +411,19 @@ void Copter::Mode::zero_throttle_and_hold_attitude()
void Copter::Mode::make_safe_spool_down()
{
// command aircraft to initiate the shutdown process
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
switch (motors->get_spool_mode()) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
switch (motors->get_spool_state()) {
case AP_Motors::SHUT_DOWN:
case AP_Motors::GROUND_IDLE:
case AP_Motors::SpoolState::SHUT_DOWN:
case AP_Motors::SpoolState::GROUND_IDLE:
// relax controllers during idle states
attitude_control->reset_rate_controller_I_terms();
attitude_control->set_yaw_target_to_current_heading();
break;
default:
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// while transitioning though active states continue to operate normally
break;
}
@ -627,15 +629,15 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli
// Alt Hold State Machine Determination
if (!motors->armed()) {
// the aircraft should moved to a shut down state
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
// transition through states as aircraft spools down
switch (motors->get_spool_mode()) {
switch (motors->get_spool_state()) {
case AP_Motors::SHUT_DOWN:
case AP_Motors::SpoolState::SHUT_DOWN:
return AltHold_MotorStopped;
case AP_Motors::DESIRED_GROUND_IDLE:
case AP_Motors::SpoolState::GROUND_IDLE:
return AltHold_Landed_Ground_Idle;
default:
@ -651,14 +653,14 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli
// the aircraft is armed and landed
if (target_climb_rate_cms < 0.0f && !ap.using_interlock) {
// the aircraft should move to a ground idle state
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
// the aircraft should prepare for imminent take off
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
if (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
// the aircraft is waiting in ground idle
return AltHold_Landed_Ground_Idle;
@ -669,7 +671,7 @@ Copter::Mode::AltHoldModeState Copter::Mode::get_alt_hold_state(float target_cli
} else {
// the aircraft is in a flying state
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
return AltHold_Flying;
}
}

20
ArduCopter/mode_acro.cpp
View File

@ -16,27 +16,35 @@ void Copter::ModeAcro::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (ap.throttle_zero) {
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}
// run attitude controller

18
ArduCopter/mode_acro_heli.cpp
View File

@ -36,27 +36,35 @@ void Copter::ModeAcro_Heli::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else {
// heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
if (motors->init_targets_on_arming()) {
attitude_control->set_attitude_target_to_current_attitude();
attitude_control->reset_rate_controller_I_terms();
}
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}
if (!motors->has_flybar()){

2
ArduCopter/mode_althold.cpp
View File

@ -88,7 +88,7 @@ void Copter::ModeAltHold::run()
break;
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance

14
ArduCopter/mode_auto.cpp
View File

@ -747,7 +747,7 @@ void Copter::ModeAuto::wp_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
@ -787,7 +787,7 @@ void Copter::ModeAuto::spline_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
wp_nav->update_spline();
@ -818,7 +818,7 @@ void Copter::ModeAuto::land_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
land_run_horizontal_control();
land_run_vertical_control();
@ -879,7 +879,7 @@ void Copter::ModeAuto::loiter_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint and z-axis position controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
@ -972,7 +972,7 @@ void Copter::ModeAuto::payload_place_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
@ -1509,7 +1509,7 @@ bool Copter::ModeAuto::verify_land()
case LandStateType_Descending:
// rely on THROTTLE_LAND mode to correctly update landing status
retval = ap.land_complete && (motors->get_spool_mode() == AP_Motors::GROUND_IDLE);
retval = ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE);
break;
default:
@ -1729,7 +1729,7 @@ bool Copter::ModeAuto::verify_RTL()
{
return (copter.mode_rtl.state_complete() &&
(copter.mode_rtl.state() == RTL_FinalDescent || copter.mode_rtl.state() == RTL_Land) &&
(motors->get_spool_mode() == AP_Motors::GROUND_IDLE));
(motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE));
}
/********************************************************************************/

4
ArduCopter/mode_autotune.cpp
View File

@ -54,9 +54,9 @@ void Copter::AutoTune::run()
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
if (target_climb_rate < 0.0f) {
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
copter.attitude_control->reset_rate_controller_I_terms();
copter.attitude_control->set_yaw_target_to_current_heading();

4
ArduCopter/mode_brake.cpp
View File

@ -39,7 +39,7 @@ void Copter::ModeBrake::run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// relax stop target if we might be landed
if (ap.land_complete_maybe) {
@ -54,7 +54,7 @@ void Copter::ModeBrake::run()
// update altitude target and call position controller
// protects heli's from inflight motor interlock disable
if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) {
if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) {
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
} else {
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);

4
ArduCopter/mode_circle.cpp
View File

@ -54,7 +54,7 @@ void Copter::ModeCircle::run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run circle controller
copter.circle_nav->update();
@ -72,7 +72,7 @@ void Copter::ModeCircle::run()
// update altitude target and call position controller
// protects heli's from inflight motor interlock disable
if (motors->get_desired_spool_state() == AP_Motors::DESIRED_GROUND_IDLE && !ap.land_complete) {
if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !ap.land_complete) {
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
} else {
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);

20
ArduCopter/mode_drift.cpp
View File

@ -80,27 +80,35 @@ void Copter::ModeDrift::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (ap.throttle_zero) {
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}
// call attitude controller

2
ArduCopter/mode_flip.cpp
View File

@ -103,7 +103,7 @@ void Copter::ModeFlip::run()
float throttle_out = get_pilot_desired_throttle();
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// get corrected angle based on direction and axis of rotation
// we flip the sign of flip_angle to minimize the code repetition

8
ArduCopter/mode_flowhold.cpp
View File

@ -255,7 +255,7 @@ void Copter::ModeFlowHold::run()
case AltHold_MotorStopped:
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
copter.attitude_control->reset_rate_controller_I_terms();
copter.attitude_control->set_yaw_target_to_current_heading();
copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
@ -264,7 +264,7 @@ void Copter::ModeFlowHold::run()
case AltHold_Takeoff:
// set motors to full range
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// initiate take-off
if (!takeoff.running()) {
@ -297,7 +297,7 @@ void Copter::ModeFlowHold::run()
break;
case AltHold_Flying:
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder
target_climb_rate = copter.get_surface_tracking_climb_rate(target_climb_rate, copter.pos_control->get_alt_target(), copter.G_Dt);
@ -353,7 +353,7 @@ void Copter::ModeFlowHold::update_height_estimate(void)
#if 1
// assume on ground when disarmed, or if we have only just started spooling the motors up
if (!hal.util->get_soft_armed() ||
copter.motors->get_desired_spool_state() != AP_Motors::DESIRED_THROTTLE_UNLIMITED ||
copter.motors->get_desired_spool_state() != AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED ||
AP_HAL::millis() - copter.arm_time_ms < 1500) {
height_offset = -ins_height;
last_ins_height = ins_height;

2
ArduCopter/mode_follow.cpp
View File

@ -33,7 +33,7 @@ void Copter::ModeFollow::run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// re-use guided mode's velocity controller
// Note: this is safe from interference from GCSs and companion computer's whose guided mode

16
ArduCopter/mode_guided.cpp
View File

@ -392,14 +392,14 @@ void Copter::Mode::auto_takeoff_run()
}
// aircraft stays in landed state until rotor speed runup has finished
if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
set_land_complete(false);
} else {
wp_nav->shift_wp_origin_to_current_pos();
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
@ -432,7 +432,7 @@ void Copter::ModeGuided::pos_control_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
@ -474,7 +474,7 @@ void Copter::ModeGuided::vel_control_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis();
@ -527,7 +527,7 @@ void Copter::ModeGuided::posvel_control_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set velocity to zero and stop rotating if no updates received for 3 seconds
uint32_t tnow = millis();
@ -614,8 +614,8 @@ void Copter::ModeGuided::angle_control_run()
// landed with positive desired climb rate, takeoff
if (ap.land_complete && (guided_angle_state.climb_rate_cms > 0.0f)) {
zero_throttle_and_relax_ac();
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
set_land_complete(false);
set_throttle_takeoff();
}
@ -623,7 +623,7 @@ void Copter::ModeGuided::angle_control_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// call attitude controller
if (guided_angle_state.use_yaw_rate) {

8
ArduCopter/mode_land.cpp
View File

@ -54,7 +54,7 @@ void Copter::ModeLand::run()
void Copter::ModeLand::gps_run()
{
// disarm when the landing detector says we've landed
if (ap.land_complete && motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.init_disarm_motors();
}
@ -64,7 +64,7 @@ void Copter::ModeLand::gps_run()
loiter_nav->init_target();
} else {
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
@ -105,7 +105,7 @@ void Copter::ModeLand::nogps_run()
}
// disarm when the landing detector says we've landed
if (ap.land_complete && motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.init_disarm_motors();
}
@ -114,7 +114,7 @@ void Copter::ModeLand::nogps_run()
make_safe_spool_down();
} else {
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// pause before beginning land descent
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {

2
ArduCopter/mode_loiter.cpp
View File

@ -172,7 +172,7 @@ void Copter::ModeLoiter::run()
case AltHold_Flying:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
#if PRECISION_LANDING == ENABLED
if (do_precision_loiter()) {

2
ArduCopter/mode_poshold.cpp
View File

@ -224,7 +224,7 @@ void Copter::ModePosHold::run()
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
#if AC_AVOID_ENABLED == ENABLED
// apply avoidance

10
ArduCopter/mode_rtl.cpp
View File

@ -144,7 +144,7 @@ void Copter::ModeRTL::climb_return_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
@ -201,7 +201,7 @@ void Copter::ModeRTL::loiterathome_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
@ -293,7 +293,7 @@ void Copter::ModeRTL::descent_run()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// process roll, pitch inputs
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
@ -357,7 +357,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
_state_complete = ap.land_complete;
// disarm when the landing detector says we've landed
if (disarm_on_land && ap.land_complete && motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
if (disarm_on_land && ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.init_disarm_motors();
}
@ -369,7 +369,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
land_run_horizontal_control();
land_run_vertical_control();

6
ArduCopter/mode_smart_rtl.cpp
View File

@ -62,7 +62,7 @@ void Copter::ModeSmartRTL::run()
void Copter::ModeSmartRTL::wait_cleanup_run()
{
// hover at current target position
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
@ -105,7 +105,7 @@ void Copter::ModeSmartRTL::path_follow_run()
}
// update controllers
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
@ -135,7 +135,7 @@ void Copter::ModeSmartRTL::pre_land_position_run()
}
// update controllers
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
wp_nav->update_wpnav();
pos_control->update_z_controller();
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true);

2
ArduCopter/mode_sport.cpp
View File

@ -118,7 +118,7 @@ void Copter::ModeSport::run()
break;
case AltHold_Flying:
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// adjust climb rate using rangefinder
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);

20
ArduCopter/mode_stabilize.cpp
View File

@ -20,27 +20,35 @@ void Copter::ModeStabilize::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else if (ap.throttle_zero) {
// Attempting to Land
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}
// call attitude controller

17
ArduCopter/mode_stabilize_heli.cpp
View File

@ -41,27 +41,34 @@ void Copter::ModeStabilize_Heli::run()
if (!motors->armed()) {
// Motors should be Stopped
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
} else {
// heli will not let the spool state progress to THROTTLE_UNLIMITED until motor interlock is enabled
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
if (motors->get_spool_mode() == AP_Motors::SHUT_DOWN) {
switch (motors->get_spool_state()) {
case AP_Motors::SpoolState::SHUT_DOWN:
// Motors Stopped
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
} else if (motors->get_spool_mode() == AP_Motors::GROUND_IDLE) {
break;
case AP_Motors::SpoolState::GROUND_IDLE:
// Landed
if (motors->init_targets_on_arming()) {
attitude_control->set_yaw_target_to_current_heading();
attitude_control->reset_rate_controller_I_terms();
}
} else if (motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
break;
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// clear landing flag above zero throttle
if (!motors->limit.throttle_lower) {
set_land_complete(false);
}
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// do nothing
break;
}
// call attitude controller

14
ArduCopter/mode_throw.cpp
View File

@ -109,9 +109,9 @@ void Copter::ModeThrow::run()
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
}
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
@ -124,9 +124,9 @@ void Copter::ModeThrow::run()
// prevent motors from rotating before the throw is detected unless enabled by the user
if (g.throw_motor_start == 1) {
motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
} else {
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
}
// Hold throttle at zero during the throw and continually reset the attitude controller
@ -142,7 +142,7 @@ void Copter::ModeThrow::run()
case Throw_Uprighting:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// demand a level roll/pitch attitude with zero yaw rate
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
@ -155,7 +155,7 @@ void Copter::ModeThrow::run()
case Throw_HgtStabilise:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f);
@ -169,7 +169,7 @@ void Copter::ModeThrow::run()
case Throw_PosHold:
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run loiter controller
loiter_nav->update();

4
ArduCopter/mode_zigzag.cpp
View File

@ -139,7 +139,7 @@ void Copter::ModeZigZag::auto_control()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run waypoint controller
copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
@ -183,7 +183,7 @@ void Copter::ModeZigZag::manual_control()
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// run loiter controller
loiter_nav->update();

2
ArduCopter/motors.cpp
View File

@ -94,7 +94,7 @@ void Copter::auto_disarm_check()
}
// if the rotor is still spinning, don't initiate auto disarm
if (motors->get_spool_mode() != AP_Motors::GROUND_IDLE) {
if (motors->get_spool_state() != AP_Motors::SpoolState::GROUND_IDLE) {
auto_disarm_begin = tnow_ms;
return;
}

4
ArduCopter/system.cpp
View File

@ -370,7 +370,7 @@ void Copter::update_auto_armed()
}
// if helicopters are on the ground, and the motor is switched off, auto-armed should be false
// so that rotor runup is checked again before attempting to take-off
if(ap.land_complete && motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) {
if(ap.land_complete && motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) {
set_auto_armed(false);
}
}else{
@ -378,7 +378,7 @@ void Copter::update_auto_armed()
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
if(motors->armed() && ap.using_interlock) {
if(!ap.throttle_zero && motors->get_spool_mode() == AP_Motors::THROTTLE_UNLIMITED) {
if(!ap.throttle_zero && motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) {
set_auto_armed(true);
}
// if motors are armed and throttle is above zero auto_armed should be true

2
ArduCopter/takeoff.cpp
View File

@ -33,7 +33,7 @@ bool Copter::Mode::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
}
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
if (motors->get_spool_mode() != AP_Motors::THROTTLE_UNLIMITED && ap.using_interlock) {
if (motors->get_spool_state() != AP_Motors::SpoolState::THROTTLE_UNLIMITED && ap.using_interlock) {
return false;
}