Added timer, config file reader, plus a lot of bug fixes

AHRS.cpp

@@ -7,43 +7,66 @@
 
 #include "AHRS.h"
 
-AHRS* AHRS::p_instance_ = NULL; //Initialise pointer
-s_rawData rawData;
-s_calibratedData calibratedData;
+AHRSClass AHRS;
 
-AHRS* AHRS::Instance()
+AHRSClass::AHRSClass()
 {
-    if(!p_instance_)
-        p_instance_ = new AHRS;
 }
 
-AHRS::AHRS()
+AHRSClass::AHRSClass(const AHRSClass& orig)
 {
+    readConfig();
 }
 
-AHRS::AHRS(const AHRS& orig)
+AHRSClass::~AHRSClass()
 {
 }
 
-AHRS::~AHRS()
+void AHRSClass::update()
 {
+    getSensors_();
+    calibrateData_();
+    calcAccelAngles_();
+    filter_();
+}
+
+void AHRSClass::readConfig()
+{
+    zeroPoints_.x = Config.getValueOfKey<int16_t>("zero_x");
+    zeroPoints_.y = Config.getValueOfKey<int16_t>("zero_y");
+    zeroPoints_.z = Config.getValueOfKey<int16_t>("zero_z");
+    zeroPoints_.p = Config.getValueOfKey<int16_t>("zero_p");
+    zeroPoints_.q = Config.getValueOfKey<int16_t>("zero_q");
+    zeroPoints_.r = Config.getValueOfKey<int16_t>("zero_r");
 }
 
 //Collect data from all the sensors. Currently only MPU is collected.
-void AHRS::getSensors(s_rawData* rawData)
+void AHRSClass::getSensors_()
 {
-    MPU6050Interface.getSensors(rawData);
+    MPU6050Interface.getSensors(&rawData_);
 }
 
-void AHRS::calibrateData(s_rawData* rawData, s_calibratedData* calibratedData) //Need to add scales and zero points + any required inversions
+void AHRSClass::calibrateData_() //Need to add scales and zero points + any required inversions
 {
-    calibratedData->x = (rawData->x - 0) * 0;
-    calibratedData->y = (rawData->y - 0) * 0;
-    calibratedData->z = (rawData->z - 0) * 0;
-    calibratedData->temp = (rawData->temp - 0) * 0;
-    calibratedData->p = (rawData->p - 0) * 0;
-    calibratedData->q = (rawData->q - 0) * 0;
-    calibratedData->r = (rawData->r - 0) * 0;
+    calibratedData.x = (rawData_.x - zeroPoints_.x) * (9.81 / 2048.0);
+    calibratedData.y = (rawData_.y - zeroPoints_.y) * (9.81 / 2048.0);
+    calibratedData.z = (rawData_.z - zeroPoints_.z) * (9.81 / 2048.0);
+    calibratedData.temp = (rawData_.temp + 521) / 340.0;
+    calibratedData.p = (rawData_.p - zeroPoints_.p) / 16.4;
+    calibratedData.q = -(rawData_.q - zeroPoints_.q) / 16.4;
+    calibratedData.r = (rawData_.r - zeroPoints_.r) / 16.4;
 }
-
 
+void AHRSClass::calcAccelAngles_()
+{
+    accelAngles.phi = (180/pi)*atan2(calibratedData.y,sqrt(pow(calibratedData.x,2) + pow(calibratedData.z,2)));
+    accelAngles.psi = (180/pi)*atan2(calibratedData.x,sqrt(pow(calibratedData.y,2) + pow(calibratedData.z,2)));
+}
+
+void AHRSClass::filter_()
+{
+    #define a 0.99
+    orientation.phi = (orientation.phi + calibratedData.p * Timer.dt)*a + accelAngles.phi*(1-a);
+    orientation.psi = (orientation.psi + calibratedData.q * Timer.dt)*a + accelAngles.psi*(1-a);
+    orientation.theta += calibratedData.r * Timer.dt;
+}

AHRS.h

@@ -10,37 +10,53 @@
 
 #include <iostream>
 #include <stdint.h>
+#include <math.h>
 
+#include "Timer.h"
 #include "MPU6050.h"
+#include "ConfigFile.h"
 
-
-extern s_rawData rawData;
+#define pi 3.14159265358979
 
 struct s_calibratedData
 {
-    float x;
-    float y;
-    float z;
-    float temp;
-    float p;
-    float q;
-    float r;
+    double x;
+    double y;
+    double z;
+    double temp;
+    double p;
+    double q;
+    double r;
 };
-extern s_calibratedData calibratedData;
 
-class AHRS
+struct s_euler
+{
+    double phi;
+    double psi;
+    double theta;
+};
+
+class AHRSClass
 {
 public:
-    static AHRS* Instance();
-    AHRS(const AHRS& orig);
-    virtual ~AHRS();
-    void getSensors(s_rawData* rawData);
-    void calibrateData(s_rawData* rawData, s_calibratedData* calibratedData);
-protected:
-    AHRS();
+    AHRSClass();
+    AHRSClass(const AHRSClass& orig);
+    virtual ~AHRSClass();
+    void update();
+    void readConfig();
+    s_calibratedData calibratedData;
+    s_rawData rawData_;
+    s_euler orientation;
+    s_euler accelAngles;
 private:
-    static AHRS* p_instance_;
+    void getSensors_();
+    void calibrateData_();
+    void filter_();
+    void calcAccelAngles_();
+    s_rawData zeroPoints_;
 };
 
+extern AHRSClass AHRS;
+
 #endif	/* AHRS_H */
 

CommandLineInterface.cpp

@@ -6,6 +6,7 @@
  */
 
 #include "CommandLineInterface.h"
+#include "PICInterface.h"
 
 CLI_class CLI;
 
@@ -16,6 +17,7 @@ CLI_class::CLI_class()
 
 CLI_class::CLI_class(const CLI_class& orig)
 {
+    std::string stringbuf_[6] = {0};
 }
 
 CLI_class::~CLI_class()
@@ -24,37 +26,76 @@ CLI_class::~CLI_class()
 
 void CLI_class::open()
 {
-    for (std::string line; std::cout << "Picopter > " && std::getline(std::cin, line);)
+    while (1)
     {
+        std::string line;
+        std::cout << "Picopter > ";
+        std::cout.flush();
+        std::getline(std::cin, line);
         std::stringstream stream(line);
         int i = 0;
         while (std::getline(stream, stringbuf_[i], ' '))
         {
             i++;
         }
-        
+
         switch (lineMap_[stringbuf_[0]])
         {
             case en_stringNotDefined:
-                std::cout << "Invalid command" << std::endl;
+                std::cout << stringbuf_[0] << " isn't a valid command" << std::endl;
                 break;
+
             case en_openlog:
                 std::cout << "Opening log at " << stringbuf_[1] << std::endl;
                 LogMan.open(stringbuf_[1].c_str());
                 break;
+
             case en_writelog:
                 std::cout << "Writing " << stringbuf_[1] << " to log" << std::endl;
                 Log << stringbuf_[1];
                 break;
+
             case en_starttimer:
                 Timer.start();
                 break;
+
+            case en_readconfig:
+                std::cout << Config.getValueOfKey<std::string> (stringbuf_[1]) << std::endl;
+                break;
+
+            case en_dumpsensors:
+                AHRS.update();
+                std::cout << AHRS.calibratedData.x << "\t" << AHRS.calibratedData.y << "\t" << AHRS.calibratedData.z << "\t" << AHRS.calibratedData.p << "\t" << AHRS.calibratedData.q << "\t" << AHRS.calibratedData.r << "\t" << AHRS.calibratedData.temp << std::endl;
+                break;
+
+            case en_dumprawsensors:
+                AHRS.update();
+                std::cout << AHRS.rawData_.x << "\t" << AHRS.rawData_.y << "\t" << AHRS.rawData_.z << "\t" << AHRS.rawData_.p << "\t" << AHRS.rawData_.q << "\t" << AHRS.rawData_.r << std::endl;
+                break;
+
+            case en_dumprx:
+                PICInterface.getRX();
+                std::cout << PICInterface.rxWidths.pitch << ", " << PICInterface.rxWidths.roll << ", " << PICInterface.rxWidths.throttle << ", " << PICInterface.rxWidths.yaw << ", " << PICInterface.rxWidths.sw1 << ", " << PICInterface.rxWidths.sw2 << ", " << std::endl;
+                break;
+
+            case en_resetmpu:
+                MPU6050Interface.initialise();
+                std::cout << "mpu reset" << std::endl;
+                break;
+
+	    case en_readregister:
+		unsigned char buf[32];
+		I2CInterface.readRegister(static_cast<unsigned char>(atoi(stringbuf_[1].c_str())), static_cast<unsigned char>(atoi(stringbuf_[2].c_str())), buf, static_cast<unsigned char>(atoi(stringbuf_[3].c_str())));
+		std::cout << buf << std::endl;
+		break;
+
+
             case en_exit:
                 exit(1);
                 break;
 
             default:
-                std::cout << "Invalid command" << std::endl;
+                std::cout << stringbuf_[0] << " isn't a valid command" << std::endl;
                 break;
         }
     }
@@ -66,5 +107,11 @@ void CLI_class::initialiseMap_()
     lineMap_["openlog"] = en_openlog;
     lineMap_["writelog"] = en_writelog;
     lineMap_["starttimer"] = en_starttimer;
+    lineMap_["readconfig"] = en_readconfig;
+    lineMap_["dr"] = en_dumprawsensors;
+    lineMap_["ds"] = en_dumpsensors;
+    lineMap_["drx"] = en_dumprx;
+    lineMap_["resetmpu"] = en_resetmpu;
+    lineMap_["rr"] = en_readregister;
     lineMap_["exit"] = en_exit;
 }

CommandLineInterface.h

@@ -19,6 +19,7 @@
 
 #include "Logger.h"
 #include "Timer.h"
+#include "ConfigFile.h"
 
 
 enum lineString
@@ -27,6 +28,12 @@ enum lineString
     en_openlog,
     en_writelog,
     en_starttimer,
+    en_readconfig,
+    en_dumpsensors,
+    en_dumprawsensors,
+    en_dumprx,
+    en_resetmpu,
+    en_readregister,
     en_exit
 };