APO fixes.

ArduBoat/BoatGeneric.h

@@ -10,7 +10,7 @@
 #define BOATGENERIC_H_
 
 // vehicle options
-static const apo::vehicle_t vehicle = apo::VEHICLE_BOAT;
+static const MAV_TYPE vehicle = UGV_SURFACE_SHIP; 
 static const apo::halMode_t halMode = apo::MODE_LIVE;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
@@ -31,7 +31,7 @@ static const uint8_t heartBeatTimeout = 3;
 static const uint32_t debugBaud = 57600;
 static const uint32_t telemBaud = 57600;
 static const uint32_t gpsBaud = 38400;
-static const uint32_t hilBaud = 57600;
+static const uint32_t hilBaud = 115200;
 
 // optional sensors
 static const bool gpsEnabled = false;

ArduRover/CarStampede.h

@@ -10,9 +10,9 @@
 #define CARSTAMPEDE_H_
 
 // vehicle options
-static const apo::vehicle_t vehicle = apo::VEHICLE_CAR;
-//static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
-static const apo::halMode_t halMode = apo::MODE_LIVE;
+static const MAV_TYPE vehicle = UGV_GROUND_ROVER;
+static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
+//static const apo::halMode_t halMode = apo::MODE_LIVE;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
 
@@ -32,7 +32,7 @@ static const uint8_t heartBeatTimeout = 3;
 static const uint32_t debugBaud = 57600;
 static const uint32_t telemBaud = 57600;
 static const uint32_t gpsBaud = 38400;
-static const uint32_t hilBaud = 57600;
+static const uint32_t hilBaud = 115200;
 
 // optional sensors
 static const bool gpsEnabled = false;

ArduRover/TankGeneric.h

@@ -11,7 +11,7 @@
 #define TANKGENERIC_H_
 
 // vehicle options
-static const apo::vehicle_t vehicle = apo::VEHICLE_TANK;
+static const MAV_TYPE vehicle = UGV_GROUND_ROVER;
 static const apo::halMode_t halMode = apo::MODE_LIVE;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
@@ -32,7 +32,7 @@ static const uint8_t heartBeatTimeout = 3;
 static const uint32_t debugBaud = 57600;
 static const uint32_t telemBaud = 57600;
 static const uint32_t gpsBaud = 38400;
-static const uint32_t hilBaud = 57600;
+static const uint32_t hilBaud = 115200;
 
 // optional sensors
 static const bool gpsEnabled = false;

apo/PlaneEasystar.h

@@ -9,7 +9,7 @@
 #define PLANEEASYSTAR_H_
 
 // vehicle options
-static const apo::vehicle_t vehicle = apo::VEHICLE_PLANE;
+static const MAV_TYPE vehicle = MAV_TYPE_FIXED_WING;
 static const apo::halMode_t halMode = apo::MODE_LIVE;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 3;
@@ -30,7 +30,7 @@ static const uint8_t heartBeatTimeout = 3;
 static const uint32_t debugBaud = 57600;
 static const uint32_t telemBaud = 57600;
 static const uint32_t gpsBaud = 38400;
-static const uint32_t hilBaud = 57600;
+static const uint32_t hilBaud = 115200;
 
 // optional sensors
 static const bool gpsEnabled = false;

apo/QuadArducopter.h

@@ -9,7 +9,7 @@
 #define QUADARDUCOPTER_H_
 
 // vehicle options
-static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
+static const MAV_TYPE vehicle = MAV_QUADROTOR;
 static const apo::halMode_t halMode = apo::MODE_HIL_CNTL;
 static const apo::board_t board = apo::BOARD_ARDUPILOTMEGA_1280;
 static const uint8_t heartBeatTimeout = 0;

libraries/APO/APO_DefaultSetup.h

@@ -30,8 +30,7 @@ void setup() {
     Serial.begin(debugBaud, 128, 128); // debug
 
     // hardware abstraction layer
-    hal = new AP_HardwareAbstractionLayer(
-        halMode, board, vehicle, heartBeatTimeout);
+    hal = new AP_HardwareAbstractionLayer(halMode, board, vehicle);
 
     // debug serial
     hal->debug = &Serial;
@@ -150,12 +149,12 @@ void setup() {
     if (board==BOARD_ARDUPILOTMEGA_2)
     {
         Serial2.begin(telemBaud, 128, 128); // gcs
-        hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal);
+        hal->gcs = new COMMLINK_CLASS(&Serial2, navigator, guide, controller, hal, heartBeatTimeout);
     }
     else
     {
         Serial3.begin(telemBaud, 128, 128); // gcs
-        hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal);
+        hal->gcs = new COMMLINK_CLASS(&Serial3, navigator, guide, controller, hal, heartBeatTimeout);
     }
 
     /*
@@ -166,7 +165,7 @@ void setup() {
         Serial1.begin(hilBaud, 128, 128);
         delay(1000);
         Serial1.println("starting hil");
-        hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal);
+        hal->hil = new COMMLINK_CLASS(&Serial1, navigator, guide, controller, hal, heartBeatTimeout);
     }
 
     /*

libraries/APO/AP_Autopilot.h

@@ -16,8 +16,10 @@
  * ArduPilotOne does not use any global
  * variables.
  */
+
 namespace apo {
 
+// enumerations
 // forward declarations
 class AP_Navigator;
 class AP_Guide;

libraries/APO/AP_CommLink.cpp

@@ -25,14 +25,16 @@ uint8_t MavlinkComm::_nChannels = 0;
 uint8_t MavlinkComm::_paramNameLengthMax = 13;
 
 AP_CommLink::AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
-                         AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
+                         AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
+                         const uint16_t heartBeatTimeout) :
     _link(link), _navigator(navigator), _guide(guide),
-    _controller(controller), _hal(hal) {
+    _controller(controller), _hal(hal), _heartBeatTimeout(heartBeatTimeout), _lastHeartBeat(0) {
 }
 
 MavlinkComm::MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
-                         AP_Controller * controller, AP_HardwareAbstractionLayer * hal) :
-    AP_CommLink(link, nav, guide, controller, hal),
+                         AP_Controller * controller, AP_HardwareAbstractionLayer * hal,
+                         const uint16_t heartBeatTimeout) :
+    AP_CommLink(link, nav, guide, controller, hal,heartBeatTimeout),
 
     // options
     _useRelativeAlt(true),
@@ -83,7 +85,7 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
     switch (id) {
 
     case MAVLINK_MSG_ID_HEARTBEAT: {
-        mavlink_msg_heartbeat_send(_channel, mavlink_system.type,
+        mavlink_msg_heartbeat_send(_channel, _hal->getVehicle(),
                                    MAV_AUTOPILOT_ARDUPILOTMEGA);
         break;
     }
@@ -311,7 +313,7 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
     case MAVLINK_MSG_ID_HEARTBEAT: {
         mavlink_heartbeat_t packet;
         mavlink_msg_heartbeat_decode(msg, &packet);
-        _hal->lastHeartBeat = micros();
+        _lastHeartBeat = micros();
         break;
     }
 

libraries/APO/AP_CommLink.h

@@ -46,7 +46,7 @@ class AP_CommLink {
 public:
 
     AP_CommLink(FastSerial * link, AP_Navigator * navigator, AP_Guide * guide,
-                AP_Controller * controller, AP_HardwareAbstractionLayer * hal);
+                AP_Controller * controller, AP_HardwareAbstractionLayer * hal, const uint16_t heartBeatTimeout);
     virtual void send() = 0;
     virtual void receive() = 0;
     virtual void sendMessage(uint8_t id, uint32_t param = 0) = 0;
@@ -56,18 +56,28 @@ public:
     virtual void sendParameters() = 0;
     virtual void requestCmds() = 0;
 
+    /// check if heartbeat is lost
+    bool heartBeatLost() {
+        if (_heartBeatTimeout == 0)
+            return false;
+        else
+            return ((micros() - _lastHeartBeat) / 1e6) > _heartBeatTimeout;
+    }
+
 protected:
     FastSerial * _link;
     AP_Navigator * _navigator;
     AP_Guide * _guide;
     AP_Controller * _controller;
     AP_HardwareAbstractionLayer * _hal;
+    uint16_t _heartBeatTimeout;              /// vehicle heartbeat timeout, s
+    uint32_t _lastHeartBeat;                 /// time of last heartbeat, s
 };
 
 class MavlinkComm: public AP_CommLink {
 public:
     MavlinkComm(FastSerial * link, AP_Navigator * nav, AP_Guide * guide,
-                AP_Controller * controller, AP_HardwareAbstractionLayer * hal);
+                AP_Controller * controller, AP_HardwareAbstractionLayer * hal, uint16_t heartBeatTimeout);
 
     virtual void send();
     void sendMessage(uint8_t id, uint32_t param = 0);

libraries/APO/AP_Controller.cpp

@@ -65,7 +65,7 @@ void AP_Controller::update(const float dt) {
         // handle emergencies
         //
         // check for heartbeat from gcs, if not found go to failsafe
-        if (_hal->heartBeatLost()) {
+        if (_hal->gcs->heartBeatLost()) {
             setMode(MAV_MODE_FAILSAFE);
             _hal->gcs->sendText(SEVERITY_HIGH, PSTR("configure gcs to send heartbeat"));
             

libraries/APO/AP_HardwareAbstractionLayer.h

@@ -36,9 +36,6 @@ enum halMode_t {
 enum board_t {
     BOARD_ARDUPILOTMEGA_1280, BOARD_ARDUPILOTMEGA_2560, BOARD_ARDUPILOTMEGA_2
 };
-enum vehicle_t {
-    VEHICLE_CAR, VEHICLE_QUAD, VEHICLE_PLANE, VEHICLE_BOAT, VEHICLE_TANK
-};
 
 class AP_HardwareAbstractionLayer {
 
@@ -47,12 +44,10 @@ public:
     // default ctors on pointers called on pointers here, this
     // allows NULL to be used as a boolean for if the device was
     // initialized
-    AP_HardwareAbstractionLayer(halMode_t mode, board_t board,
-                                vehicle_t vehicle, uint8_t heartBeatTimeout) :
+    AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle) :
         adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), 
         radio(), rc(), gcs(),
-        hil(), debug(), load(), lastHeartBeat(),
-        _heartBeatTimeout(heartBeatTimeout), _mode(mode),
+        hil(), debug(), load(), _mode(mode),
         _board(board), _vehicle(vehicle) {
 
         /*
@@ -132,7 +127,6 @@ public:
      * data
      */
     uint8_t load;
-    uint32_t lastHeartBeat;
 
     /**
      * settings
@@ -151,23 +145,16 @@ public:
     board_t getBoard() {
         return _board;
     }
-    vehicle_t getVehicle() {
+    MAV_TYPE getVehicle() {
         return _vehicle;
     }
-    bool heartBeatLost() {
-        if (_heartBeatTimeout == 0)
-            return false;
-        else
-            return ((micros() - lastHeartBeat) / 1e6) > _heartBeatTimeout;
-    }
 
 private:
 
     // enumerations
-    uint8_t _heartBeatTimeout;
     halMode_t _mode;
     board_t _board;
-    vehicle_t _vehicle;
+    MAV_TYPE _vehicle;
 };
 
 } // namespace apo