Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: update advanced failsafe

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)
{
// 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
rover.disarm_motors();
// Set to HOLD mode
rover.set_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);
rover.set_mode(rover.mode_hold);
}
/*
@ -46,13 +29,8 @@ void AP_AdvancedFailsafe_Rover::setup_IO_failsafe(void)
*/
AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void)
{
switch (rover.control_mode) {
case AUTO:
case GUIDED:
case RTL:
if (rover.control_mode->is_autopilot_mode()) {
return AP_AdvancedFailsafe::AFS_AUTO;
default:
break;
}
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);
// called to set all outputs to termination state
void terminate_vehicle(void);
void terminate_vehicle(void) override;
protected:
// setup failsafe values for if FMU firmware stops running
void setup_IO_failsafe(void);
// setup failsafe values - this is handled by motors library
void setup_IO_failsafe(void) override {}
// return the AFS mapped control mode
enum control_mode afs_mode(void);
enum control_mode afs_mode(void) override;
};
#endif // ADVANCED_FAILSAFE