Merge branch 'master' of github.com:arktools/ardupilotone

ArduBoat/BoatGeneric.h

@@ -22,7 +22,6 @@ static const uint8_t heartBeatTimeout = 3;
 #define COMMLINK_CLASS MavlinkComm
 
 // hardware selection
-#define ADC_CLASS AP_ADC_ADS7844
 #define COMPASS_CLASS AP_Compass_HMC5843
 #define BARO_CLASS APM_BMP085_Class
 #define RANGE_FINDER_CLASS AP_RangeFinder_MaxsonarXL

apo/QuadArducopter.h

@@ -34,14 +34,14 @@ static const uint32_t gpsBaud = 38400;
 static const uint32_t hilBaud = 115200;
 
 // optional sensors
-static const bool gpsEnabled = false;
+static const bool gpsEnabled = true;
-static const bool baroEnabled = false;
+static const bool baroEnabled = true;
 static const bool compassEnabled = true;
 static const Matrix3f compassOrientation = AP_COMPASS_COMPONENTS_UP_PINS_FORWARD;
 // compass orientation: See AP_Compass_HMC5843.h for possible values
 
 // battery monitoring
-static const bool batteryMonitorEnabled = false;
+static const bool batteryMonitorEnabled = true;
 static const uint8_t batteryPin = 0;
 static const float batteryVoltageDivRatio = 6;
 static const float batteryMinVolt = 10.0;
@@ -55,7 +55,7 @@ static const bool rangeFinderUpEnabled = false;
 static const bool rangeFinderDownEnabled = false;
 
 // loop rates
-static const float loopRate = 250; // attitude nav
+static const float loopRate = 200; // attitude nav
 static const float loop0Rate = 50; // controller
 static const float loop1Rate = 10; 	// pos nav/ gcs fast
 static const float loop2Rate = 1; 	// gcs slow

libraries/APO/APO.h

@@ -8,6 +8,7 @@
 #ifndef APO_H_
 #define APO_H_
 
+#include <FastSerial.h>
 #include "AP_Autopilot.h"
 #include "AP_Guide.h"
 #include "AP_Controller.h"
@@ -20,6 +21,7 @@
 #include "AP_ArmingMechanism.h"
 #include "AP_CommLink.h"
 #include "AP_Var_keys.h"
+#include "DcmNavigator.h"
 #include "constants.h"
 
 #endif /* APO_H_ */

libraries/APO/APO_DefaultSetup.h

@@ -16,7 +16,7 @@ void setup() {
 
     using namespace apo;
 
-    AP_Var::load_all();
+    Wire.begin();
 
     // Declare all parts of the system
     AP_Navigator * navigator = NULL;
@@ -36,23 +36,6 @@ void setup() {
     hal->debug = &Serial;
     hal->debug->println_P(PSTR("initializing debug line"));
 
-    // isr
-    hal->isr_registry = new Arduino_Mega_ISR_Registry;
-
-    // initialize radio
-    hal->radio = new APM_RC_APM1;
-    hal->radio->Init(hal->isr_registry);
-
-    // initialize scheduler
-    hal->scheduler = new AP_TimerProcess;
-    hal->scheduler->init(hal->isr_registry);
-
-
-    // initialize the adc
-    hal->debug->println_P(PSTR("initializing adc"));
-    hal->adc = new ADC_CLASS;
-    hal->adc->Init(hal->scheduler);
-
     /*
      * Sensor initialization
      */
@@ -74,11 +57,14 @@ void setup() {
         if (baroEnabled) {
             hal->debug->println_P(PSTR("initializing baro"));
             hal->baro = new BARO_CLASS;
-            hal->baro->Init();
+            if (board==BOARD_ARDUPILOTMEGA_2) {
+                hal->baro->Init(0,true);
+            } else {
+                hal->baro->Init(0,false);
+            }
         }
 
         if (compassEnabled) {
-            Wire.begin();
             hal->debug->println_P(PSTR("initializing compass"));
             hal->compass = new COMPASS_CLASS;
             hal->compass->set_orientation(compassOrientation);
@@ -155,7 +141,11 @@ void setup() {
     /*
      * Select guidance, navigation, control algorithms
      */
-    navigator = new NAVIGATOR_CLASS(hal);
+    if (hal->getMode() == MODE_LIVE) {
+        navigator = new NAVIGATOR_CLASS(hal,k_nav);
+    } else {
+        navigator = new AP_Navigator(hal);
+    }
     guide = new GUIDE_CLASS(navigator, hal, velCmd, xt, xtLim);
     controller = new CONTROLLER_CLASS(navigator,guide,hal);
 

libraries/APO/AP_Autopilot.cpp

@@ -49,6 +49,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
      * Look for valid initial state
      */
     while (_navigator) {
+
         // letc gcs known we are alive
         hal->gcs->sendMessage(MAVLINK_MSG_ID_HEARTBEAT);
         hal->gcs->sendMessage(MAVLINK_MSG_ID_SYS_STATUS);
@@ -98,10 +99,11 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
                           AP_MavlinkCommand::home.getLat()*rad2Deg,
                           AP_MavlinkCommand::home.getLon()*rad2Deg,
                           AP_MavlinkCommand::home.getCommand());
+
     guide->setCurrentIndex(0);
     controller->setMode(MAV_MODE_LOCKED);
     controller->setState(MAV_STATE_STANDBY);
-
+  
     /*
      * Attach loops, stacking for priority
      */
@@ -117,7 +119,7 @@ AP_Autopilot::AP_Autopilot(AP_Navigator * navigator, AP_Guide * guide,
 
 void AP_Autopilot::callback(void * data) {
     AP_Autopilot * apo = (AP_Autopilot *) data;
-    //apo->hal()->debug->println_P(PSTR("callback"));
+    //apo->getHal()->debug->println_P(PSTR("callback"));
 
     /*
      * ahrs update
@@ -178,6 +180,7 @@ void AP_Autopilot::callback1(void * data) {
      */
     if (apo->getHal()->gcs) {
         apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_ATTITUDE);
+        apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GLOBAL_POSITION);
     }
 
     /*
@@ -218,11 +221,10 @@ void AP_Autopilot::callback2(void * data) {
      */
     if (apo->getHal()->gcs) {
         // send messages
-        apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW);
+        //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_GPS_RAW_INT);
+        //apo->getHal()->gcs->sendMessage(MAVLINK_MSG_ID_LOCAL_POSITION);
         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

@@ -116,21 +116,21 @@ void MavlinkComm::sendMessage(uint8_t id, uint32_t param) {
 
     case MAVLINK_MSG_ID_GPS_RAW: {
         mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
-                                 _navigator->getLat() * rad2Deg,
+                                 _hal->gps->latitude/1.0e7,
-                                 _navigator->getLon() * rad2Deg, _navigator->getAlt(), 0, 0,
+                                 _hal->gps->longitude/1.0e7, _hal->gps->altitude/100.0, 0, 0,
-                                 _navigator->getGroundSpeed()*1000.0,
+                                 _hal->gps->ground_speed/100.0,
-                                 _navigator->getYaw() * rad2Deg);
+                                 _hal->gps->ground_course/10.0);
         break;
     }
 
-    /*
+    case MAVLINK_MSG_ID_GPS_RAW_INT: {
-     case MAVLINK_MSG_ID_GPS_RAW_INT: {
+        mavlink_msg_gps_raw_send(_channel, timeStamp, _hal->gps->status(),
-     mavlink_msg_gps_raw_int_send(_channel,timeStamp,_hal->gps->status(),
+                                 _hal->gps->latitude,
-     _navigator->getLat_degInt(), _navigator->getLon_degInt(),_navigator->getAlt_intM(), 0,0,
+                                 _hal->gps->longitude, _hal->gps->altitude*10.0, 0, 0,
-     _navigator->getGroundSpeed(),_navigator->getYaw()*rad2Deg);
+                                 _hal->gps->ground_speed/100.0,
-     break;
+                                 _hal->gps->ground_course/10.0);
+        break;
      }
-     */
 
     case MAVLINK_MSG_ID_SCALED_IMU: {
         int16_t xmag, ymag, zmag;

libraries/APO/AP_HardwareAbstractionLayer.cpp

@@ -5,5 +5,95 @@
  *      Author: jgoppert
  */
 
+
+// Libraries
+#include <FastSerial.h>
+#include <Arduino_Mega_ISR_Registry.h>
 #include "AP_HardwareAbstractionLayer.h"
+#include <AP_ADC.h>
+#include <APM_RC.h>
+#include <AP_AnalogSource.h>
+#include <AP_PeriodicProcess.h>
+
+namespace apo {
+
+// 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::AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle) :
+    adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), 
+    radio(), rc(), gcs(),
+    hil(), debug(), load(), _mode(mode),
+    _board(board), _vehicle(vehicle) {
+
+
+    AP_Var::load_all();
+
+    /*
+     * Board specific hardware initialization
+     */
+    if (board == BOARD_ARDUPILOTMEGA_1280) {
+        slideSwitchPin = 40;
+        pushButtonPin = 41;
+        aLedPin = 37;
+        bLedPin = 36;
+        cLedPin = 35;
+        eepromMaxAddr = 1024;
+        pinMode(aLedPin, OUTPUT); //  extra led
+        pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
+        pinMode(cLedPin, OUTPUT); //  gps led
+        pinMode(slideSwitchPin, INPUT);
+        pinMode(pushButtonPin, INPUT);
+        DDRL |= B00000100; // set port L, pint 2 to output for the relay
+        adc = new AP_ADC_ADS7844;
+        radio = new APM_RC_APM1;
+    } else if (board == BOARD_ARDUPILOTMEGA_2560) {
+        slideSwitchPin = 40;
+        pushButtonPin = 41;
+        aLedPin = 37;
+        bLedPin = 36;
+        cLedPin = 35;
+        eepromMaxAddr = 2048;
+        pinMode(aLedPin, OUTPUT); //  extra led
+        pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
+        pinMode(cLedPin, OUTPUT); //  gps led
+        pinMode(slideSwitchPin, INPUT);
+        pinMode(pushButtonPin, INPUT);
+        DDRL |= B00000100; // set port L, pint 2 to output for the relay
+        adc = new AP_ADC_ADS7844;
+        radio = new APM_RC_APM1;
+    } else if (board == BOARD_ARDUPILOTMEGA_2) {
+        slideSwitchPin = 40;
+        pushButtonPin = 41;
+        aLedPin = 37;
+        bLedPin = 36;
+        cLedPin = 35;
+        eepromMaxAddr = 2048;
+        pinMode(aLedPin, OUTPUT); //  extra led
+        pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
+        pinMode(cLedPin, OUTPUT); //  gps led
+        pinMode(slideSwitchPin, INPUT);
+        pinMode(pushButtonPin, INPUT);
+        DDRL |= B00000100; // set port L, pint 2 to output for the relay
+        /// FIXME adc = new ?
+        adc = new AP_ADC_ADS7844;
+        radio = new APM_RC_APM2;
+    }
+
+    // isr
+    isr_registry = new Arduino_Mega_ISR_Registry;
+
+    // initialize radio
+    radio->Init(isr_registry);
+
+    // initialize scheduler
+    scheduler = new AP_TimerProcess;
+    scheduler->init(isr_registry);
+
+    // initialize the adc
+    adc->Init(scheduler);
+}
+
+} // apo
+
 // vim:ts=4:sw=4:expandtab

libraries/APO/AP_HardwareAbstractionLayer.h

@@ -47,56 +47,7 @@ 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, MAV_TYPE vehicle) :
+    AP_HardwareAbstractionLayer(halMode_t mode, board_t board, MAV_TYPE vehicle);
-        adc(), gps(), baro(), compass(), rangeFinders(), imu(), batteryMonitor(), 
-        radio(), rc(), gcs(),
-        hil(), debug(), load(), _mode(mode),
-        _board(board), _vehicle(vehicle) {
-
-        /*
-         * Board specific hardware initialization
-         */
-        if (board == BOARD_ARDUPILOTMEGA_1280) {
-            slideSwitchPin = 40;
-            pushButtonPin = 41;
-            aLedPin = 37;
-            bLedPin = 36;
-            cLedPin = 35;
-            eepromMaxAddr = 1024;
-            pinMode(aLedPin, OUTPUT); //  extra led
-            pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
-            pinMode(cLedPin, OUTPUT); //  gps led
-            pinMode(slideSwitchPin, INPUT);
-            pinMode(pushButtonPin, INPUT);
-            DDRL |= B00000100; // set port L, pint 2 to output for the relay
-        } else if (board == BOARD_ARDUPILOTMEGA_2560) {
-            slideSwitchPin = 40;
-            pushButtonPin = 41;
-            aLedPin = 37;
-            bLedPin = 36;
-            cLedPin = 35;
-            eepromMaxAddr = 2048;
-            pinMode(aLedPin, OUTPUT); //  extra led
-            pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
-            pinMode(cLedPin, OUTPUT); //  gps led
-            pinMode(slideSwitchPin, INPUT);
-            pinMode(pushButtonPin, INPUT);
-            DDRL |= B00000100; // set port L, pint 2 to output for the relay
-        } else if (board == BOARD_ARDUPILOTMEGA_2) {
-            slideSwitchPin = 40;
-            pushButtonPin = 41;
-            aLedPin = 37;
-            bLedPin = 36;
-            cLedPin = 35;
-            eepromMaxAddr = 2048;
-            pinMode(aLedPin, OUTPUT); //  extra led
-            pinMode(bLedPin, OUTPUT); //  imu ledclass AP_CommLink;
-            pinMode(cLedPin, OUTPUT); //  gps led
-            pinMode(slideSwitchPin, INPUT);
-            pinMode(pushButtonPin, INPUT);
-            DDRL |= B00000100; // set port L, pint 2 to output for the relay
-        }
-    }
 
     /**
      * Sensors

libraries/APO/AP_Navigator.cpp

@@ -6,18 +6,7 @@
  */
 
 #include "AP_Navigator.h"
-#include "../FastSerial/FastSerial.h"
-#include "AP_HardwareAbstractionLayer.h"
-#include "../AP_DCM/AP_DCM.h"
-#include "../AP_Math/AP_Math.h"
-#include "../AP_Compass/AP_Compass.h"
 #include "AP_MavlinkCommand.h"
-#include "AP_Var_keys.h"
-#include "../AP_RangeFinder/AP_RangeFinder.h"
-#include "../AP_IMU/AP_IMU.h"
-#include "../AP_InertialSensor/AP_InertialSensor.h"
-#include "../APM_BMP085/APM_BMP085_hil.h"
-#include "../APM_BMP085/APM_BMP085.h"
 
 namespace apo {
 
@@ -28,8 +17,6 @@ AP_Navigator::AP_Navigator(AP_HardwareAbstractionLayer * hal) :
     _vN(0), _vE(0), _vD(0), _lat_degInt(0),
     _lon_degInt(0), _alt_intM(0) {
 }
-void AP_Navigator::calibrate() {
-}
 float AP_Navigator::getPD() const {
     return AP_MavlinkCommand::home.getPD(getAlt_intM());
 }
@@ -54,153 +41,5 @@ void AP_Navigator::setPN(float _pN) {
     setLon(AP_MavlinkCommand::home.getLon(_pN));
 }
 
-DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal) :
-    AP_Navigator(hal), _dcm(), _imuOffsetAddress(0) {
-
-    // if orientation equal to front, store as front
-    /**
-     * rangeFinder<direction> is assigned values based on orientation which
-     * is specified in ArduPilotOne.pde.
-     */
-    for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) {
-        if (_hal->rangeFinders[i] == NULL)
-            continue;
-        if (_hal->rangeFinders[i]->orientation_x == 0
-                && _hal->rangeFinders[i]->orientation_y == 0
-                && _hal->rangeFinders[i]->orientation_z == 1)
-            _rangeFinderDown = _hal->rangeFinders[i];
-    }
-
-    if (_hal->getMode() == MODE_LIVE) {
-
-        if (_hal->imu) {
-            _dcm = new AP_DCM(_hal->imu, _hal->gps, _hal->compass);
-
-            // tune down dcm
-            _dcm->kp_roll_pitch(0.030000);
-            _dcm->ki_roll_pitch(0.00001278),	// 50 hz I term
-
-                 // tune down compass in dcm
-                 _dcm->kp_yaw(0.08);
-            _dcm->ki_yaw(0);
-        }
-
-        if (_hal->compass) {
-            _dcm->set_compass(_hal->compass);
-        }
-    }
-}
-void DcmNavigator::calibrate() {
-
-    AP_Navigator::calibrate();
-
-    // TODO: handle cold/warm restart
-    if (_hal->imu) {
-        _hal->imu->init(IMU::COLD_START,delay,_hal->scheduler);
-    }
-}
-void DcmNavigator::updateFast(float dt) {
-
-    if (_hal->getMode() != MODE_LIVE)
-        return;
-
-    setTimeStamp(micros()); // if running in live mode, record new time stamp
-
-
-    //_hal->debug->println_P(PSTR("nav loop"));
-
-    /**
-     * The altitued is read off the barometer by implementing the following formula:
-     * altitude (in m) = 44330*(1-(p/po)^(1/5.255)),
-     * where, po is pressure in Pa at sea level (101325 Pa).
-     * See http://www.sparkfun.com/tutorials/253 or type this formula
-     * in a search engine for more information.
-     * altInt contains the altitude in meters.
-     */
-    if (_hal->baro) {
-
-        if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695)
-            setAlt(_rangeFinderDown->distance);
-
-        else {
-            float tmp = (_hal->baro->Press / 101325.0);
-            tmp = pow(tmp, 0.190295);
-            //setAlt(44330 * (1.0 - tmp)); //sets the altitude in meters XXX wrong, baro reads 0 press
-            setAlt(0.0);
-        }
-    }
-
-    // dcm class for attitude
-    if (_dcm) {
-        _dcm->update_DCM_fast();
-        setRoll(_dcm->roll);
-        setPitch(_dcm->pitch);
-        setYaw(_dcm->yaw);
-        setRollRate(_dcm->get_gyro().x);
-        setPitchRate(_dcm->get_gyro().y);
-        setYawRate(_dcm->get_gyro().z);
-
-        /*
-         * accel/gyro debug
-         */
-        /*
-         Vector3f accel = _hal->imu->get_accel();
-         Vector3f gyro = _hal->imu->get_gyro();
-         Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"),
-         accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z);
-         */
-    }
-}
-void DcmNavigator::updateSlow(float dt) {
-    if (_hal->getMode() != MODE_LIVE)
-        return;
-
-    setTimeStamp(micros()); // if running in live mode, record new time stamp
-
-    if (_hal->gps) {
-        _hal->gps->update();
-        updateGpsLight();
-        if (_hal->gps->fix && _hal->gps->new_data) {
-            setLat_degInt(_hal->gps->latitude);
-            setLon_degInt(_hal->gps->longitude);
-            setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm
-            setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s
-        }
-    }
-
-    if (_hal->compass) {
-        _hal->compass->read();
-        _hal->compass->calculate(_dcm->get_dcm_matrix());
-        _hal->compass->null_offsets(_dcm->get_dcm_matrix());
-        //_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading);
-    }
-}
-void DcmNavigator::updateGpsLight(void) {
-    // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
-    // ---------------------------------------------------------------------
-    static bool GPS_light = false;
-    switch (_hal->gps->status()) {
-    case (2):
-        //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
-        break;
-
-    case (1):
-        if (_hal->gps->valid_read == true) {
-            GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
-            if (GPS_light) {
-                digitalWrite(_hal->cLedPin, LOW);
-            } else {
-                digitalWrite(_hal->cLedPin, HIGH);
-            }
-            _hal->gps->valid_read = false;
-        }
-        break;
-
-    default:
-        digitalWrite(_hal->cLedPin, LOW);
-        break;
-    }
-}
-
 } // namespace apo
 // vim:ts=4:sw=4:expandtab

libraries/APO/AP_Navigator.h

@@ -22,10 +22,6 @@
 #include "constants.h"
 #include <inttypes.h>
 
-class RangeFinder;
-class IMU;
-class AP_DCM;
-
 namespace apo {
 
 class AP_HardwareAbstractionLayer;
@@ -34,9 +30,14 @@ class AP_HardwareAbstractionLayer;
 class AP_Navigator {
 public:
     AP_Navigator(AP_HardwareAbstractionLayer * hal);
-    virtual void calibrate();
+
-    virtual void updateFast(float dt) = 0;
+    // note, override these with derived navigator functionality
-    virtual void updateSlow(float dt) = 0;
+    virtual void calibrate() {};
+    virtual void updateFast(float dt) {};
+    virtual void updateSlow(float dt) {};
+
+
+    // accessors
     float getPD() const;
     float getPE() const;
     float getPN() const;
@@ -85,6 +86,7 @@ public:
 
     float getLon() const {
         return _lon_degInt * degInt2Rad;
+
     }
 
     void setLon(float _lon) {
@@ -155,6 +157,7 @@ public:
         return y;
     }
 
+
     float getSpeedOverGround() const {
         return sqrt(getVN()*getVN()+getVE()*getVE());
     }
@@ -264,7 +267,6 @@ public:
         _windSpeed = windSpeed;
     }
 
-
 protected:
     AP_HardwareAbstractionLayer * _hal;
 private:
@@ -289,25 +291,6 @@ private:
     int32_t _alt_intM; // meters / 1e3
 };
 
-class DcmNavigator: public AP_Navigator {
-private:
-    /**
-     * Sensors
-     */
-
-    RangeFinder * _rangeFinderDown;
-    AP_DCM * _dcm;
-    IMU * _imu;
-    uint16_t _imuOffsetAddress;
-
-public:
-    DcmNavigator(AP_HardwareAbstractionLayer * hal);
-    virtual void calibrate();
-    virtual void updateFast(float dt);
-    virtual void updateSlow(float dt);
-    void updateGpsLight(void);
-};
-
 } // namespace apo
 
 #endif // AP_Navigator_H

libraries/APO/AP_Var_keys.h

@@ -8,6 +8,7 @@ enum keys {
     k_cntrl,
     k_guide,
     k_sensorCalib,
+    k_nav,
 
     k_radioChannelsStart=10,
 

libraries/APO/DcmNavigator.cpp

@@ -0,0 +1,220 @@
+/*
+ * DcmNavigator.cpp
+ *
+ *  Created on: Dec 6, 2011
+ *      Author: jgoppert/ wenyaoxie
+ */
+
+#include "../FastSerial/FastSerial.h"
+#include "DcmNavigator.h"
+#include "AP_CommLink.h"
+#include "AP_HardwareAbstractionLayer.h"
+#include "../AP_DCM/AP_DCM.h"
+#include "../AP_Math/AP_Math.h"
+#include "../AP_Compass/AP_Compass.h"
+#include "AP_MavlinkCommand.h"
+#include "AP_Var_keys.h"
+#include "../AP_RangeFinder/RangeFinder.h"
+#include "../AP_IMU/AP_IMU.h"
+#include "../AP_InertialSensor/AP_InertialSensor.h"
+#include "../APM_BMP085/APM_BMP085_hil.h"
+#include "../APM_BMP085/APM_BMP085.h"
+
+namespace apo {
+
+DcmNavigator::DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key, const prog_char_t * name) :
+    AP_Navigator(hal), _imuOffsetAddress(0),
+    _dcm(_hal->imu, _hal->gps, _hal->compass),
+    _rangeFinderDown(),
+    _group(key, name ? : PSTR("NAV_")),
+    _baroLowPass(&_group,1,10,PSTR("BAROLP")),
+    _groundTemperature(&_group,2, 25.0,PSTR("GNDT")), // TODO check temp
+    _groundPressure(&_group,3,0.0,PSTR("GNDP")) {
+
+    // if orientation equal to front, store as front
+    /**
+     * rangeFinder<direction> is assigned values based on orientation which
+     * is specified in ArduPilotOne.pde.
+     */
+    for (uint8_t i = 0; i < _hal-> rangeFinders.getSize(); i++) {
+        if (_hal->rangeFinders[i] == NULL)
+            continue;
+        if (_hal->rangeFinders[i]->orientation_x == 0
+                && _hal->rangeFinders[i]->orientation_y == 0
+                && _hal->rangeFinders[i]->orientation_z == 1)
+            _rangeFinderDown = _hal->rangeFinders[i];
+    }
+
+    // tune down dcm
+    _dcm.kp_roll_pitch(0.030000);
+    _dcm.ki_roll_pitch(0.00001278),	// 50 hz I term
+
+    // tune down compass in dcm
+    _dcm.kp_yaw(0.08);
+    _dcm.ki_yaw(0);
+
+    if (_hal->compass) {
+        _dcm.set_compass(_hal->compass);
+    }
+}
+void DcmNavigator::calibrate() {
+
+    AP_Navigator::calibrate();
+
+    // TODO: handle cold/warm restart
+    if (_hal->imu) {
+        _hal->imu->init(IMU::COLD_START,delay,_hal->scheduler);
+    }
+
+    if (_hal->baro) {
+
+        int flashcount = 0;
+
+        while(_groundPressure == 0){
+            _hal->baro->Read(); 					// Get initial data from absolute pressure sensor
+            _groundPressure = _hal->baro->Press;
+            _groundTemperature = _hal->baro->Temp/10.0;
+            delay(20);
+        }
+
+        for(int i = 0; i < 30; i++){		// We take some readings...
+
+            // set using low pass filters
+            _groundPressure = _groundPressure * 0.9   + _hal->baro->Press * 0.1;
+            _groundTemperature = _groundTemperature * 0.9   + (_hal->baro->Temp/10.0) * 0.1;
+
+            //mavlink_delay(20);
+            delay(20);
+            if(flashcount == 5) {
+                digitalWrite(_hal->cLedPin, LOW);
+                digitalWrite(_hal->aLedPin, HIGH);
+                digitalWrite(_hal->bLedPin, LOW);
+            }
+
+            if(flashcount >= 10) {
+                flashcount = 0;
+                digitalWrite(_hal->cLedPin, LOW);
+                digitalWrite(_hal->aLedPin, HIGH);
+                digitalWrite(_hal->bLedPin, LOW);
+            }
+            flashcount++;
+        }
+        
+        _groundPressure.save();
+        _groundTemperature.save();
+
+        _hal->debug->printf_P(PSTR("ground pressure: %ld ground temperature: %d\n"),_groundPressure.get(), _groundTemperature.get());
+        _hal->gcs->sendText(SEVERITY_LOW, PSTR("barometer calibration complete\n"));
+    }
+}
+
+void DcmNavigator::updateFast(float dt) {
+
+    if (_hal->getMode() != MODE_LIVE)
+        return;
+
+    setTimeStamp(micros()); // if running in live mode, record new time stamp
+
+    // use range finder if attached and close to the ground
+    if (_rangeFinderDown != NULL && _rangeFinderDown->distance <= 695) {
+        setAlt(_rangeFinderDown->distance);
+
+    // otherwise if you have a baro attached, use it
+    } else if (_hal->baro) {
+        /**
+         * The altitued is read off the barometer by implementing the following formula:
+         * altitude (in m) = 44330*(1-(p/po)^(1/5.255)),
+         * where, po is pressure in Pa at sea level (101325 Pa).
+         * See http://www.sparkfun.com/tutorials/253 or type this formula
+         * in a search engine for more information.
+         * altInt contains the altitude in meters.
+         *
+         * pressure input is in pascals
+         * temp input is in deg C *10
+         */
+        _hal->baro->Read();		// Get new data from absolute pressure sensor
+        float reference = 44330 * (1.0 - (pow(_groundPressure.get()/101325.0,0.190295)));
+        setAlt(_baroLowPass.update((44330 * (1.0 - (pow((_hal->baro->Press/101325.0),0.190295)))) - reference,dt));
+        //_hal->debug->printf_P(PSTR("Ground Pressure %f\tAltitude = %f\tGround Temperature = %f\tPress = %ld\tTemp = %d\n"),_groundPressure.get(),getAlt(),_groundTemperature.get(),_hal->baro->Press,_hal->baro->Temp);
+        
+    // last resort, use gps altitude
+    } else if (_hal->gps && _hal->gps->fix) {
+        setAlt_intM(_hal->gps->altitude * 10); // gps in cm, intM in mm
+    }
+
+    // update dcm calculations and navigator data
+    //
+    _dcm.update_DCM_fast();
+    setRoll(_dcm.roll);
+    setPitch(_dcm.pitch);
+    setYaw(_dcm.yaw);
+    setRollRate(_dcm.get_gyro().x);
+    setPitchRate(_dcm.get_gyro().y);
+    setYawRate(_dcm.get_gyro().z);
+    setXAccel(_dcm.get_accel().x);
+    setYAccel(_dcm.get_accel().y);
+    setZAccel(_dcm.get_accel().z);
+
+    /*
+     * accel/gyro debug
+     */
+    /*
+     Vector3f accel = _hal->imu->get_accel();
+     Vector3f gyro = _hal->imu->get_gyro();
+     Serial.printf_P(PSTR("accel: %f %f %f gyro: %f %f %f\n"),
+     accel.x,accel.y,accel.z,gyro.x,gyro.y,gyro.z);
+     */
+}
+
+void DcmNavigator::updateSlow(float dt) {
+    if (_hal->getMode() != MODE_LIVE)
+        return;
+
+    setTimeStamp(micros()); // if running in live mode, record new time stamp
+
+    if (_hal->gps) {
+        _hal->gps->update();
+        updateGpsLight();
+        if (_hal->gps->fix && _hal->gps->new_data) {
+            setLat_degInt(_hal->gps->latitude);
+            setLon_degInt(_hal->gps->longitude);
+            setGroundSpeed(_hal->gps->ground_speed / 100.0); // gps is in cm/s
+        }
+    }
+
+    if (_hal->compass) {
+        _hal->compass->read();
+        _hal->compass->calculate(_dcm.get_dcm_matrix());
+        _hal->compass->null_offsets(_dcm.get_dcm_matrix());
+        //_hal->debug->printf_P(PSTR("heading: %f"), _hal->compass->heading);
+    }
+}
+void DcmNavigator::updateGpsLight(void) {
+    // GPS LED on if we have a fix or Blink GPS LED if we are receiving data
+    // ---------------------------------------------------------------------
+    static bool GPS_light = false;
+    switch (_hal->gps->status()) {
+    case (2):
+        //digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
+        break;
+
+    case (1):
+        if (_hal->gps->valid_read == true) {
+            GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
+            if (GPS_light) {
+                digitalWrite(_hal->cLedPin, LOW);
+            } else {
+                digitalWrite(_hal->cLedPin, HIGH);
+            }
+            _hal->gps->valid_read = false;
+        }
+        break;
+
+    default:
+        digitalWrite(_hal->cLedPin, LOW);
+        break;
+    }
+}
+
+} // namespace apo
+// vim:ts=4:sw=4:expandtab

libraries/APO/DcmNavigator.h

@@ -0,0 +1,51 @@
+/*
+ * DcmNavigator.h
+ * Copyright (C) James Goppert/ Wenyao Xie 2011 james.goppert@gmail.com/ wenyaoxie@gmail.com 
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef DcmNavigator_H
+#define DcmNavigator_H
+
+#include "AP_Navigator.h"
+#include <FastSerial.h>
+#include "AP_ControllerBlock.h"
+#include <AP_DCM.h>
+
+class RangeFinder;
+
+namespace apo {
+
+class DcmNavigator: public AP_Navigator {
+public:
+    DcmNavigator(AP_HardwareAbstractionLayer * hal, const uint16_t key, const prog_char_t * name = NULL);
+    virtual void calibrate();
+    virtual void updateFast(float dt);
+    virtual void updateSlow(float dt);
+    void updateGpsLight(void);
+private:
+    AP_DCM _dcm;
+    AP_Var_group _group;
+    uint16_t _imuOffsetAddress;
+    BlockLowPass _baroLowPass;
+    AP_Float _groundTemperature;
+    AP_Float _groundPressure;
+    RangeFinder * _rangeFinderDown;
+};
+
+} // namespace apo
+
+#endif // DcmNavigator_H
+// vim:ts=4:sw=4:expandtab

libraries/APO/template.h

@@ -24,7 +24,7 @@ namespace apo {
 /// Class description
 class Class {
 public:
-}
+};
 
 } // namespace apo
 

libraries/AP_DCM/AP_DCM.cpp

@@ -173,7 +173,10 @@ void
 AP_DCM::accel_adjust(void)
 {
 	Vector3f veloc, temp;
-	veloc.x = _gps->ground_speed / 100;		// We are working with acceleration in m/s^2 units
+
+	if (_gps) {
+		veloc.x = _gps->ground_speed / 100;		// We are working with acceleration in m/s^2 units
+	}
 
 	// We are working with a modified version of equation 26 as our IMU object reports acceleration in the positive axis direction as positive
 
@@ -301,7 +304,7 @@ AP_DCM::drift_correction(void)
 		// We make the gyro YAW drift correction based on compass magnetic heading
 		error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);	// Equation 23, Calculating YAW error
 
-	} else {
+	} else if (_gps) {
 
 		// Use GPS Ground course to correct yaw gyro drift
 		if (_gps->ground_speed >= SPEEDFILT) {