mirror of https://github.com/ArduPilot/ardupilot
APM_OBC: update for AP_Mission and AP_GPS changes
This commit is contained in:
parent
43ebd86bb1
commit
6f55dc1686
|
@ -74,15 +74,12 @@ const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
|
|||
// access to geofence state
|
||||
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
|
||||
// ArduPlane code
|
||||
void
|
||||
APM_OBC::check(APM_OBC::control_mode mode,
|
||||
uint32_t last_heartbeat_ms,
|
||||
uint32_t last_gps_fix_ms)
|
||||
AP_GPS &gps, AP_Mission &mission)
|
||||
{
|
||||
// we always check for fence breach
|
||||
if (geofence_breached()) {
|
||||
|
@ -105,7 +102,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
|
|||
|
||||
uint32_t now = hal.scheduler->millis();
|
||||
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) {
|
||||
case STATE_PREFLIGHT:
|
||||
|
@ -123,10 +120,8 @@ APM_OBC::check(APM_OBC::control_mode mode,
|
|||
hal.console->println_P(PSTR("State DATA_LINK_LOSS"));
|
||||
_state = STATE_DATA_LINK_LOSS;
|
||||
if (_wp_comms_hold) {
|
||||
if (_command_index != NULL) {
|
||||
_saved_wp = _command_index->get();
|
||||
}
|
||||
change_command(_wp_comms_hold);
|
||||
_saved_wp = mission.get_current_nav_cmd().index;
|
||||
mission.set_current_cmd(_wp_comms_hold);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
@ -134,10 +129,8 @@ APM_OBC::check(APM_OBC::control_mode mode,
|
|||
hal.console->println_P(PSTR("State GPS_LOSS"));
|
||||
_state = STATE_GPS_LOSS;
|
||||
if (_wp_gps_loss) {
|
||||
if (_command_index != NULL) {
|
||||
_saved_wp = _command_index->get();
|
||||
}
|
||||
change_command(_wp_gps_loss);
|
||||
_saved_wp = mission.get_current_nav_cmd().index;
|
||||
mission.set_current_cmd(_wp_gps_loss);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
@ -153,7 +146,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
|
|||
_state = STATE_AUTO;
|
||||
hal.console->println_P(PSTR("GCS OK"));
|
||||
if (_saved_wp != 0) {
|
||||
change_command(_saved_wp);
|
||||
mission.set_current_cmd(_saved_wp);
|
||||
_saved_wp = 0;
|
||||
}
|
||||
}
|
||||
|
@ -169,7 +162,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
|
|||
hal.console->println_P(PSTR("GPS OK"));
|
||||
_state = STATE_AUTO;
|
||||
if (_saved_wp != 0) {
|
||||
change_command(_saved_wp);
|
||||
mission.set_current_cmd(_saved_wp);
|
||||
_saved_wp = 0;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -22,6 +22,8 @@
|
|||
|
||||
#include <AP_Common.h>
|
||||
#include <AP_Param.h>
|
||||
#include <AP_Mission.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <inttypes.h>
|
||||
|
||||
|
||||
|
@ -51,16 +53,12 @@ public:
|
|||
_state = STATE_PREFLIGHT;
|
||||
_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;
|
||||
}
|
||||
|
||||
void check(enum control_mode control_mode,
|
||||
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
|
||||
// FS_TERM_ACTTION set to 43
|
||||
|
@ -92,9 +90,6 @@ private:
|
|||
|
||||
bool _heartbeat_pin_value;
|
||||
|
||||
// pointer to command index parameter in g
|
||||
AP_Int8 *_command_index;
|
||||
|
||||
// saved waypoint for resuming mission
|
||||
uint8_t _saved_wp;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue