diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp index 7ad8b7e8b9..0fdf259869 100644 --- a/libraries/APM_OBC/APM_OBC.cpp +++ b/libraries/APM_OBC/APM_OBC.cpp @@ -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 +#include #include +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); } } diff --git a/libraries/APM_OBC/examples/trivial_APM_OBC/Arduino.h b/libraries/APM_OBC/examples/trivial_APM_OBC/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/APM_OBC/examples/trivial_APM_OBC/Makefile b/libraries/APM_OBC/examples/trivial_APM_OBC/Makefile new file mode 100644 index 0000000000..d1f40fd90f --- /dev/null +++ b/libraries/APM_OBC/examples/trivial_APM_OBC/Makefile @@ -0,0 +1 @@ +include ../../../AP_Common/Arduino.mk diff --git a/libraries/APM_OBC/examples/trivial_APM_OBC/nocore.inoflag b/libraries/APM_OBC/examples/trivial_APM_OBC/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/APM_OBC/examples/trivial_APM_OBC/trivial_APM_OBC.pde b/libraries/APM_OBC/examples/trivial_APM_OBC/trivial_APM_OBC.pde new file mode 100644 index 0000000000..e36519bab2 --- /dev/null +++ b/libraries/APM_OBC/examples/trivial_APM_OBC/trivial_APM_OBC.pde @@ -0,0 +1,18 @@ + +#include +#include +#include +#include +#include +#include + +#include +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();