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

Rover: update advanced failsafe

Browse Source
This commit is contained in:
Randy Mackay 2017-07-19 13:34:04 +09:00
parent 707b006f76
commit e7424a456f
2 changed files with 6 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
APMrover2/afs_rover.cpp
View File

@ -17,28 +17,11 @@ AP_AdvancedFailsafe_Rover::AP_AdvancedFailsafe_Rover(AP_Mission &_mission, AP_Ba
*/ */
void AP_AdvancedFailsafe_Rover::terminate_vehicle(void) void AP_AdvancedFailsafe_Rover::terminate_vehicle(void)
{ {
// stop motors
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_ZERO_PWM);
rover.lateral_acceleration = 0;
// disarm as well // disarm as well
rover.disarm_motors(); rover.disarm_motors();
// Set to HOLD mode // Set to HOLD mode
rover.set_mode(HOLD); rover.set_mode(rover.mode_hold);
// and set all aux channels
SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::output_ch_all();
}
void AP_AdvancedFailsafe_Rover::setup_IO_failsafe(void)
{
// setup failsafe for all aux channels
SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
} }
/* /*
@ -46,13 +29,8 @@ void AP_AdvancedFailsafe_Rover::setup_IO_failsafe(void)
*/ */
AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void) AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void)
{ {
switch (rover.control_mode) { if (rover.control_mode->is_autopilot_mode()) {
case AUTO:
case GUIDED:
case RTL:
return AP_AdvancedFailsafe::AFS_AUTO; return AP_AdvancedFailsafe::AFS_AUTO;
default:
break;
} }
return AP_AdvancedFailsafe::AFS_STABILIZED; return AP_AdvancedFailsafe::AFS_STABILIZED;
} }

8
APMrover2/afs_rover.h
View File

@ -30,14 +30,14 @@ public:
AP_AdvancedFailsafe_Rover(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap); AP_AdvancedFailsafe_Rover(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap);
// called to set all outputs to termination state // called to set all outputs to termination state
void terminate_vehicle(void); void terminate_vehicle(void) override;
protected: protected:
// setup failsafe values for if FMU firmware stops running // setup failsafe values - this is handled by motors library
void setup_IO_failsafe(void); void setup_IO_failsafe(void) override {}
// return the AFS mapped control mode // return the AFS mapped control mode
enum control_mode afs_mode(void); enum control_mode afs_mode(void) override;
}; };
#endif // ADVANCED_FAILSAFE #endif // ADVANCED_FAILSAFE