aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM_OBC: translate to AP_HAL, make trivial test sketch to test build

Browse Source
This commit is contained in:
Pat Hickey 2012-11-12 13:34:14 -08:00 committed by Andrew Tridgell
parent 3b1150d857
commit eefb0f4515
5 changed files with 37 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/APM_OBC/APM_OBC.cpp
View File

@ -8,9 +8,11 @@
as published by the Free Software Foundation; either version 2.1
of the License, or (at your option) any later version.
*/
#include <FastSerial.h>
#include <AP_HAL.h>
#include <APM_OBC.h>
extern const AP_HAL::HAL& hal;
// table of user settable parameters
const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
// @Param: MAN_PIN
@ -64,9 +66,6 @@ extern bool geofence_breached(void);
// function to change waypoint
extern void change_command(uint8_t cmd_index);
// for sending messages
extern void gcs_send_text_fmt(const prog_char_t *fmt, ...);
// check for Failsafe conditions. This is called at 10Hz by the main
// ArduPlane code
void
@ -77,7 +76,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
// we always check for fence breach
if (geofence_breached()) {
if (!_terminate) {
gcs_send_text_fmt(PSTR("Fence TERMINATE"));
hal.console->println_P(PSTR("Fence TERMINATE"));
_terminate.set(1);
}
}
@ -87,13 +86,13 @@ APM_OBC::check(APM_OBC::control_mode mode,
// receiver
if (_manual_pin != -1) {
if (_manual_pin != _last_manual_pin) {
pinMode(_manual_pin, OUTPUT);
hal.gpio->pinMode(_manual_pin, GPIO_OUTPUT);
_last_manual_pin = _manual_pin;
}
digitalWrite(_manual_pin, mode==OBC_MANUAL);
hal.gpio->write(_manual_pin, mode==OBC_MANUAL);
}
uint32_t now = millis();
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);
@ -102,7 +101,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
// we startup in preflight mode. This mode ends when
// we first enter auto.
if (mode == OBC_AUTO) {
gcs_send_text_fmt(PSTR("Starting OBC_AUTO"));
hal.console->println_P(PSTR("Starting OBC_AUTO"));
_state = STATE_AUTO;
}
break;
@ -110,7 +109,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
case STATE_AUTO:
// this is the normal mode.
if (!gcs_link_ok) {
gcs_send_text_fmt(PSTR("State DATA_LINK_LOSS"));
hal.console->println_P(PSTR("State DATA_LINK_LOSS"));
_state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) {
if (_command_index != NULL) {
@ -121,7 +120,7 @@ APM_OBC::check(APM_OBC::control_mode mode,
break;
}
if (!gps_lock_ok) {
gcs_send_text_fmt(PSTR("State GPS_LOSS"));
hal.console->println_P(PSTR("State GPS_LOSS"));
_state = STATE_GPS_LOSS;
if (_wp_gps_loss) {
if (_command_index != NULL) {
@ -137,11 +136,11 @@ APM_OBC::check(APM_OBC::control_mode mode,
if (!gps_lock_ok) {
// losing GPS lock when data link is lost
// leads to termination
gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
hal.console->println_P(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gcs_link_ok) {
_state = STATE_AUTO;
gcs_send_text_fmt(PSTR("GCS OK"));
hal.console->println_P(PSTR("GCS OK"));
if (_saved_wp != 0) {
change_command(_saved_wp);
_saved_wp = 0;
@ -153,10 +152,10 @@ APM_OBC::check(APM_OBC::control_mode mode,
if (!gcs_link_ok) {
// losing GCS link when GPS lock lost
// leads to termination
gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
hal.console->println_P(PSTR("Dual loss TERMINATE"));
_terminate.set(1);
} else if (gps_lock_ok) {
gcs_send_text_fmt(PSTR("GPS OK"));
hal.console->println_P(PSTR("GPS OK"));
_state = STATE_AUTO;
if (_saved_wp != 0) {
change_command(_saved_wp);
@ -170,19 +169,19 @@ APM_OBC::check(APM_OBC::control_mode mode,
// pin configured then toggle the heartbeat pin at 10Hz
if (_heartbeat_pin != -1 && (_terminate_pin != -1 || !_terminate)) {
if (_heartbeat_pin != _last_heartbeat_pin) {
pinMode(_heartbeat_pin, OUTPUT);
hal.gpio->pinMode(_heartbeat_pin, GPIO_OUTPUT);
_last_heartbeat_pin = _heartbeat_pin;
}
_heartbeat_pin_value = !_heartbeat_pin_value;
digitalWrite(_heartbeat_pin, _heartbeat_pin_value);
hal.gpio->write(_heartbeat_pin, _heartbeat_pin_value);
}
// set the terminate pin
if (_terminate_pin != -1) {
if (_terminate_pin != _last_terminate_pin) {
pinMode(_terminate_pin, OUTPUT);
hal.gpio->pinMode(_terminate_pin, GPIO_OUTPUT);
_last_terminate_pin = _terminate_pin;
}
digitalWrite(_terminate_pin, _terminate?HIGH:LOW);
hal.gpio->write(_terminate_pin, _terminate?1:0);
}
}

0
libraries/APM_OBC/examples/trivial_APM_OBC/Arduino.h Normal file
View File

1
libraries/APM_OBC/examples/trivial_APM_OBC/Makefile Normal file
View File

@ -0,0 +1 @@
include ../../../AP_Common/Arduino.mk

0
libraries/APM_OBC/examples/trivial_APM_OBC/nocore.inoflag Normal file
View File

18
libraries/APM_OBC/examples/trivial_APM_OBC/trivial_APM_OBC.pde Normal file
View File

@ -0,0 +1,18 @@
#include <AP_Common.h>
#include <AP_Progmem.h>
#include <AP_Param.h>
#include <AP_Math.h>
#include <AP_HAL.h>
#include <APM_OBC.h>
#include <AP_HAL_AVR.h>
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
void setup () {
hal.console->println_P(PSTR("Unit test for APM_OBC. This sketch"
"has no functionality, it only tests build."));
}
void loop () {}
AP_HAL_MAIN();