Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add and use new afs_mode mode method

Browse Source
This commit is contained in:
Iampete1 2024-09-15 16:09:08 +01:00 committed by Randy Mackay
parent 1ab778cc6f
commit 20cd9e523c
2 changed files with 30 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
ArduCopter/afs_copter.cpp
View File

@ -55,17 +55,7 @@ void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)
*/ */
AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void) AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Copter::afs_mode(void)
{ {
switch (copter.flightmode->mode_number()) { return copter.flightmode->afs_mode();
case Mode::Number::AUTO:
case Mode::Number::AUTO_RTL:
case Mode::Number::GUIDED:
case Mode::Number::RTL:
case Mode::Number::LAND:
return AP_AdvancedFailsafe::AFS_AUTO;
default:
break;
}
return AP_AdvancedFailsafe::AFS_STABILIZED;
} }
//to force entering auto mode when datalink loss //to force entering auto mode when datalink loss

29
ArduCopter/mode.h
View File

@ -4,6 +4,10 @@
#include <AP_Math/chirp.h> #include <AP_Math/chirp.h>
#include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this #include <AP_ExternalControl/AP_ExternalControl_config.h> // TODO why is this needed if Copter.h includes this
#if ADVANCED_FAILSAFE
#include "afs_copter.h"
#endif
class Parameters; class Parameters;
class ParametersG2; class ParametersG2;
@ -130,6 +134,11 @@ public:
virtual bool allows_flip() const { return false; } virtual bool allows_flip() const { return false; }
virtual bool crash_check_enabled() const { return true; } virtual bool crash_check_enabled() const { return true; }
#if ADVANCED_FAILSAFE
// Return the type of this mode for use by advanced failsafe
virtual AP_AdvancedFailsafe_Copter::control_mode afs_mode() const { return AP_AdvancedFailsafe_Copter::control_mode::AFS_STABILIZED; }
#endif
#if FRAME_CONFIG == HELI_FRAME #if FRAME_CONFIG == HELI_FRAME
virtual bool allows_inverted() const { return false; }; virtual bool allows_inverted() const { return false; };
#endif #endif
@ -509,6 +518,11 @@ public:
bool allows_inverted() const override { return true; }; bool allows_inverted() const override { return true; };
#endif #endif
#if ADVANCED_FAILSAFE
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif
// Auto modes // Auto modes
enum class SubMode : uint8_t { enum class SubMode : uint8_t {
TAKEOFF, TAKEOFF,
@ -1047,6 +1061,11 @@ public:
bool requires_terrain_failsafe() const override { return true; } bool requires_terrain_failsafe() const override { return true; }
#if ADVANCED_FAILSAFE
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif
// Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option) // Sets guided's angular target submode: Using a rotation quaternion, angular velocity, and climbrate or thrust (depends on user option)
// attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes // attitude_quat: IF zero: ang_vel (angular velocity) must be provided even if all zeroes
// IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity // IF non-zero: attitude_control is performed using both the attitude quaternion and angular velocity
@ -1214,6 +1233,11 @@ public:
bool is_landing() const override { return true; }; bool is_landing() const override { return true; };
#if ADVANCED_FAILSAFE
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif
void do_not_use_GPS(); void do_not_use_GPS();
// returns true if LAND mode is trying to control X/Y position // returns true if LAND mode is trying to control X/Y position
@ -1391,6 +1415,11 @@ public:
bool requires_terrain_failsafe() const override { return true; } bool requires_terrain_failsafe() const override { return true; }
#if ADVANCED_FAILSAFE
// Return the type of this mode for use by advanced failsafe
AP_AdvancedFailsafe_Copter::control_mode afs_mode() const override { return AP_AdvancedFailsafe_Copter::control_mode::AFS_AUTO; }
#endif
// for reporting to GCS // for reporting to GCS
bool get_wp(Location &loc) const override; bool get_wp(Location &loc) const override;