Working on new hil message for mavlink.

apo/ControllerQuad.h

@@ -88,10 +88,6 @@ private:
     void autoLoop(const float dt) {
         autoPositionLoop(dt);
         autoAttitudeLoop(dt);
-
-        // XXX currently auto loop not tested, so
-        // put vehicle in standby
-        _hal->setState(MAV_STATE_STANDBY);
     }
     void autoPositionLoop(float dt) {
         float cmdNorthTilt = pidPN.update(_nav->getPN(),_nav->getVN(),dt);
@@ -105,6 +101,11 @@ private:
             _cmdPitch = cmdEastTilt * trigCos - cmdNorthTilt * trigSin;
             _cmdRoll = -cmdEastTilt * trigSin + cmdNorthTilt * trigCos;
             // note that the north tilt is negative of the pitch
+            
+        Serial.print("  trigSin: ");
+        Serial.print(trigSin);
+        Serial.print("  trigCos: ");
+        Serial.print(trigCos);
         }
         _cmdYawRate = 0;
 
@@ -116,6 +117,34 @@ private:
 
         if (fabs(_cmdPitch) > 0.5) _thrustMix *= 1.13949393;
         else _thrustMix /= cos(_cmdPitch);
+        
+        //debug statements
+        Serial.print(" getPN: ");
+        Serial.print(_nav->getPN());
+        Serial.print("  getPE: ");
+        Serial.print(_nav->getPE());
+        Serial.print("  getPD: ");
+        Serial.print(_nav->getPD());
+        Serial.print("  getVN: ");
+        Serial.print(_nav->getVN());
+        Serial.print("  getVE: ");
+        Serial.print(_nav->getVE());
+        Serial.print("  getVD: ");
+        Serial.println(_nav->getVD());
+        //Serial.print("Roll: ");
+        //Serial.print(_cmdRoll);
+        //Serial.print("    Pitch");
+        //Serial.print(_cmdPitch);
+        //Serial.print("    YawRate");
+        //Serial.print(_cmdYawRate);
+        //Serial.print("    ThrustMix");
+        //Serial.print(_thrustMix);
+        //Serial.print("    North Tilt");
+        //Serial.print(cmdNorthTilt);
+        //Serial.print("    East Tilt");
+        //Serial.print(cmdEastTilt);
+        //Serial.print("   Down");
+        //Serial.println(cmdDown);
     }
     void autoAttitudeLoop(float dt) {
         _rollMix = pidRoll.update(_cmdRoll - _nav->getRoll(),
@@ -123,8 +152,29 @@ private:
         _pitchMix = pidPitch.update(_cmdPitch - _nav->getPitch(),
                                     _nav->getPitchRate(), dt);
         _yawMix = pidYawRate.update(_cmdYawRate - _nav->getYawRate(), dt);
+        
+        //debug statements
+        //Serial.print("Roll Cmd: ");
+        //Serial.print(_cmdRoll*1000);
+        //Serial.print("  Nav_GetRoll: ");
+        //Serial.print(_nav->getRoll()*1000);
+        //Serial.print("  Roll Error: ");
+        //Serial.print((_cmdRoll - _nav->getRoll())*1000);
+        //Serial.print("  Pitch Cmd: ");
+        //Serial.print(_cmdPitch*1000);
+        //Serial.print("  Nav_GetPitch: ");
+        //Serial.print(_nav->getPitch()*1000);
+        //Serial.print("  Pitch Error: ");
+        //Serial.println((_cmdPitch - _nav->getPitch())*1000);
+        //Serial.print("  YawRate Cmd: ");
+        //Serial.print(_cmdYawRate);
+        //Serial.print("  Nav_GetYawRate: ");
+        //Serial.print( _nav->getYawRate());
+        //Serial.print("  YawRate Error: ");
+        //Serial.println(_cmdYawRate - _nav->getYawRate());
     }
     void setMotorsActive() {
+    
         // turn all motors off if below 0.1 throttle
         if (fabs(_hal->rc[ch_thrust]->getRadioPosition()) < 0.1) {
             setAllRadioChannelsToNeutral();

apo/QuadArducopter.h

@@ -10,7 +10,7 @@
 
 // vehicle options
 static const apo::vehicle_t vehicle = apo::VEHICLE_QUAD;
-static const apo::halMode_t halMode = apo::MODE_LIVE;
+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 = 3;
 

apo/apo.pde

@@ -8,7 +8,6 @@
 #include <AP_ADC.h>
 #include <AP_DCM.h>
 #include <AP_Compass.h>
-#include <Wire.h>
 #include <AP_GPS.h>
 #include <AP_IMU.h>
 #include <APM_BMP085.h>
@@ -16,8 +15,8 @@
 #include <APO.h>
 
 // Vehicle Configuration
-//#include "QuadArducopter.h"
-#include "PlaneEasystar.h"
+#include "QuadArducopter.h"
+//#include "PlaneEasystar.h"
 
 // ArduPilotOne Default Setup
 #include "APO_DefaultSetup.h"

libraries/APO/AP_Autopilot.cpp

@@ -232,6 +232,7 @@ void AP_Autopilot::callback2(void * data) {
         apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_SCALED);
         apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_RC_CHANNELS_RAW);
         //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
+        apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION);
         apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_SCALED_IMU);
     }
 

libraries/APO/AP_CommLink.cpp

@@ -105,11 +105,18 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
         break;
     }
 
+    case MAVLINK_MSG_ID_LOCAL_POSITION: {
+        mavlink_msg_local_position_send(_channel, timeStamp,
+                _navigator->getPN(),_navigator->getPE(), _navigator->getPD(),
+                _navigator->getVN(), _navigator->getVE(), _navigator->getVD());
+        break;
+    }
+
     case MAVLINK_MSG_ID_GPS_RAW: {
         mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
                                  _navigator->getLat() * rad2Deg,
                                  _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0,
-                                 _navigator->getGroundSpeed(),
+                                 _navigator->getGroundSpeed()*1000.0,
                                  _navigator->getYaw() * rad2Deg);
         break;
     }
@@ -328,6 +335,26 @@ void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
         break;
     }
 
+    case MAVLINK_MSG_ID_HIL_STATE: {
+        // decode
+        mavlink_hil_state_t packet;
+        mavlink_msg_hil_state_decode(msg, &packet);
+        _navigator->setTimeStamp(timeStamp);
+        _navigator->setRoll(packet.roll);
+        _navigator->setPitch(packet.pitch);
+        _navigator->setYaw(packet.yaw);
+        _navigator->setRollRate(packet.rollspeed);
+        _navigator->setPitchRate(packet.pitchspeed);
+        _navigator->setYawRate(packet.yawspeed);
+        _navigator->setVN(packet.vx/ 1e2);
+        _navigator->setVE(packet.vy/ 1e2);
+        _navigator->setVD(packet.vz/ 1e2);
+        _navigator->setLat_degInt(packet.lat);
+        _navigator->setLon_degInt(packet.lon);
+        _navigator->setAlt(packet.alt / 1e3);
+        break; 
+    } 
+
     case MAVLINK_MSG_ID_ATTITUDE: {
         // decode
         mavlink_attitude_t packet;

libraries/APO/AP_Navigator.cpp

@@ -22,8 +22,9 @@ namespace apo {
 
 AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) :
     _hal(hal), _timeStamp(0), _roll(0), _rollRate(0), _pitch(0),
-    _pitchRate(0), _yaw(0), _yawRate(0), _airSpeed(0),
-    _groundSpeed(0), _vD(0), _lat_degInt(0),
+    _pitchRate(0), _yaw(0), _yawRate(0), 
+    _windSpeed(0), _windDirection(0),
+    _vN(0), _vE(0), _vD(0), _lat_degInt(0),
     _lon_degInt(0), _alt_intM(0) {
 }
 void AP_Navigator::calibrate() {

libraries/APO/AP_Navigator.h

@@ -45,7 +45,18 @@ public:
     void setPN(float _pN);
 
     float getAirSpeed() const {
-        return _airSpeed;
+        // neglects vertical wind
+        float vWN = getVN() + getWindSpeed()*cos(getWindDirection());
+        float vWE = getVE() + getWindSpeed()*sin(getWindDirection());
+        return sqrt(vWN*vWN+vWE+vWE+getVD()*getVD());
+    }
+
+    float getGroundSpeed() const {
+        return sqrt(getVN()*getVN()+getVE()*getVE());
+    } 
+
+    float getWindSpeed() const {
+        return _windSpeed;
     }
 
     int32_t getAlt_intM() const {
@@ -80,16 +91,16 @@ public:
         this->_lon_degInt = _lon * rad2DegInt;
     }
 
-    float getVD() const {
-        return _vD;
+    float getVN() const {
+        return _vN;
     }
 
     float getVE() const {
-        return sin(getYaw()) * getGroundSpeed();
+        return _vE;
     }
 
-    float getGroundSpeed() const {
-        return _groundSpeed;
+    float getVD() const {
+        return _vD;
     }
 
     int32_t getLat_degInt() const {
@@ -103,10 +114,6 @@ public:
         return _lon_degInt;
     }
 
-    float getVN() const {
-        return cos(getYaw()) * getGroundSpeed();
-    }
-
     float getPitch() const {
         return _pitch;
     }
@@ -131,20 +138,71 @@ public:
         return _yawRate;
     }
 
+    float getWindDirection() const {
+        return _windDirection;
+    }
+
+    float getCourseOverGround() const {
+        return atan2(getVE(),getVN());
+    }
+
+    float getSpeedOverGround() const {
+        return sqrt(getVN()*getVN()+getVE()*getVE());
+    }
+
+    float getXAccel() const {
+        return _xAccel;
+    }
+
+    float getYAccel() const {
+        return _yAccel;
+    }
+
+    float getZAccel() const {
+        return _zAccel;
+    }
+
     void setAirSpeed(float airSpeed) {
-        _airSpeed = airSpeed;
+        // assumes wind constant and rescale navigation speed
+        float vScale = (1 + airSpeed/getAirSpeed());
+        float vNorm = sqrt(getVN()*getVN()+getVE()*getVE()+getVD()*getVD());
+        _vN *= vScale/vNorm;
+        _vE *= vScale/vNorm;
+        _vD *= vScale/vNorm;
     }
 
     void setAlt_intM(int32_t alt_intM) {
         _alt_intM = alt_intM;
     }
 
+    void setVN(float vN) {
+        _vN = vN;
+    }
+
+    void setVE(float vE) {
+        _vE = vE;
+    }
+
     void setVD(float vD) {
         _vD = vD;
     }
 
+    void setXAcc(float xAccel) {
+        _xAccel = xAccel;
+    }
+
+    void setYAcc(float yAccel) {
+        _yAccel = yAccel;
+    }
+
+    void setZAcc(float zAccel) {
+        _zAccel = zAccel;
+    }
+
     void setGroundSpeed(float groundSpeed) {
-        _groundSpeed = groundSpeed;
+        float cog = getCourseOverGround();
+        _vN = cos(cog)*groundSpeed;
+        _vE = sin(cog)*groundSpeed;
     }
 
     void setLat_degInt(int32_t lat_degInt) {
@@ -180,13 +238,24 @@ public:
     void setYawRate(float yawRate) {
         _yawRate = yawRate;
     }
+
     void setTimeStamp(int32_t timeStamp) {
         _timeStamp = timeStamp;
     }
+
     int32_t getTimeStamp() const {
         return _timeStamp;
     }
 
+    void setWindDirection(float windDirection) {
+        _windDirection = windDirection;
+    }
+
+    void setWindSpeed(float windSpeed) {
+        _windSpeed = windSpeed;
+    }
+
+
 protected:
     AP_HardwareAbstractionLayer * _hal;
 private:
@@ -197,9 +266,15 @@ private:
     float _pitchRate; // rad/s
     float _yaw; // rad
     float _yawRate; // rad/s
-    float _airSpeed; // m/s
-    float _groundSpeed; // m/s
+    // wind assumed to be N-E plane
+    float _windSpeed; // m/s
+    float _windDirection; // m/s
+    float _vN;
+    float _vE;
     float _vD; // m/s
+    float _xAccel;
+    float _yAccel;
+    float _zAccel;
     int32_t _lat_degInt; // deg / 1e7
     int32_t _lon_degInt; // deg / 1e7
     int32_t _alt_intM; // meters / 1e3

libraries/AP_Common/AP_Vector.h

@@ -19,6 +19,7 @@
 #ifndef Vector_H
 #define Vector_H
 
+#include "../FastSerial/BetterStream.h"
 #include <stdlib.h>
 #include <inttypes.h>
 #include <WProgram.h>