From 9b25875bafb1d7017ab0a7adf80aed6d29b38048 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sun, 1 May 2011 03:38:41 +0000 Subject: [PATCH] Heartbeat failsafe added for APO. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1940 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/APO/AP_Guide.h | 14 +++++++------- libraries/APO/AP_HardwareAbstractionLayer.h | 2 +- libraries/APO/controllers.h | 1 + .../APO/examples/AutopilotCar/AutopilotCar.pde | 2 +- .../APO/examples/AutopilotQuad/AutopilotQuad.pde | 2 +- 5 files changed, 11 insertions(+), 10 deletions(-) diff --git a/libraries/APO/AP_Guide.h b/libraries/APO/AP_Guide.h index 9cdc7b6a51..c96d54a583 100644 --- a/libraries/APO/AP_Guide.h +++ b/libraries/APO/AP_Guide.h @@ -200,13 +200,13 @@ public: //airSpeedCommand = 0; //groundSpeedCommand = 0; headingCommand -= 45*deg2Rad; - _hal->debug->print("Obstacle Distance (m): "); - _hal->debug->println(frontDistance); - _hal->debug->print("Obstacle avoidance Heading Command: "); - _hal->debug->println(headingCommand); - _hal->debug->printf_P( - PSTR("Front Distance, %f\n"), - frontDistance); +// _hal->debug->print("Obstacle Distance (m): "); +// _hal->debug->println(frontDistance); +// _hal->debug->print("Obstacle avoidance Heading Command: "); +// _hal->debug->println(headingCommand); +// _hal->debug->printf_P( +// PSTR("Front Distance, %f\n"), +// frontDistance); } if (_rangeFinderBack && _rangeFinderBack->distance < 5) { airSpeedCommand = 0; diff --git a/libraries/APO/AP_HardwareAbstractionLayer.h b/libraries/APO/AP_HardwareAbstractionLayer.h index 0fdeee2aae..c24e5f2c14 100644 --- a/libraries/APO/AP_HardwareAbstractionLayer.h +++ b/libraries/APO/AP_HardwareAbstractionLayer.h @@ -47,7 +47,7 @@ public: AP_HardwareAbstractionLayer(halMode_t mode, board_t board, vehicle_t vehicle) : _mode(mode), _board(board), _vehicle(vehicle), adc(), gps(), baro(), compass(), rangeFinders(), - imu(), rc(), gcs(), hil(), debug(), load() + imu(), rc(), gcs(), hil(), debug(), load(), lastHeartBeat() { } /** diff --git a/libraries/APO/controllers.h b/libraries/APO/controllers.h index 408dc1532c..0cf1a8475a 100644 --- a/libraries/APO/controllers.h +++ b/libraries/APO/controllers.h @@ -131,6 +131,7 @@ public: _hal->rc[CH_RIGHT]->setPosition(0); _hal->rc[CH_FRONT]->setPosition(0); _hal->rc[CH_BACK]->setPosition(0); + return; } // read and set pwm so they can be read as positions later diff --git a/libraries/APO/examples/AutopilotCar/AutopilotCar.pde b/libraries/APO/examples/AutopilotCar/AutopilotCar.pde index f82f24471f..84e5f4cb22 100644 --- a/libraries/APO/examples/AutopilotCar/AutopilotCar.pde +++ b/libraries/APO/examples/AutopilotCar/AutopilotCar.pde @@ -60,7 +60,7 @@ const float loop4Rate = 0.1; // max time in seconds to allow flight without ground station comms // zero will ignore timeout -const uint8_t heartbeatTimeout = 3.0; +const uint8_t heartbeatTimeout = 3; //---------HARDWARE CONFIG ----------------// diff --git a/libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde b/libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde index 2e3cab841b..1332b9049b 100644 --- a/libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde +++ b/libraries/APO/examples/AutopilotQuad/AutopilotQuad.pde @@ -60,7 +60,7 @@ const float loop4Rate = 0.1; // max time in seconds to allow flight without ground station comms // zero will ignore timeout -const uint8_t heartbeatTimeout = 3.0; +const uint8_t heartbeatTimeout = 3; //---------HARDWARE CONFIG ----------------//