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

APM_OBC: added pressure altitude termination

This commit is contained in:
Andrew Tridgell 2014-04-11 16:35:01 +10:00
parent 43fc6ce0b8
commit ad18019fb1
2 changed files with 85 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/APM_OBC/APM_OBC.cpp
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
@ -68,6 +69,27 @@ const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
// @User: Advanced
AP_GROUPINFO("TERM_PIN", 7, APM_OBC, _terminate_pin, -1),
// @Param: AMSL_LIMIT
// @DisplayName: AMSL limit
// @Description: This sets the AMSL (above mean sea level) altitude limit. If the pressure altitude determined by QNH exceeds this limit then flight termination will be forced. Note that this limit is in meters, whereas pressure altitude limits are often quoted in feet. A value of zero disables the pressure altitude limit.
// @User: Advanced
// @Units: meters
AP_GROUPINFO("AMSL_LIMIT", 8, APM_OBC, _amsl_limit, 0),
// @Param: AMSL_ERR_GPS
// @DisplayName: Error margin for GPS based AMSL limit
// @Description: This sets margin for error in GPS derived altitude limit. This error margin is only used if the barometer has failed. If the barometer fails then the GPS will be used to enforce the AMSL_LIMIT, but this margin will be subtracted from the AMSL_LIMIT first, to ensure that even with the given amount of GPS altitude error the pressure altitude is not breached. OBC users should set this to comply with their D2 safety case. A value of -1 will mean that barometer failure will lead to immediate termination.
// @User: Advanced
// @Units: meters
AP_GROUPINFO("AMSL_ERR_GPS", 9, APM_OBC, _amsl_margin_gps, -1),
// @Param: QNH_PRESSURE
// @DisplayName: QNH pressure
// @Description: This sets the QNH pressure in millibars to be used for pressure altitude in the altitude limit. A value of zero disables the altitude limit.
// @Units: millibar
// @User: Advanced
AP_GROUPINFO("QNH_PRESSURE", 10, APM_OBC, _qnh_pressure, 0),
AP_GROUPEND
};
@ -77,12 +99,10 @@ extern bool geofence_breached(void);
// 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,
AP_GPS &gps, AP_Mission &mission)
APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms)
{
// we always check for fence breach
if (geofence_breached()) {
if (geofence_breached() || check_altlimit()) {
if (!_terminate) {
hal.console->println_P(PSTR("Fence TERMINATE"));
_terminate.set(1);
@ -189,3 +209,35 @@ APM_OBC::check(APM_OBC::control_mode mode,
hal.gpio->write(_terminate_pin, _terminate?1:0);
}
}
// check for altitude limit breach
bool
APM_OBC::check_altlimit(void)
{
if (_amsl_limit == 0 || _qnh_pressure <= 0) {
// no limit set
return false;
}
// see if the barometer is dead
if (hal.scheduler->millis() - baro.get_last_update() > 5000) {
// the barometer has been unresponsive for 5 seconds. See if we can switch to GPS
if (_amsl_margin_gps != -1 &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
gps.location().alt*0.01f <= _amsl_limit - _amsl_margin_gps) {
// GPS based altitude OK
return false;
}
// no barometer - immediate termination
return true;
}
float alt_amsl = baro.get_altitude_difference(_qnh_pressure*100, baro.get_pressure());
if (alt_amsl > _amsl_limit) {
// pressure altitude breach
return true;
}
// all OK
return false;
}

44
libraries/APM_OBC/APM_OBC.h
View File

@ -1,3 +1,5 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef APM_OBC_H
#define APM_OBC_H
/*
@ -23,6 +25,7 @@
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Mission.h>
#include <AP_Baro.h>
#include <AP_GPS.h>
#include <inttypes.h>
@ -44,24 +47,25 @@ public:
};
// Constructor
APM_OBC(void)
{
AP_Param::setup_object_defaults(this, var_info);
APM_OBC(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps) :
mission(_mission),
baro(_baro),
gps(_gps)
{
AP_Param::setup_object_defaults(this, var_info);
_last_heartbeat_pin = -1;
_last_manual_pin = -1;
_state = STATE_PREFLIGHT;
_terminate.set(0);
_saved_wp = 0;
}
_last_heartbeat_pin = -1;
_last_manual_pin = -1;
_state = STATE_PREFLIGHT;
_terminate.set(0);
_saved_wp = 0;
}
void check(enum control_mode control_mode,
uint32_t last_heartbeat_ms,
AP_GPS &gps, AP_Mission &mission);
void check(enum control_mode control_mode, uint32_t last_heartbeat_ms);
// should we crash the plane? Only possible with
// FS_TERM_ACTTION set to 43
// FS_TERM_ACTTION set to 42
bool crash_plane(void) {
return _terminate && _terminate_action == 42;
}
@ -72,6 +76,10 @@ public:
private:
enum state _state;
AP_Mission &mission;
AP_Baro &baro;
const AP_GPS &gps;
// digital output pins for communicating with the failsafe board
AP_Int8 _heartbeat_pin;
AP_Int8 _manual_pin;
@ -88,10 +96,16 @@ private:
AP_Int8 _wp_comms_hold;
AP_Int8 _wp_gps_loss;
AP_Float _qnh_pressure;
AP_Int32 _amsl_limit;
AP_Int32 _amsl_margin_gps;
bool _heartbeat_pin_value;
// saved waypoint for resuming mission
uint8_t _saved_wp;
bool check_altlimit(void);
};
// map from ArduPlane control_mode to APM_OBC::control_mode