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

APM_OBC: update for AP_Mission and AP_GPS changes

This commit is contained in:
Andrew Tridgell 2014-04-07 07:28:21 +10:00
parent 43ebd86bb1
commit 6f55dc1686
2 changed files with 11 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/APM_OBC/APM_OBC.cpp
View File

@ -74,15 +74,12 @@ const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
// access to geofence state // access to geofence state
extern bool geofence_breached(void); extern bool geofence_breached(void);
// function to change waypoint
extern void change_command(uint8_t cmd_index);
// check for Failsafe conditions. This is called at 10Hz by the main // check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code // ArduPlane code
void void
APM_OBC::check(APM_OBC::control_mode mode, APM_OBC::check(APM_OBC::control_mode mode,
uint32_t last_heartbeat_ms, uint32_t last_heartbeat_ms,
uint32_t last_gps_fix_ms) AP_GPS &gps, AP_Mission &mission)
{ {
// we always check for fence breach // we always check for fence breach
if (geofence_breached()) { if (geofence_breached()) {
@ -105,7 +102,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000); bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
bool gps_lock_ok = ((now - last_gps_fix_ms) < 3000); bool gps_lock_ok = ((now - gps.last_fix_time_ms()) < 3000);
switch (_state) { switch (_state) {
case STATE_PREFLIGHT: case STATE_PREFLIGHT:
@ -123,10 +120,8 @@ APM_OBC::check(APM_OBC::control_mode mode,
hal.console->println_P(PSTR("State DATA_LINK_LOSS")); hal.console->println_P(PSTR("State DATA_LINK_LOSS"));
_state = STATE_DATA_LINK_LOSS; _state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) { if (_wp_comms_hold) {
if (_command_index != NULL) { _saved_wp = mission.get_current_nav_cmd().index;
_saved_wp = _command_index->get(); mission.set_current_cmd(_wp_comms_hold);
}
change_command(_wp_comms_hold);
} }
break; break;
} }
@ -134,10 +129,8 @@ APM_OBC::check(APM_OBC::control_mode mode,
hal.console->println_P(PSTR("State GPS_LOSS")); hal.console->println_P(PSTR("State GPS_LOSS"));
_state = STATE_GPS_LOSS; _state = STATE_GPS_LOSS;
if (_wp_gps_loss) { if (_wp_gps_loss) {
if (_command_index != NULL) { _saved_wp = mission.get_current_nav_cmd().index;
_saved_wp = _command_index->get(); mission.set_current_cmd(_wp_gps_loss);
}
change_command(_wp_gps_loss);
} }
break; break;
} }
@ -153,7 +146,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
_state = STATE_AUTO; _state = STATE_AUTO;
hal.console->println_P(PSTR("GCS OK")); hal.console->println_P(PSTR("GCS OK"));
if (_saved_wp != 0) { if (_saved_wp != 0) {
change_command(_saved_wp); mission.set_current_cmd(_saved_wp);
_saved_wp = 0; _saved_wp = 0;
} }
} }
@ -169,7 +162,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
hal.console->println_P(PSTR("GPS OK")); hal.console->println_P(PSTR("GPS OK"));
_state = STATE_AUTO; _state = STATE_AUTO;
if (_saved_wp != 0) { if (_saved_wp != 0) {
change_command(_saved_wp); mission.set_current_cmd(_saved_wp);
_saved_wp = 0; _saved_wp = 0;
} }
} }

11
libraries/APM_OBC/APM_OBC.h
View File

@ -22,6 +22,8 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Param.h> #include <AP_Param.h>
#include <AP_Mission.h>
#include <AP_GPS.h>
#include <inttypes.h> #include <inttypes.h>
@ -51,16 +53,12 @@ public:
_state = STATE_PREFLIGHT; _state = STATE_PREFLIGHT;
_terminate.set(0); _terminate.set(0);
// get a pointer to COMMAND_INDEX so we can resume a
// auto mission when a failsafe condition is resolved
enum ap_var_type var_type;
_command_index = (AP_Int8 *)AP_Param::find("CMD_INDEX", &var_type);
_saved_wp = 0; _saved_wp = 0;
} }
void check(enum control_mode control_mode, void check(enum control_mode control_mode,
uint32_t last_heartbeat_ms, uint32_t last_heartbeat_ms,
uint32_t last_gps_fix_ms); AP_GPS &gps, AP_Mission &mission);
// should we crash the plane? Only possible with // should we crash the plane? Only possible with
// FS_TERM_ACTTION set to 43 // FS_TERM_ACTTION set to 43
@ -92,9 +90,6 @@ private:
bool _heartbeat_pin_value; bool _heartbeat_pin_value;
// pointer to command index parameter in g
AP_Int8 *_command_index;
// saved waypoint for resuming mission // saved waypoint for resuming mission
uint8_t _saved_wp; uint8_t _saved_wp;
}; };