Copter: add RTL to front of RTLState enum values
This removes the confusing Land enum value which also appears in the flight mode enum
This commit is contained in:
parent
c600c1a746
commit
09a98b89b7
@ -712,7 +712,7 @@ extern bool rtl_state_complete;
|
||||
// returns true with RTL has completed successfully
|
||||
static bool verify_RTL()
|
||||
{
|
||||
return (rtl_state_complete && (rtl_state == FinalDescent || rtl_state == Land));
|
||||
return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land));
|
||||
}
|
||||
|
||||
// verify_spline_wp - check if we have reached the next way point using spline
|
||||
|
@ -25,23 +25,23 @@ static void rtl_run()
|
||||
// check if we need to move to next state
|
||||
if (rtl_state_complete) {
|
||||
switch (rtl_state) {
|
||||
case InitialClimb:
|
||||
case RTL_InitialClimb:
|
||||
rtl_return_start();
|
||||
break;
|
||||
case ReturnHome:
|
||||
case RTL_ReturnHome:
|
||||
rtl_loiterathome_start();
|
||||
break;
|
||||
case LoiterAtHome:
|
||||
case RTL_LoiterAtHome:
|
||||
if (g.rtl_alt_final > 0 && !failsafe.radio) {
|
||||
rtl_descent_start();
|
||||
}else{
|
||||
rtl_land_start();
|
||||
}
|
||||
break;
|
||||
case FinalDescent:
|
||||
case RTL_FinalDescent:
|
||||
// do nothing
|
||||
break;
|
||||
case Land:
|
||||
case RTL_Land:
|
||||
// do nothing - rtl_land_run will take care of disarming motors
|
||||
break;
|
||||
}
|
||||
@ -50,23 +50,23 @@ static void rtl_run()
|
||||
// call the correct run function
|
||||
switch (rtl_state) {
|
||||
|
||||
case InitialClimb:
|
||||
case RTL_InitialClimb:
|
||||
rtl_climb_return_run();
|
||||
break;
|
||||
|
||||
case ReturnHome:
|
||||
case RTL_ReturnHome:
|
||||
rtl_climb_return_run();
|
||||
break;
|
||||
|
||||
case LoiterAtHome:
|
||||
case RTL_LoiterAtHome:
|
||||
rtl_loiterathome_run();
|
||||
break;
|
||||
|
||||
case FinalDescent:
|
||||
case RTL_FinalDescent:
|
||||
rtl_descent_run();
|
||||
break;
|
||||
|
||||
case Land:
|
||||
case RTL_Land:
|
||||
rtl_land_run();
|
||||
break;
|
||||
}
|
||||
@ -75,7 +75,7 @@ static void rtl_run()
|
||||
// rtl_climb_start - initialise climb to RTL altitude
|
||||
static void rtl_climb_start()
|
||||
{
|
||||
rtl_state = InitialClimb;
|
||||
rtl_state = RTL_InitialClimb;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// initialise waypoint and spline controller
|
||||
@ -106,7 +106,7 @@ static void rtl_climb_start()
|
||||
// rtl_return_start - initialise return to home
|
||||
static void rtl_return_start()
|
||||
{
|
||||
rtl_state = ReturnHome;
|
||||
rtl_state = RTL_ReturnHome;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// set target to above home/rally point
|
||||
@ -171,7 +171,7 @@ static void rtl_climb_return_run()
|
||||
// rtl_return_start - initialise return to home
|
||||
static void rtl_loiterathome_start()
|
||||
{
|
||||
rtl_state = LoiterAtHome;
|
||||
rtl_state = RTL_LoiterAtHome;
|
||||
rtl_state_complete = false;
|
||||
rtl_loiter_start_time = millis();
|
||||
|
||||
@ -237,7 +237,7 @@ static void rtl_loiterathome_run()
|
||||
// rtl_descent_start - initialise descent to final alt
|
||||
static void rtl_descent_start()
|
||||
{
|
||||
rtl_state = FinalDescent;
|
||||
rtl_state = RTL_FinalDescent;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// Set wp navigation target to above home
|
||||
@ -300,7 +300,7 @@ static void rtl_descent_run()
|
||||
// rtl_loiterathome_start - initialise controllers to loiter over home
|
||||
static void rtl_land_start()
|
||||
{
|
||||
rtl_state = Land;
|
||||
rtl_state = RTL_Land;
|
||||
rtl_state_complete = false;
|
||||
|
||||
// Set wp navigation target to above home
|
||||
|
@ -195,11 +195,11 @@ enum GuidedMode {
|
||||
|
||||
// RTL states
|
||||
enum RTLState {
|
||||
InitialClimb,
|
||||
ReturnHome,
|
||||
LoiterAtHome,
|
||||
FinalDescent,
|
||||
Land
|
||||
RTL_InitialClimb,
|
||||
RTL_ReturnHome,
|
||||
RTL_LoiterAtHome,
|
||||
RTL_FinalDescent,
|
||||
RTL_Land
|
||||
};
|
||||
|
||||
// Flip states
|
||||
|
@ -41,7 +41,7 @@ static int16_t get_pilot_desired_collective(int16_t control_in)
|
||||
static void check_dynamic_flight(void)
|
||||
{
|
||||
if (!motors.armed() || !motors.motor_runup_complete() ||
|
||||
control_mode == LAND || (control_mode==RTL && rtl_state == Land) || (control_mode == AUTO && auto_mode == Auto_Land)) {
|
||||
control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) {
|
||||
heli_dynamic_flight_counter = 0;
|
||||
heli_flags.dynamic_flight = false;
|
||||
return;
|
||||
@ -109,7 +109,7 @@ static void heli_update_landing_swash()
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
if (rtl_state == Land) {
|
||||
if (rtl_state == RTL_Land) {
|
||||
motors.set_collective_for_landing(true);
|
||||
}else{
|
||||
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
|
||||
|
@ -11,7 +11,7 @@ static void landinggear_update(){
|
||||
|
||||
// if we are doing an automatic landing procedure, force the landing gear to deploy.
|
||||
// To-Do: should we pause the auto-land procedure to give time for gear to come down?
|
||||
if (control_mode == LAND || (control_mode==RTL && rtl_state == Land) || (control_mode == AUTO && auto_mode == Auto_Land)){
|
||||
if (control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)){
|
||||
landinggear.force_deploy(true);
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user