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

Copter: add GCS failsafe

This commit is contained in:
Randy Mackay 2013-04-29 21:30:22 +09:00
parent 00146ca97d
commit bab9fa25e5
9 changed files with 139 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/AP_State.pde
View File

@ -91,6 +91,12 @@ static void set_failsafe_gps(bool mode)
ap.failsafe_gps = mode;
}
// ---------------------------------------------
static void set_failsafe_gcs(bool mode)
{
ap.failsafe_gcs = mode;
}
// ---------------------------------------------
void set_takeoff_complete(bool b)
{

19
ArduCopter/ArduCopter.pde
View File

@ -372,11 +372,13 @@ static union {
uint8_t failsafe_radio : 1; // 8 // A status flag for the radio failsafe
uint8_t failsafe_batt : 1; // 9 // A status flag for the battery failsafe
uint8_t failsafe_gps : 1; // 10 // A status flag for the gps failsafe
uint8_t do_flip : 1; // 11 // Used to enable flip code
uint8_t takeoff_complete : 1; // 12
uint8_t land_complete : 1; // 13
uint8_t compass_status : 1; // 14
uint8_t gps_status : 1; // 15
uint8_t failsafe_gcs : 1; // 11 // A status flag for the ground station failsafe
uint8_t rc_override_active : 1; // 12 // true if rc control are overwritten by ground station
uint8_t do_flip : 1; // 13 // Used to enable flip code
uint8_t takeoff_complete : 1; // 14
uint8_t land_complete : 1; // 15
uint8_t compass_status : 1; // 16
uint8_t gps_status : 1; // 17
};
uint16_t value;
} ap;
@ -750,6 +752,7 @@ static AC_WPNav wp_nav(&inertial_nav, &g.pi_loiter_lat, &g.pi_loiter_lon, &g.pid
////////////////////////////////////////////////////////////////////////////////
// The number of GPS fixes we have had
static uint8_t gps_fix_count;
static int16_t pmTest1;
// System Timers
// --------------
@ -771,6 +774,8 @@ static uint32_t rtl_loiter_start_time;
static uint8_t auto_disarming_counter;
// prevents duplicate GPS messages from entering system
static uint32_t last_gps_time;
// the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
static uint32_t last_heartbeat_ms;
// Used to exit the roll and pitch auto trim function
static uint8_t auto_trim_counter;
@ -923,6 +928,7 @@ static void perf_update(void)
}
perf_info_reset();
gps_fix_count = 0;
pmTest1 = 0;
}
void loop()
@ -1194,6 +1200,9 @@ static void slow_loop()
slow_loopCounter++;
superslow_loopCounter++;
// check if we've lost contact with the ground station
failsafe_gcs_check();
// record if the compass is healthy
set_compass_healthy(compass.healthy);

22
ArduCopter/GCS_Mavlink.pde
View File

@ -1867,6 +1867,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
v[6] = packet.chan7_raw;
v[7] = packet.chan8_raw;
hal.rcin->set_overrides(v, 8);
// record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
ap.rc_override_active = true;
// a RC override message is consiered to be a 'heartbeat' from the ground station for failsafe purposes
last_heartbeat_ms = millis();
break;
}
@ -1951,15 +1956,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
#endif // HIL_MODE != HIL_MODE_DISABLED
case MAVLINK_MSG_ID_HEARTBEAT:
{
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
if(msg->sysid != g.sysid_my_gcs) break;
last_heartbeat_ms = millis();
pmTest1++;
break;
}
/*
* case MAVLINK_MSG_ID_HEARTBEAT:
* {
* // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
* if(msg->sysid != g.sysid_my_gcs) break;
* rc_override_fs_timer = millis();
* break;
* }
*
* #if HIL_MODE != HIL_MODE_DISABLED
* // This is used both as a sensor and to pass the location
* // in HIL_ATTITUDE mode.

4
ArduCopter/Log.pde
View File

@ -375,6 +375,7 @@ struct PACKED log_Performance {
uint16_t num_long_running;
uint16_t num_loops;
uint32_t max_time;
int16_t pm_test;
uint8_t i2c_lockup_count;
};
@ -389,6 +390,7 @@ static void Log_Write_Performance()
num_long_running : perf_info_get_num_long_running(),
num_loops : perf_info_get_num_loops(),
max_time : perf_info_get_max_time(),
pm_test : pmTest1,
i2c_lockup_count : hal.i2c->lockup_count()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -790,7 +792,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
{ LOG_COMPASS_MSG, sizeof(log_Compass),
"MAG", "hhhhhhhhh", "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
{ LOG_PERFORMANCE_MSG, sizeof(log_Performance),
"PM", "BBBHHIB", "RenCnt,RenBlw,FixCnt,NLon,NLoop,MaxT,I2CErr" },
"PM", "BBBHHIhB", "RenCnt,RenBlw,FixCnt,NLon,NLoop,MaxT,PMT,I2CErr" },
{ LOG_CMD_MSG, sizeof(log_Cmd),
"CMD", "BBBBBeLL", "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
{ LOG_ATTITUDE_MSG, sizeof(log_Attitude),

4
ArduCopter/Parameters.h
View File

@ -194,9 +194,10 @@ public:
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled,
k_param_throttle_mid,
k_param_failsafe_gps_enabled, // 195
k_param_failsafe_gps_enabled,
k_param_rc_9,
k_param_rc_12,
k_param_failsafe_gcs, // 198
//
// 200: flight modes
@ -274,6 +275,7 @@ public:
AP_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Int8 failsafe_gps_enabled; // gps failsafe enabled
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int8 compass_enabled;
AP_Int8 optflow_enabled;

7
ArduCopter/Parameters.pde
View File

@ -106,6 +106,13 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard
GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS),
// @Param: FS_GCS_ENABLE
// @DisplayName: Ground Station Failsafe Enable
// @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds
// @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode
// @User: Standard
GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),
// @Param: VOLT_DIVIDER
// @DisplayName: Voltage Divider
// @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin voltage * INPUT_VOLTS/1024 * VOLT_DIVIDER)

12
ArduCopter/config.h
View File

@ -396,6 +396,18 @@
# define FAILSAFE_GPS_TIMEOUT_MS 5000 // gps failsafe triggers after 5 seconds with no GPS
#endif
// GCS failsafe
#ifndef FS_GCS
# define FS_GCS DISABLED
#endif
#ifndef FS_GCS_TIMEOUT_MS
# define FS_GCS_TIMEOUT_MS 5000 // gcs failsafe triggers after 5 seconds with no GCS heartbeat
#endif
// possible values for FS_GCS parameter
#define FS_GCS_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1
#define FS_GCS_ENABLED_CONTINUE_MISSION 2
//////////////////////////////////////////////////////////////////////////////
// MAGNETOMETER
#ifndef MAGNETOMETER

3
ArduCopter/defines.h
View File

@ -435,7 +435,8 @@ enum gcs_severity {
#define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5
#define ERROR_SUBSYSTEM_FAILSAFE_BATT 6
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 8
#define ERROR_SUBSYSTEM_FAILSAFE_GCS 8
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1

78
ArduCopter/events.pde
View File

@ -171,6 +171,84 @@ static void failsafe_gps_off_event(void)
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_RESOLVED);
}
// failsafe_gcs_check - check for ground station failsafe
static void failsafe_gcs_check()
{
uint32_t last_gcs_update_ms;
// return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs
if( g.failsafe_gcs == FS_GCS_DISABLED || last_heartbeat_ms == 0 || !ap.rc_override_active) {
return;
}
// calc time since last gps update
last_gcs_update_ms = millis() - last_heartbeat_ms;
// check if all is well
if( last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
// check for recovery from gps failsafe
if( ap.failsafe_gcs ) {
failsafe_gcs_off_event();
set_failsafe_gcs(false);
}
return;
}
// do nothing if gps failsafe already triggered or motors disarmed
if( ap.failsafe_gcs || !motors.armed()) {
return;
}
// GCS failsafe event has occured
// update state, log to dataflash
set_failsafe_gcs(true);
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
// This is how to handle a failsafe.
// use the throttle failsafe setting to decide what to do
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
}else if(ap.home_is_set == true && home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
}
break;
case AUTO:
// if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
if(home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
}else{
// We are very close to home so we will land
set_mode(LAND);
}
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
if(ap.home_is_set == true && home_distance > wp_nav.get_waypoint_radius()) {
set_mode(RTL);
}else{
// We have no GPS or are very close to home so we will land
set_mode(LAND);
}
break;
}
}
// failsafe_gps_off_event - actions to take when GPS contact is restored
static void failsafe_gcs_off_event(void)
{
// log recovery of GPS in logs?
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
}
static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
if(event_repeat == 0 || (millis() - event_timer) < event_delay)