Browse Source

Copter: make private enum class for LAND mode's state

c415-sdk
Peter Barker 5 years ago committed by Randy Mackay
parent
commit
2296448027
  1. 5
      ArduCopter/defines.h
  2. 6
      ArduCopter/mode.h
  3. 12
      ArduCopter/mode_auto.cpp

5
ArduCopter/defines.h

@ -125,11 +125,6 @@ enum SmartRTLState {
SmartRTL_Land SmartRTL_Land
}; };
enum LandStateType {
LandStateType_FlyToLocation = 0,
LandStateType_Descending = 1
};
enum PayloadPlaceStateType { enum PayloadPlaceStateType {
PayloadPlaceStateType_FlyToLocation, PayloadPlaceStateType_FlyToLocation,
PayloadPlaceStateType_Calibrating_Hover_Start, PayloadPlaceStateType_Calibrating_Hover_Start,

6
ArduCopter/mode.h

@ -484,7 +484,11 @@ private:
int32_t condition_value; // used in condition commands (eg delay, change alt, etc.) int32_t condition_value; // used in condition commands (eg delay, change alt, etc.)
uint32_t condition_start; uint32_t condition_start;
LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending) enum class State {
FlyToLocation = 0,
Descending = 1
};
State state = State::FlyToLocation;
struct { struct {
PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...) PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...)

12
ArduCopter/mode_auto.cpp

@ -1166,14 +1166,14 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
// if location provided we fly to that location at current altitude // if location provided we fly to that location at current altitude
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) { if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
// set state to fly to location // set state to fly to location
land_state = LandStateType_FlyToLocation; state = State::FlyToLocation;
const Location target_loc = terrain_adjusted_location(cmd); const Location target_loc = terrain_adjusted_location(cmd);
wp_start(target_loc); wp_start(target_loc);
} else { } else {
// set landing state // set landing state
land_state = LandStateType_Descending; state = State::Descending;
// initialise landing controller // initialise landing controller
land_start(); land_start();
@ -1519,8 +1519,8 @@ bool ModeAuto::verify_land()
{ {
bool retval = false; bool retval = false;
switch (land_state) { switch (state) {
case LandStateType_FlyToLocation: case State::FlyToLocation:
// check if we've reached the location // check if we've reached the location
if (copter.wp_nav->reached_wp_destination()) { if (copter.wp_nav->reached_wp_destination()) {
// get destination so we can use it for loiter target // get destination so we can use it for loiter target
@ -1530,11 +1530,11 @@ bool ModeAuto::verify_land()
land_start(dest); land_start(dest);
// advance to next state // advance to next state
land_state = LandStateType_Descending; state = State::Descending;
} }
break; break;
case LandStateType_Descending: case State::Descending:
// rely on THROTTLE_LAND mode to correctly update landing status // rely on THROTTLE_LAND mode to correctly update landing status
retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE); retval = copter.ap.land_complete && (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE);
break; break;

Loading…
Cancel
Save