Serial out

First working example of sending data back to the remote. For now we send some dummy data.

.travis.yml

@@ -1,5 +1,6 @@
 sudo: required
 language: c++
+
   
 before_install:
   - sudo add-apt-repository -y ppa:team-gcc-arm-embedded/ppa
@@ -9,7 +10,7 @@ before_install:
 addons:
     apt:
         sources:
-            
+
         packages:
             - lib32bz2-1.0
             - lib32ncurses5
@@ -18,5 +19,6 @@ addons:
 install:
     - sudo pip install -r mbed-os/requirements.txt
 
+
 script:
     make

.vscode/settings.json

@@ -3,6 +3,23 @@
     //"C_Cpp.intelliSenseEngine": "Default" // Does not work with "Go to definition" etc.
     "C_Cpp.intelliSenseEngine": "Tag Parser",
     "files.associations": {
-        "cmath": "cpp"
+        "cmath": "cpp",
+        "queue": "cpp",
+        "__bit_reference": "cpp",
+        "__functional_base": "cpp",
+        "atomic": "cpp",
+        "deque": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "limits": "cpp",
+        "memory": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "vector": "cpp",
+        "string": "cpp",
+        "initializer_list": "cpp",
+        "string_view": "cpp",
+        "utility": "cpp",
+        "algorithm": "cpp"
     }
 }

Makefile

@@ -645,6 +645,12 @@ OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
 OBJECTS += ./src/drivers/stepper.o
 OBJECTS += ./src/drivers/servo.o
+OBJECTS += ./src/control/ImuFusion.o
+OBJECTS += ./src/control/PID.o
+OBJECTS += ./src/control/MadgwickAHRS.o
+OBJECTS += ./src/math/Utilities.o
+OBJECTS += ./src/serialization/RCProtocol.o
+
 
 
 INCLUDE_PATHS += -I../

NodeMCU/OSCWifiBridge/OSCWifiBridge.ino

@@ -0,0 +1,322 @@
+#ifdef ESP8266
+#include <ESP8266WiFi.h>
+#else
+#include <WiFi.h>
+#endif
+#include <WiFiUdp.h>
+#include <OSCMessage.h>
+#include <OSCBundle.h>
+#include <OSCData.h>
+
+char ssid[] = "ROBOT";
+char pass[] = "myrobotisawesome";
+
+const long sendInterval = 30; // in ms
+
+WiFiUDP Udp;
+
+// remote IP (not needed for receive)
+// Will be updated once the first package is received
+// Gateway IP normally is 192.168.4.1
+IPAddress outIp(192,168,4,2);
+
+const unsigned int outPort = 9999;
+const unsigned int localPort = 8888;
+
+OSCErrorCode error;
+
+// LED to reflect the active status of the robot
+// Start "on"
+unsigned int ledState = HIGH; 
+
+typedef struct PacketOut {
+  int16_t MagicWordLow;
+  int16_t MagicWordHigh;
+  
+  int16_t Throttle;
+  int16_t Steering;
+  
+  int16_t Kp;
+  int16_t Ki;
+  int16_t Kd;
+  
+  bool Enabled;
+
+  uint8_t checksum;
+  
+} __attribute__ ((__packed__));
+
+struct PacketIn {
+  
+  int16_t BatteryLevel;
+  
+  int16_t Mode;
+  
+  uint8_t checksum;
+  
+} __attribute__ ((__packed__));
+
+const int bufferOutSize = sizeof(PacketOut); // Size of Packet
+uint8_t bufferOut[bufferOutSize];
+PacketOut* pPacketOut = (PacketOut*)bufferOut;
+
+const int bufferInSize = sizeof(PacketIn);
+uint8_t bufferIn[bufferInSize];
+PacketIn* pPacketIn = (PacketIn*)bufferIn;
+
+void calcChecksum()
+{
+  uint16_t sum = 0;
+  
+  for (int i = 4; i < (bufferOutSize - 1); i++)
+  {
+    sum += bufferOut[i];
+  }
+
+  pPacketOut->checksum = (uint8_t)(sum & 0xFF);
+  //Serial.print("sum: ");
+  //Serial.println(sum);
+  //Serial.print("checksum: ");
+  //Serial.println(pPacket->checksum);
+}
+
+void setup() {
+  WiFi.softAP(ssid, pass);
+  IPAddress apip = WiFi.softAPIP();
+  pinMode(LED_BUILTIN, OUTPUT);
+  //Serial.begin(115200);
+  Serial.begin(250000);
+  Serial.println();
+  Serial.print("AP IP: ");
+  Serial.println(apip);
+  Udp.begin(localPort);
+
+  // Initialize the packet
+  pPacketOut->MagicWordLow = 0x0DED;
+  pPacketOut->MagicWordHigh = 0x0DEC;
+
+  pPacketOut->Throttle = 1;
+  pPacketOut->Steering = 2;
+  pPacketOut->Kp = 3;
+  pPacketOut->Ki = 4;
+  pPacketOut->Kd = 5;
+  pPacketOut->Enabled = false;
+  pPacketOut->checksum = 0;
+}
+
+void sendPacket()
+{
+  calcChecksum();
+  Serial.write(bufferOut, bufferOutSize);
+}
+
+void OSCMsgReceive()
+{
+  OSCMessage msgIN;
+  int size;
+  if ((size = Udp.parsePacket()) > 0)
+  {
+    while(size--)
+    {
+      msgIN.fill(Udp.read());
+    }
+    if (!msgIN.hasError())
+    {
+      msgIN.route("/Robot/Enable",funcEnabled);
+      msgIN.route("/Fader/Throttle",funcThrottle);
+      msgIN.route("/Fader/Steering",funcSteering);
+      msgIN.route("/Gain/Kp",funcKp);
+      msgIN.route("/Gain/Ki",funcKi);
+      msgIN.route("/Gain/Kd",funcKd);
+    }
+  }
+}
+
+void sendUdp(OSCMessage* pMessage)
+{
+  IPAddress remoteIp = Udp.remoteIP();
+  
+  /* Address is exchanged for 0.0.0.0 so when
+   *  forwarding from the other board the 
+   *  package does not reach the phone
+   *  TODO: Check for address other than 0.0.0.0
+   *  before saving
+  if (remoteIp != outIp)
+  {
+    outIp = remoteIp;
+    Serial.print("New source address: ");
+    Serial.println(outIp);
+  }
+  */
+  
+  //send osc message back to controll object in TouchOSC
+  //Local feedback is turned off in the TouchOSC interface.
+  //The button is turned on in TouchOSC interface whe the conrol receives this message.
+  Udp.beginPacket(outIp, outPort);
+  pMessage->send(Udp); // send the bytes
+  Udp.endPacket(); // mark the end of the OSC Packet
+  pMessage->empty(); // free space occupied by message
+}
+
+void funcEnabled(OSCMessage &msg, int addrOffset){
+  ledState = !(boolean) msg.getFloat(0);
+  OSCMessage msgOUT("/Robot/Enable");
+
+  digitalWrite(BUILTIN_LED, ledState);
+
+  msgOUT.add(!ledState);
+  sendUdp(&msgOUT);
+  pPacketOut->Enabled = !ledState;
+
+  //Serial.println( ledState ? "Robot disabled" : "Robot enabled");
+}
+
+void funcThrottle(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Fader/Throttle");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->Throttle = value;
+}
+
+void funcSteering(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Fader/Steering");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->Steering = value;
+}
+
+void funcKp(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Kp");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->Kp = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/KpOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+void funcKi(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Ki");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->Ki = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/KiOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+void funcKd(OSCMessage &msg, int addrOffset ){
+
+  int value = msg.getFloat(0);
+  OSCMessage msgOUT("/Gain/Kd");
+  msgOUT.add(value);
+  sendUdp(&msgOUT);
+  pPacketOut->Kd = value;
+
+  // Redo this for label
+  OSCMessage msgLABEL("/Gain/KdOut");
+  msgLABEL.add(value);
+  sendUdp(&msgLABEL);
+}
+
+// Send OSC packets updating touchOSC on the mobile
+void remotePresentation()
+{
+  OSCMessage msgLABEL("/battery");
+  msgLABEL.add(pPacketIn->BatteryLevel);
+  sendUdp(&msgLABEL);
+
+  // TODO: Add the desired data to be forwarded from pPacketIn
+}
+
+bool parsePacket()
+{
+  static uint8_t magicWord[4];
+  static uint32_t* pMagicWord = (uint32_t*)magicWord;
+  
+  static uint8_t byteCounter = 0;
+  static const uint8_t payloadSize = sizeof(PacketIn);
+  static uint8_t payload[payloadSize];
+  static PacketIn* pPacketInWorker = (PacketIn*)payload;
+
+  uint8_t newByte = Serial.read();
+  
+  // We have the header correct
+  if (*pMagicWord == 0xDEC0DED)
+  {
+    payload[byteCounter++] = newByte;
+
+    static uint16_t sum = 0;
+  
+    if (byteCounter >= payloadSize)
+    {
+        *pMagicWord = 0; // Unvalidate the magic word
+        byteCounter = 0; // Reset the read counter
+
+        uint8_t checksum = sum & 0xFF;
+        sum = 0;
+
+        if (checksum == pPacketInWorker->checksum)
+        {
+          // If robot enabled we blink 
+          // when receiving data
+          if (pPacketOut->Enabled)
+          {
+            ledState = !ledState;
+            digitalWrite(BUILTIN_LED, ledState);
+          }
+
+          *pPacketIn = *((PacketIn*)payload);
+
+          remotePresentation();
+          
+          return true; // new package available
+        }
+    }
+    sum +=newByte; // placed here not to include the checksum
+  }
+  else
+  {
+    *pMagicWord = *pMagicWord >> 8 | newByte << 8*3; // Works on revo
+    //Serial.print("Byte: ");
+    //Serial.println(newByte);
+  }
+  
+  return false;
+}
+
+void loop() {
+  OSCMsgReceive();
+
+  static unsigned long previousMillis = 0;
+
+  long currentMillis = millis();
+
+  if (currentMillis - previousMillis >= sendInterval)
+  {
+    // If something can be printed there is enough time
+    // between iterations
+    //Serial.println("Sending data");
+    previousMillis = currentMillis;
+    sendPacket(); // Send on UART
+  }
+  
+  if (Serial.available())
+  {
+    parsePacket();
+  }
+  
+}
+

main.cpp

@@ -1,50 +1,241 @@
 #include "mbed.h"
 
+// STD
 #include <cmath>
 
+// Mbed
 #include "mbed_events.h"
 
+// target definitions
 #include "src/targets/revo_f4/pins.h"
+// Mmath
+#include "src/math/Utilities.h"
+// Drivers
 #include "src/drivers/MPU6000.h"
 #include "src/drivers/stepper.h"
 #include "src/drivers/servo.h"
+// Control
 #include "src/control/lpf.h"
+#include "src/control/PID.h"
+#include "src/control/ImuFusion.h"
+#include "src/control/MadgwickAHRS.h"
+// Serialization
+#include "src/serialization/RCProtocol.h"
 
-#ifndef PI
-    #define PI 3.14159265358979f
-#endif
+#define WHEEL_SIZE 0.09f
 
+using namespace targets::revo_f4;
 using namespace drivers;
 using namespace control;
+using namespace math;
+using namespace serialization;
 
 EventQueue queue;
 
-Ticker queueReader;
+// Serial port (Servo Outputs)
+RawSerial serial(PA_2, PA_3, 250000);
+RCProtocol RC;
 
+// Pool to send Remote Control packages between threads
+MemoryPool<RCProtocol::PacketIn, 16> mpool;
+Queue<RCProtocol::PacketIn, 16> RCQueue;
 
 // MPU setup
 SPI spi(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
 mpu6000_spi imu(spi,PA_4);   //define the mpu6000 object
 
-PwmOut led1(D4); //PB_5
+PwmOut ledBlue(D4);
 DigitalOut ledOrg(D5);
 
-Stepper motor1(PC_9, PC_7, PC_8);
-Stepper motor2(PB_15, PB_14, PC_6);
+// Stepper motorL(PC_9, PB_1, PC_8);
+Stepper motorL(PC_9, PC_7, PC_8);
+Stepper motorR(PB_15, PB_14, PC_6);
 
 Servo servo(PA_0);
 
 // Interrupt pin from Gyro to MCU
-InterruptIn gyroINT(PC_4);
+InterruptIn gyroISR(PC_4);
+// Madwick filter
+Madgwick madgwick;
+// Timer to calculate dT
+Timer timer;
 
-// Draft of some control function
-void controlFunc(int a)
+// A PI controller for throttle control
+controllerPI throttleControl(35.0, 3.0, 20, 60); // 35.0, 3.0, 20, 60
+
+// A PID for angle control
+controllerPID angleControl(100.0f, 200.0f, 0.5f, 4000, 2000); // 100.0f, 200.0f, 0.5f, 4000, 2000
+
+// The control function to balance the robot
+// Ran at high prio and triggered as an event by the gyro Interrupt pin
+void runControl()
 {
-    static incrementalLPF filter;
-    double accY = 1;
-    accY = imu.read_acc(1);
-    double filtered = filter.filter(accY);
-    motor1.setSpeed(360.0*2*filtered);   
+    static ImuFusion imuFusion(&imu);
+    static float controlOutput(0.0f);
+
+    static RCProtocol::PacketIn remote;
+    osEvent evt = RCQueue.get(0);
+    if (evt.status == osEventMessage) {
+        RCProtocol::PacketIn* pPacketIn = (RCProtocol::PacketIn*)evt.value.p;
+        remote = *pPacketIn;
+        mpool.free(pPacketIn);
+
+        throttleControl.setGainScaling(remote.Ki, remote.Kd);
+        //throttleControl.setGainScaling(0, 0);
+
+        //angleControl.setGainScaling(remote.Kp, remote.Ki, remote.Kd);
+        angleControl.setGainScaling(remote.Kp, 0, -900);
+
+        servo.setPosition((float)remote.Throttle/1000.0f);
+    }
+    
+
+    // Calculate dT in sec
+    float dT = timer.read_us()/(float)10e6;
+    timer.reset();
+
+    // Retrieve IMU angle
+    madgwick.updateIMU(imu.read_rot(0),
+                       imu.read_rot(1),
+                       imu.read_rot(2),
+                       imu.read_acc(0),
+                       imu.read_acc(1), 
+                       imu.read_acc(2));
+    float angleX = madgwick.getPitch();
+
+    // Reset anything left in the IMU FIFO queue
+    imu.fifo_reset();
+
+    // If the robot is above this angle we turn off motors
+    static bool disabled = false;
+    if (!remote.Enabled || (abs(angleX) > (float)50.0f && !disabled))
+    {
+        controlOutput = 0.0f; // rinse integral
+        disabled = true;
+        motorL.disable();
+        motorR.disable();
+    }
+    else if (remote.Enabled && abs(angleX) < (float)10.0f && disabled)
+    {
+        disabled = false;
+        motorL.enable();
+        motorR.enable();
+    }
+
+    if (disabled) 
+    { 
+        throttleControl.flushIntegral(); 
+        angleControl.flushIntegral();
+    }
+
+    /* --------------------------
+    Calculate estimated groundspeed 
+    of the robot in m/s
+    -------------------------- */
+    // Static variables
+    static float angleXOld = 0;
+    static float motorAngularVelocity = 0;
+    static float motorAngularVelocityOld = 0;
+    static incrementalLPF filteredGroundSpeed;
+
+    if (disabled) 
+    {
+        angleXOld = 0;
+        motorAngularVelocity = 0;
+        motorAngularVelocityOld = 0;
+        filteredGroundSpeed.clear();
+    }
+    
+    // angular velocity - This way we get use of the madgwick filter
+    float angularVelocity = (angleX - angleXOld) * dT;
+    angleXOld = angleX;
+
+    // Motor Speed - Convert from deg/s to m/s
+    motorAngularVelocityOld = motorAngularVelocity;
+    motorAngularVelocity = ((motorL.getSpeed() + motorR.getSpeed())/2);
+
+    float estimatedGroundSpeed = 
+        (motorAngularVelocityOld - angularVelocity)/360.0f*PI*WHEEL_SIZE;
+    estimatedGroundSpeed = filteredGroundSpeed.filter(estimatedGroundSpeed);
+
+    /* --------------------------
+    Calculate the setpoint for 
+    the main control (control the angle)
+    through this throttle control loop (PI)
+    -------------------------- */
+    float throttle = remote.Throttle*0.001f;
+
+    if (remote.Throttle < 50 && remote.Throttle > -50) {throttle = 0;}
+    
+    // TODO: Figure out why we have the wrong sign here. I also had to reverse motors
+    // For the angle control loop. Is the madgwick filter outputting the wrong sign?
+    float angleSP = -throttleControl.run(dT, estimatedGroundSpeed, throttle);
+
+    /* --------------------------
+    The last control loop. Angle (PD)
+    -------------------------- */
+    if (!disabled)
+    {
+        controlOutput = angleControl.run(dT, angleX, angleSP);
+
+        controlOutput = constrain(controlOutput, 3000.0f);
+    }
+
+    /* --------------------------
+     Lastly the steering is added
+     straight on the output.
+    -------------------------- */
+    float steering = remote.Steering*0.5;
+    if (remote.Steering < 50 && remote.Steering > -50) {steering = 0;}
+    motorL.setSpeed(controlOutput - steering);
+    motorR.setSpeed(controlOutput + steering);
+
+    // Blink LED at 1hz
+    static int i = 0;
+    if (++i > (disabled ? 25 : 100))
+    {
+        i = 0;
+        ledOrg = !ledOrg;
+    }
+}
+
+void serialISR()
+{
+    // Receive
+    while (serial.readable())
+    {
+        bool newPackage = RC.readByte(serial.getc());
+
+        if (newPackage)
+        {
+            RCProtocol::PacketIn* pPacketIn = mpool.alloc();
+            *pPacketIn = RC.readPacket();
+            RCQueue.put(pPacketIn);
+        }
+    }
+}
+
+void serialWrite()
+{
+    RCProtocol::PacketOut packetOut;
+    packetOut.BatteryLevel = 123;
+    packetOut.Mode = 213;
+    RC.setOutput(packetOut);
+    bool done;
+    
+    while (true)
+    {
+        while (!serial.writeable());
+        
+        serial.putc(RC.writeByte(done));
+
+        // If we successfully complete
+        // writing a packet we sleep
+        if (done)
+        {
+            Thread::wait(500);
+        }
+    }
 }
 
 // This context just pulses the blue LED
@@ -60,85 +251,84 @@ void pulseLedContext()
             // We cannot set pulsewidth or period due to
             // sharing timer with motor outputs. "write"
             // instead sets the dutycycle in range 0-1
-            led1.write(out);
+            ledBlue.write(out);
             Thread::wait(10);
         }
     }
 }
 
-void readQueue()
-{
-    //queue.dispatch(0);
-}
-
-// Interrupt driven function
-// At the moment its running at 100hz
-void ISRGYRO() 
-{
-    static int i = 0;
-    i++;
-    if (i > 100)
-    {
-        i = 0;
-        ledOrg = !ledOrg;
-    }
-    // Report that there is new data to read
-    // TODO: This hack has to be done in a better way
-    queue.call(controlFunc, 0);
-}
-
-int i = 0;
-
 // main() runs in its own thread
 int main() {
 
+    // MPU startup at 100hz
+    if(imu.init(10,BITS_DLPF_CFG_188HZ)){
+        //printf("\nCouldn't initialize MPU6000 via SPI!");
+    }  
+
+    wait(0.1);
+    // Set IMU scale
+    int accScale = imu.set_acc_scale(BITS_FS_16G);
+    wait(0.1);
+    int gyroScale = imu.set_gyro_scale(BITS_FS_2000DPS);
+    wait(0.1);
+
+    // Calibrate and Trim acc & gyro
+    bool calibrationResult = imu.resetOffset_gyro();
+    imu.calib_acc(0);
+    calibrationResult = imu.resetOffset_acc();
+
+    // Setup Madwick filter
+    madgwick.begin(100);
+
+    /*-------------- Visible start sequence ------------*/
     // Start sweeping the arm
-    servo.sweep(0.0, 1, 2);
-
-    Thread ledPulseThread;
-    ledPulseThread.start(callback(&pulseLedContext));
-
-    // Register a event queue reader at set interval
-    queueReader.attach(&readQueue, 0.001);
+    //servo.sweep(0.0, 1, 2);
 
     // Enable motor controllers (Will power the motors with no movement)
-    motor1.enable();
-    motor2.enable();
+    // motorL.enable();
+    // motorR.enable();
+    motorL.disable();
+    motorR.disable();
+    motorL.setDirection(-1);
+    motorR.setDirection(1);
 
-    // Set motor 2 to a fixed speed 
-    // (To test that timers don't interfeer with each other)
-    motor2.setSpeed(90.0);
+    // Servo Nod to tell us that we are done
+    //servo.nod();
 
-    // Function to attach to gyro interrupt
-    gyroINT.rise(&ISRGYRO);
+    //servo.setPosition(-0.2);
 
-    // MPU startup
-    if(imu.init(10,BITS_DLPF_CFG_188HZ)){
-        printf("\nCouldn't initialize MPU6000 via SPI!");
-    }  
+    /*********************** Start all threads last **********************/
+    
+    // Start the pulsing blue led
+    ledBlue.period_ms(10); // Any period. This will be overwritten by motors
+    Thread ledPulseThread;
+    ledPulseThread.start(callback(&pulseLedContext));
+    Thread::wait(100);
+
+    // Serial in / out
+    serial.set_dma_usage_rx(DMA_USAGE_ALWAYS);
+    serial.attach(&serialISR);
+    Thread::wait(100);
+
+    // ISR won't be called if the serial is not emptied first.
+    serialISR();
+
+    // Serial Write thread
+    Thread serialWriteThread;
+    serialWriteThread.start(callback(&serialWrite));
 
     // Enable/Activate the Gyro interrupt
     imu.enableInterrupt();
 
-    // Read some test values
-    int wmi = imu.whoami();
-    int scale = imu.set_acc_scale(BITS_FS_16G);
+    // Start the timer used by the control loop
+    timer.start(); // Used to calc dT
 
-    Thread::wait(1000);
+    // Create realtime eventhandler for control loop
+    Thread eventThread(osPriorityHigh);
+    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
 
-    servo.nod();
+    // Attach gyro interrupt to add a control event
+    gyroISR.rise(queue.event(&runControl));
 
-    Thread::wait(1000);
-
-    servo.sweep(1.0, 1.5, 3);
-
-    servo.setPosition(-0.5);
-
-    while (true) 
-    {
-        // HACK!!!
-        // Dispatch any content of the event queue.
-        queue.dispatch(0);
-        Thread::wait(1);
-    }
+    wait(osWaitForever);
 }

src/control/ImuFusion.cpp

@@ -0,0 +1,32 @@
+#include "src/control/ImuFusion.h"
+
+namespace control {
+
+ImuFusion::ImuFusion(mpu6000_spi* pImu)
+: m_pImu(pImu)
+, m_angle(0)
+{
+}
+
+// TODO: Implement for all axis
+float ImuFusion::getAngle(float dT)
+{
+    int axis = 0;
+
+    float rot = m_pImu->read_rot(axis);
+
+    m_angle += dT*rot;
+
+    float ratio = 0.99; 
+    //float ratio = 0.95f;
+    //float ratio = 0.9996;
+
+    float rawAngle = m_pImu->read_acc_deg(axis); // conversion from G to Deg
+
+    m_angle = (m_angle * ratio) + (rawAngle * ((float)1.0f-ratio));
+
+    return m_angle;
+}
+   
+
+}

src/control/ImuFusion.h

@@ -0,0 +1,23 @@
+#include "src/drivers/MPU6000.h"
+
+namespace control {
+
+class ImuFusion
+{
+
+public:
+ 
+    ImuFusion(mpu6000_spi* pImu);
+
+    float getAngle(float dT);
+
+private:
+
+    mpu6000_spi* m_pImu;
+
+    float m_angle;
+
+
+};
+
+}

src/control/MadgwickAHRS.cpp

@@ -0,0 +1,250 @@
+//=============================================================================================
+// MadgwickAHRS.c
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+// 19/02/2012	SOH Madgwick	Magnetometer measurement is normalised
+//
+//=============================================================================================
+
+//-------------------------------------------------------------------------------------------
+// Header files
+
+#include "MadgwickAHRS.h"
+#include <math.h>
+
+//-------------------------------------------------------------------------------------------
+// Definitions
+
+#define sampleFreqDef   512.0f          // sample frequency in Hz
+#define betaDef         0.1f            // 2 * proportional gain
+
+
+//============================================================================================
+// Functions
+
+//-------------------------------------------------------------------------------------------
+// AHRS algorithm update
+
+Madgwick::Madgwick() {
+	beta = betaDef;
+	q0 = 1.0f;
+	q1 = 0.0f;
+	q2 = 0.0f;
+	q3 = 0.0f;
+	invSampleFreq = 1.0f / sampleFreqDef;
+	anglesComputed = 0;
+}
+
+void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
+	float recipNorm;
+	float s0, s1, s2, s3;
+	float qDot1, qDot2, qDot3, qDot4;
+	float hx, hy;
+	float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
+
+	// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
+	if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+		updateIMU(gx, gy, gz, ax, ay, az);
+		return;
+	}
+
+	// Convert gyroscope degrees/sec to radians/sec
+	gx *= 0.0174533f;
+	gy *= 0.0174533f;
+	gz *= 0.0174533f;
+
+	// Rate of change of quaternion from gyroscope
+	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+		// Normalise accelerometer measurement
+		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;
+
+		// Normalise magnetometer measurement
+		recipNorm = invSqrt(mx * mx + my * my + mz * mz);
+		mx *= recipNorm;
+		my *= recipNorm;
+		mz *= recipNorm;
+
+		// Auxiliary variables to avoid repeated arithmetic
+		_2q0mx = 2.0f * q0 * mx;
+		_2q0my = 2.0f * q0 * my;
+		_2q0mz = 2.0f * q0 * mz;
+		_2q1mx = 2.0f * q1 * mx;
+		_2q0 = 2.0f * q0;
+		_2q1 = 2.0f * q1;
+		_2q2 = 2.0f * q2;
+		_2q3 = 2.0f * q3;
+		_2q0q2 = 2.0f * q0 * q2;
+		_2q2q3 = 2.0f * q2 * q3;
+		q0q0 = q0 * q0;
+		q0q1 = q0 * q1;
+		q0q2 = q0 * q2;
+		q0q3 = q0 * q3;
+		q1q1 = q1 * q1;
+		q1q2 = q1 * q2;
+		q1q3 = q1 * q3;
+		q2q2 = q2 * q2;
+		q2q3 = q2 * q3;
+		q3q3 = q3 * q3;
+
+		// Reference direction of Earth's magnetic field
+		hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
+		hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
+		_2bx = sqrtf(hx * hx + hy * hy);
+		_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
+		_4bx = 2.0f * _2bx;
+		_4bz = 2.0f * _2bz;
+
+		// Gradient decent algorithm corrective step
+		s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+		s0 *= recipNorm;
+		s1 *= recipNorm;
+		s2 *= recipNorm;
+		s3 *= recipNorm;
+
+		// Apply feedback step
+		qDot1 -= beta * s0;
+		qDot2 -= beta * s1;
+		qDot3 -= beta * s2;
+		qDot4 -= beta * s3;
+	}
+
+	// Integrate rate of change of quaternion to yield quaternion
+	q0 += qDot1 * invSampleFreq;
+	q1 += qDot2 * invSampleFreq;
+	q2 += qDot3 * invSampleFreq;
+	q3 += qDot4 * invSampleFreq;
+
+	// Normalise quaternion
+	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+	anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// IMU algorithm update
+
+void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
+	float recipNorm;
+	float s0, s1, s2, s3;
+	float qDot1, qDot2, qDot3, qDot4;
+	float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+	// Convert gyroscope degrees/sec to radians/sec
+	gx *= 0.0174533f;
+	gy *= 0.0174533f;
+	gz *= 0.0174533f;
+
+	// Rate of change of quaternion from gyroscope
+	qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+	qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+	qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+	qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
+	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+
+		// Normalise accelerometer measurement
+		recipNorm = invSqrt(ax * ax + ay * ay + az * az);
+		ax *= recipNorm;
+		ay *= recipNorm;
+		az *= recipNorm;
+
+		// Auxiliary variables to avoid repeated arithmetic
+		_2q0 = 2.0f * q0;
+		_2q1 = 2.0f * q1;
+		_2q2 = 2.0f * q2;
+		_2q3 = 2.0f * q3;
+		_4q0 = 4.0f * q0;
+		_4q1 = 4.0f * q1;
+		_4q2 = 4.0f * q2;
+		_8q1 = 8.0f * q1;
+		_8q2 = 8.0f * q2;
+		q0q0 = q0 * q0;
+		q1q1 = q1 * q1;
+		q2q2 = q2 * q2;
+		q3q3 = q3 * q3;
+
+		// Gradient decent algorithm corrective step
+		s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+		s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+		s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+		s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
+		recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
+		s0 *= recipNorm;
+		s1 *= recipNorm;
+		s2 *= recipNorm;
+		s3 *= recipNorm;
+
+		// Apply feedback step
+		qDot1 -= beta * s0;
+		qDot2 -= beta * s1;
+		qDot3 -= beta * s2;
+		qDot4 -= beta * s3;
+	}
+
+	// Integrate rate of change of quaternion to yield quaternion
+	q0 += qDot1 * invSampleFreq;
+	q1 += qDot2 * invSampleFreq;
+	q2 += qDot3 * invSampleFreq;
+	q3 += qDot4 * invSampleFreq;
+
+	// Normalise quaternion
+	recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+	q0 *= recipNorm;
+	q1 *= recipNorm;
+	q2 *= recipNorm;
+	q3 *= recipNorm;
+	anglesComputed = 0;
+}
+
+//-------------------------------------------------------------------------------------------
+// Fast inverse square-root
+// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
+
+float Madgwick::invSqrt(float x) {
+	float halfx = 0.5f * x;
+	float y = x;
+	long i = *(long*)&y;
+	i = 0x5f3759df - (i>>1);
+	y = *(float*)&i;
+	y = y * (1.5f - (halfx * y * y));
+	y = y * (1.5f - (halfx * y * y));
+	return y;
+}
+
+//-------------------------------------------------------------------------------------------
+
+void Madgwick::computeAngles()
+{
+	roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
+	pitch = asinf(-2.0f * (q1*q3 - q0*q2));
+	yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
+	anglesComputed = 1;
+}

src/control/MadgwickAHRS.h

@@ -0,0 +1,71 @@
+//=============================================================================================
+// MadgwickAHRS.h
+//=============================================================================================
+//
+// Implementation of Madgwick's IMU and AHRS algorithms.
+// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
+//
+// From the x-io website "Open-source resources available on this website are
+// provided under the GNU General Public Licence unless an alternative licence
+// is provided in source."
+//
+// Date			Author          Notes
+// 29/09/2011	SOH Madgwick    Initial release
+// 02/10/2011	SOH Madgwick	Optimised for reduced CPU load
+//
+//=============================================================================================
+#ifndef MadgwickAHRS_h
+#define MadgwickAHRS_h
+#include <math.h>
+
+//--------------------------------------------------------------------------------------------
+// Variable declaration
+class Madgwick{
+private:
+    static float invSqrt(float x);
+    float beta;				// algorithm gain
+    float q0;
+    float q1;
+    float q2;
+    float q3;	// quaternion of sensor frame relative to auxiliary frame
+    float invSampleFreq;
+    float roll;
+    float pitch;
+    float yaw;
+    char anglesComputed;
+    void computeAngles();
+
+//-------------------------------------------------------------------------------------------
+// Function declarations
+public:
+    Madgwick(void);
+    void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
+    void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
+    void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
+    
+    float getRoll() {
+        if (!anglesComputed) computeAngles();
+        return roll * 57.29578f;
+    }
+    float getPitch() {
+        if (!anglesComputed) computeAngles();
+        return pitch * 57.29578f;
+    }
+    float getYaw() {
+        if (!anglesComputed) computeAngles();
+        return yaw * 57.29578f + 180.0f;
+    }
+    float getRollRadians() {
+        if (!anglesComputed) computeAngles();
+        return roll;
+    }
+    float getPitchRadians() {
+        if (!anglesComputed) computeAngles();
+        return pitch;
+    }
+    float getYawRadians() {
+        if (!anglesComputed) computeAngles();
+        return yaw;
+    }
+};
+#endif

src/control/PID.cpp

@@ -0,0 +1,226 @@
+#include "src/control/PID.h"
+#include "src/math/Utilities.h"
+
+
+using namespace math;
+
+namespace control {
+
+/*----------------------------------- P Controller -----------------------------------*/
+controllerP::controllerP(float kP)
+: m_kP(kP)
+{
+}
+
+float controllerP::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    // output
+    return m_kP*error;
+}
+
+/*----------------------------------- PD Controller ----------------------------------*/
+controllerPD::controllerPD(float kP, float kD, float saturation)
+: m_kP(kP)
+, m_kD(kD)
+, m_saturation(saturation)
+, m_lastError(0)
+, m_pt1FilterApply4(50.0f)
+{
+}
+
+float controllerPD::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kP*error);
+
+    float dTerm((dT != 0.0f) ? m_kD*((error-m_lastError)/dT) : 0.0f);
+
+    // Store error for next iteration
+    m_lastError = error;
+
+    dTerm = m_pt1FilterApply4.filter(dTerm, dT);
+
+    float output(constrain(pTerm + dTerm, m_saturation));
+
+    return output;
+}
+
+/*----------------------------------- Alternative PD Controller ----------------------------------*/
+controllerPD2::controllerPD2(float kP, float kD, float saturation)
+: m_kP(kP)
+, m_kD(kD)
+, m_saturation(saturation)
+, m_inputTMinus1(0.0f)
+, m_inputTMinus2(0.0f)
+, m_setPointTMinus1(0.0f)
+{
+}
+
+float controllerPD2::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kP*error);
+
+    float dTerm = + (m_kD*(setPoint - m_setPointTMinus1) - m_kD*(input - m_inputTMinus2))/dT;
+    
+    m_inputTMinus2 = m_inputTMinus1;
+    m_inputTMinus1 = input;
+    m_setPointTMinus1 = setPoint;
+    return(pTerm + dTerm);
+}
+
+/*----------------------------------- PI Controller ----------------------------------*/
+controllerPI::controllerPI(float kP, float kI, float saturation, float iTermSaturation)
+: m_kP(kP)
+, m_kI(kI)
+, m_saturation(saturation)
+, m_iTermSaturation(iTermSaturation)
+, m_integrator(0)
+{
+}
+
+float controllerPI::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kPScale * m_kP * error);
+
+    // Accumulate to the integrator
+    m_integrator += error * dT;
+    m_integrator = constrain(m_integrator, m_iTermSaturation);
+
+    float iTerm = m_integrator * m_kIScale * m_kI;
+
+    float output(constrain(pTerm + iTerm, m_saturation));
+
+    return output;
+}
+
+void controllerPI::setGainScaling(float kPScale, float kIScale)
+{
+    static float scaleFactor(1.0f / 1000.0f);
+    
+    m_kPScale = (1000.0f + kPScale) * scaleFactor;
+    m_kIScale = (1000.0f + kIScale) * scaleFactor;
+}
+
+void controllerPI::flushIntegral()
+{
+    m_integrator = 0;
+}
+
+/*----------------------------------- PID Controller ---------------------------------*/
+
+controllerPID::controllerPID(float kP, float kI, float kD, float saturation, float iTermSaturation)
+: m_kP(kP)
+, m_kI(kI)
+, m_kD(kD)
+, m_kPScale(0.0f)
+, m_kIScale(0.0f)
+, m_kDScale(0.0f)
+, m_saturation(saturation)
+, m_lastError(0.0f)
+, m_iTermSaturation(iTermSaturation)
+, m_integrator(0.0f)
+, m_pt1FilterApply4(20.0f)
+{
+}
+
+float controllerPID::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kPScale * m_kP * error);
+
+    // Accumulate to the integrator
+    m_integrator += error * dT;
+    m_integrator = constrain(m_integrator, m_iTermSaturation);
+
+    float iTerm = m_integrator * m_kIScale * m_kI;
+
+    float dTerm((dT != 0.0f) ? m_kDScale * m_kD * ((error-m_lastError)/dT) : 0.0f);
+
+    // Store error for next iteration
+    m_lastError = error;
+
+    dTerm = m_pt1FilterApply4.filter(dTerm, dT);
+
+    float output(constrain(pTerm + iTerm + dTerm, m_saturation));
+
+    return output;
+}
+
+void controllerPID::setGainScaling(float kPScale, float kIScale, float kDScale)
+{
+    static float scaleFactor(1.0f / 1000.0f);
+    
+    m_kPScale = (1000.0f + kPScale) * scaleFactor;
+    m_kIScale = (1000.0f + kIScale) * scaleFactor;
+    m_kDScale = (1000.0f + kDScale) * scaleFactor;
+}
+
+void controllerPID::flushIntegral()
+{
+    m_integrator = 0;
+}
+
+/*----------------------------------- PID Controller ---------------------------------*/
+
+controllerPID2::controllerPID2(float kP, float kI, float kD, float saturation, float iTermSaturation)
+: m_kP(kP)
+, m_kI(kI)
+, m_kD(kD)
+, m_kPScale(0.0f)
+, m_kIScale(0.0f)
+, m_kDScale(0.0f)
+, m_saturation(saturation)
+, m_iTermSaturation(iTermSaturation)
+, m_integrator(0.0f)
+, m_inputTMinus1(0.0f)
+, m_inputTMinus2(0.0f)
+, m_setPointTMinus1(0.0f)
+{
+}
+
+float controllerPID2::run(float dT, float input, float setPoint)
+{
+    float error(setPoint-input);
+
+    float pTerm(m_kPScale * m_kP * error);
+
+    // Accumulate to the integrator
+    m_integrator += error * dT;
+    m_integrator = constrain(m_integrator, m_iTermSaturation);
+
+    float iTerm = m_integrator * m_kIScale * m_kI;
+
+    float dTerm = + (m_kD*m_kDScale*((setPoint - m_setPointTMinus1) - (input - m_inputTMinus2)))/dT;
+    
+    m_inputTMinus2 = m_inputTMinus1;
+    m_inputTMinus1 = input;
+    m_setPointTMinus1 = setPoint;
+
+    float output(constrain(pTerm + iTerm + dTerm, m_saturation));
+
+    return output;
+}
+
+void controllerPID2::setGainScaling(float kPScale, float kIScale, float kDScale)
+{
+    static float scaleFactor(1.0f / 1000.0f);
+    
+    m_kPScale = (1000.0f + kPScale) * scaleFactor;
+    m_kIScale = (1000.0f + kIScale) * scaleFactor;
+    m_kDScale = (1000.0f + kDScale) * scaleFactor;
+}
+
+void controllerPID2::flushIntegral()
+{
+    m_integrator = 0;
+}
+
+}

src/control/PID.h

@@ -0,0 +1,114 @@
+#include "src/control/lpf.h"
+
+namespace control {
+
+class controllerP
+{
+public:
+    controllerP(float kP);
+
+    float run(float dT, float input, float setPoint);
+
+private:
+    float m_kP;
+
+};
+
+class controllerPD
+{
+public:
+    controllerPD(float kP, float kD, float saturation);
+
+    float run(float dT, float input, float setPoint);
+
+private:
+    float m_kP;
+    float m_kD;
+    float m_saturation;
+    float m_lastError;
+    pt1FilterApply4 m_pt1FilterApply4;
+};
+
+class controllerPD2
+{
+public:
+    controllerPD2(float kP, float kD, float saturation);
+
+    float run(float dT, float input, float setPoint);
+
+private:
+    float m_kP;
+    float m_kD;
+    float m_saturation;
+    float m_inputTMinus1;
+    float m_inputTMinus2;
+    float m_setPointTMinus1;
+};
+
+class controllerPI
+{
+public:
+    controllerPI(float kP, float kI, float saturation, float iTermSaturation);
+
+    float run(float dT, float input, float setPoint);
+
+    void setGainScaling(float kPScale, float kIScale);
+
+    void flushIntegral();
+
+private:
+    float m_kP, m_kI;
+    float m_kPScale, m_kIScale;
+    float m_saturation;
+    float m_iTermSaturation;
+    float m_integrator;
+};
+
+class controllerPID
+{
+public:
+    controllerPID(float kP, float kI, float kD, float saturation, float iTermSaturation);
+
+    float run(float dT, float input, float setPoint);
+
+    //! Input values in range [-1000, 1000] will result in scales 
+    //! from [0-2].
+    void setGainScaling(float kPScale, float kIScale, float kDScale);
+
+    void flushIntegral();
+
+private:
+    float m_kP, m_kI, m_kD;
+    float m_kPScale, m_kIScale, m_kDScale;
+    float m_saturation;
+    float m_lastError;
+    float m_iTermSaturation;
+    float m_integrator;
+    pt1FilterApply4 m_pt1FilterApply4;
+};
+
+class controllerPID2
+{
+public:
+    controllerPID2(float kP, float kI, float kD, float saturation, float iTermSaturation);
+
+    float run(float dT, float input, float setPoint);
+
+    //! Input values in range [-1000, 1000] will result in scales 
+    //! from [0-2].
+    void setGainScaling(float kPScale, float kIScale, float kDScale);
+
+    void flushIntegral();
+
+private:
+    float m_kP, m_kI, m_kD;
+    float m_kPScale, m_kIScale, m_kDScale;
+    float m_saturation;
+    float m_iTermSaturation;
+    float m_integrator;
+    float m_inputTMinus1;
+    float m_inputTMinus2;
+    float m_setPointTMinus1;
+};
+
+}

src/control/lpf.cpp

@@ -1,17 +1,44 @@
 #include "src/control/lpf.h"
+#include "src/math/Utilities.h"
+
+using namespace math;
 
 namespace control {
 
-
 incrementalLPF::incrementalLPF()
 : m_filtered(0)
 {
 }
 
-double incrementalLPF::filter(double latestValue)
+float incrementalLPF::filter(float latestValue)
 {
-    m_filtered = m_filtered*0.95 + latestValue*0.05;
+    m_filtered = m_filtered*0.95f + latestValue*0.05f;
     return m_filtered;
 }
 
+void incrementalLPF::clear()
+{
+    m_filtered = 0;
+}
+
+pt1FilterApply4::pt1FilterApply4(float freqCut)
+: m_freqCut(freqCut)
+, m_RC(1.0f / (2.0f * (float)PI * m_freqCut))
+, m_filtered(0.0f)
+{
+}
+
+float pt1FilterApply4::filter(float input, float dT)
+{
+    if (dT != 0.0f && m_RC + dT != 0.0f)
+    {
+        float gain(dT / (m_RC + dT));
+
+        m_filtered += gain * (input - m_filtered);
+    }
+
+	return m_filtered;
+}
+
+
 } // namespace control

src/control/lpf.h

@@ -9,11 +9,31 @@ public:
 
     incrementalLPF();
 
-    double filter(double latestValue);
+    float filter(float latestValue);
+
+    void clear();
 
 private:
 
-    double m_filtered;
+    float m_filtered;
 };
 
+
+class pt1FilterApply4
+{
+
+public:
+
+    pt1FilterApply4(float freqCut);
+
+    float filter(float dT, float latestValue);
+
+private:
+
+    float m_freqCut;
+    float m_RC;
+    float m_filtered;
+};
+
+
 } // namespace control

src/drivers/MPU6000.cpp

@@ -4,8 +4,20 @@ www.xene.it
  
 #include <mbed.h>
 #include "MPU6000.h"
+#include <math.h>
+#include "src/math/Utilities.h"
  
-mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs) : spi(_spi), cs(_cs) {}
+mpu6000_spi::mpu6000_spi(SPI& _spi, PinName _cs)
+: spi(_spi)
+, cs(_cs) 
+{
+    _gyro_offset[0] = 0;
+    _gyro_offset[1] = 0;
+    _gyro_offset[2] = 0;
+    _acc_offset[0] = 0;
+    _acc_offset[1] = 0;
+    _acc_offset[2] = 0;
+}
  
 
 bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
@@ -33,6 +45,11 @@ bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
     response=spi.write(MPUREG_USER_CTRL);
     response=spi.write(BIT_I2C_IF_DIS);
     deselect();
+    // DISABLE STANDBY MODE
+    select();
+    response=spi.write(MPUREG_PWR_MGMT_2);
+    response=spi.write(0x00);
+    deselect();
     //WHO AM I?
     select();
     response=spi.write(MPUREG_WHOAMI|READ_FLAG);
@@ -54,6 +71,8 @@ bool mpu6000_spi::init(int sample_rate_div,int low_pass_filter){
     response=spi.write(MPUREG_INT_ENABLE);
     response=spi.write(0x00);
     deselect();
+
+    return false; // TODO
 }
 
 int mpu6000_spi::enableInterrupt()
@@ -183,7 +202,22 @@ float mpu6000_spi::read_acc(int axis){
     data=(float)bit_data;
     data=data/acc_divider;
     deselect();
-    return data;
+    return data - _acc_offset[axis];;
+}
+
+float mpu6000_spi::read_acc_deg(int axis)
+{
+    float g(read_acc(axis));
+    
+    if (g > 1.0f)
+    {
+        g = 1.0f;
+    }
+    else if (g < -1.0f)
+    {
+        g = -1.0f;
+    }
+    return asin(g)/PI*180;
 }
  
 float mpu6000_spi::read_rot(int axis){
@@ -208,7 +242,7 @@ float mpu6000_spi::read_rot(int axis){
     data=(float)bit_data;
     data=data/gyro_divider;
     deselect();
-    return data;
+    return data - _gyro_offset[axis];
 }
  
 float mpu6000_spi::read_temp(){
@@ -226,6 +260,60 @@ float mpu6000_spi::read_temp(){
     return data;
 }
  
+bool mpu6000_spi::resetOffset_gyro(){
+    wait_ms(20);
+
+    bool result(true);
+
+    float validThreshold(5.0);
+    
+    for (int axisIt = 0; axisIt <= 2 ; axisIt++)
+    {
+        for (int samples = 0; samples <= 5; samples++)
+        {
+            Thread::wait(20);
+            float abc = read_rot(axisIt);
+            _gyro_offset[axisIt] += abc;
+        }
+        _gyro_offset[axisIt] /= 6;
+
+        if (abs(_gyro_offset[axisIt]) > validThreshold)
+        {
+            // Calibration failed
+            result = false;
+            _gyro_offset[axisIt] = 0.0;
+        }
+    }
+    return result;
+}
+
+bool mpu6000_spi::resetOffset_acc(){
+    wait_ms(20);
+
+    bool result(true);
+
+    float validThreshold(5.0);
+    
+    for (int axisIt = 0; axisIt <= 2 ; axisIt++)
+    {
+        for (int samples = 0; samples <= 5; samples++)
+        {
+            Thread::wait(20);
+            float abc = read_acc(axisIt);
+            _acc_offset[axisIt] += abc;
+        }
+        _acc_offset[axisIt] /= 6;
+
+        if (abs(_acc_offset[axisIt]) > validThreshold)
+        {
+            // Calibration failed
+            result = false;
+            _acc_offset[axisIt] = 0.0;
+        }
+    }
+    return result;
+}
+
 int mpu6000_spi::calib_acc(int axis){
     uint8_t responseH,responseL,calib_data;
     int temp_scale;
@@ -282,3 +370,13 @@ void mpu6000_spi::deselect() {
     //Set CS high to stop transmission (restarts conversion)
     cs = 1;
 }
+
+void mpu6000_spi::fifo_reset()
+{
+    unsigned int response;
+    //FIFO RESET
+    select();
+    response=spi.write(MPUREG_USER_CTRL);
+    response=spi.write(BIT_FIFO_RESET);
+    deselect();
+}

src/drivers/MPU6000.h

@@ -83,6 +83,8 @@ class mpu6000_spi
     returns the value in Gs
     -----------------------------------------------------------------------------------------------*/
     float read_acc(int axis);
+
+    float read_acc_deg(int axis);
     
     /*-----------------------------------------------------------------------------------------------
                                     READ GYROSCOPE
@@ -118,6 +120,24 @@ class mpu6000_spi
     -----------------------------------------------------------------------------------------------*/
     unsigned int set_acc_scale(int scale);
 
+
+
+    /*-----------------------------------------------------------------------------------------------
+                                    CALIBRATE GYRO OFFSET
+    usage: call this function to reset what the gyro sees as still.
+    returns true if successfull. The board cannot be in movement
+    during this process
+    -----------------------------------------------------------------------------------------------*/
+    bool resetOffset_gyro();
+
+    /*-----------------------------------------------------------------------------------------------
+                                    CALIBRATE ACC OFFSET
+    usage: call this function to reset what the acc sees as 0 deg.
+    returns true if successfull. The board cannot be in movement
+    during this process. Should be straight (in desired 0 position)
+    -----------------------------------------------------------------------------------------------*/
+    bool resetOffset_acc();
+
     /*-----------------------------------------------------------------------------------------------
                                     READ ACCELEROMETER CALIBRATION
     usage: call this function to read accelerometer data. Axis represents selected axis:
@@ -154,6 +174,8 @@ class mpu6000_spi
     returns the I2C address (104)
     -----------------------------------------------------------------------------------------------*/
     unsigned int whoami();
+
+    void fifo_reset();
     
     float acc_divider;
     float gyro_divider;
@@ -163,6 +185,8 @@ class mpu6000_spi
     PinName _SO_pin;
     PinName _SCK_pin;
     float _error;
+    float _gyro_offset[3];
+    float _acc_offset[3];
 };
  
 #endif
@@ -254,6 +278,10 @@ class mpu6000_spi
 #define BIT_INT_ANYRD_2CLEAR        0x10
 #define BIT_RAW_RDY_EN              0x01
 #define BIT_I2C_IF_DIS              0x10
+
+#define BIT_FIFO_EN                 0x40
+#define BIT_FIFO_RESET              0x04
+
  
 #define READ_FLAG                   0x80
  

src/drivers/stepper.cpp

@@ -1,4 +1,7 @@
 #include "src/drivers/stepper.h"
+#include "src/math/Utilities.h"
+
+using namespace math;
 
 namespace drivers {
 
@@ -8,13 +11,21 @@ Stepper::Stepper(PinName stepPin,
 : m_step(stepPin)
 , m_dir(dirPin)
 , m_en(enPin)
+, m_accelerationLimitOn(true)
+, m_accelerationLimit(80.0f)
 , m_stepsPerRevolution(200)
 , m_microStepResolution(8)
-, m_currentPeriod()
+, m_currentPeriod(0)
+, m_configuredDirection(1)
+, m_lastDirection(1)
+, m_latestSpeed(0.0f)
 {
-    m_step.pulsewidth_us(1);
+    m_step.period_us(1000000);
+    m_step.pulsewidth_us(5.0f);
 
+    // Start controller deactivated
     m_en.write(1);
+    m_dir.write(m_configuredDirection);
 }
 
 void Stepper::init()
@@ -42,45 +53,77 @@ bool Stepper::isEnabled()
 
 void Stepper::setDirection(int dir)
 {
-    m_dir.write(dir);
+    m_lastDirection = -1; // Deem invalid
+    m_configuredDirection = dir;
+    m_dir.write(dir == 1 ? 1 : 0);
 }
 
 int Stepper::getDirection()
 {
-    return m_dir.read();
+    return m_configuredDirection;
 }
 
-void Stepper::setSpeed(const double& DPS)
+float Stepper::limitAcceleration(float DPS)
 {
-    if (DPS == 0)
-    {
-        disable();
-        return;
-    }
-    else
-    {
-        enable();
-    }
+    float delta = DPS - m_latestSpeed;
 
-    double revPerSecond = abs(DPS)/360.0;
+    if (abs(delta) > m_accelerationLimit)
+    {
+        return (delta > 0) ? 
+            m_latestSpeed + m_accelerationLimit : 
+            m_latestSpeed - m_accelerationLimit;
+    }
+    return DPS;
+}
 
-    double stepsPerSecond = 
+void Stepper::setSpeed(const float& DPS)
+{
+    m_latestSpeed = limitAcceleration(DPS);
+    
+    float revPerSecond = abs(m_latestSpeed)/(float)360.0;
+    revPerSecond = constrain(revPerSecond, 100.0f);
+
+    float stepsPerSecond = 
         m_stepsPerRevolution*m_microStepResolution*revPerSecond;
 
-    double usPerSecond = 1000000;
+    float usPerSecond = 1000000.0f;
 
-    double periodUs = usPerSecond/stepsPerSecond;
+    // Some high value to make motors "stop" | And to avoid division by zero
+    float periodUs = (stepsPerSecond == 0.0) ? 100000.0 : usPerSecond/stepsPerSecond;
     
-    if (periodUs == m_currentPeriod)
+    // Precaution. Don't know how close the period can be to 
+    // the pulsewidth or if it will cause any issues
+    if (periodUs < (float)100.0f)
     {
-        return;
+        periodUs = (float)100.0f;
+    }
+    if (periodUs > (float)100000.0f)
+    {
+        periodUs = (float)100000.0f;
+    }
+
+    // No need to set anything new
+    if (periodUs != m_currentPeriod)
+    {
+        m_step.period_us(periodUs);
+        m_step.pulsewidth_us(5.0f);
     }
     m_currentPeriod = periodUs;
 
-    m_step.period_us(periodUs);
-    m_step.pulsewidth_us(5);
+    int dir = (m_latestSpeed > (float)0.0f) ? 1 : -1;
 
-    m_dir = (DPS > 0) ? 1 : 0;
+    if (m_lastDirection != dir)
+    {
+        // Give dir to motor controller
+        m_dir.write((m_configuredDirection*dir == 1) ? 1 : 0);
+
+        m_lastDirection = dir;
+    }
+}
+
+float Stepper::getSpeed()
+{
+    return m_latestSpeed;
 }
 
 } // namespace drivers

src/drivers/stepper.h

@@ -21,12 +21,17 @@ public:
 
     bool isEnabled();
 
+    // Direction is -1 (backward) or 1 (forward)
     void setDirection(int dir);
 
     int getDirection();
 
+    float limitAcceleration(float DPS);
+
     // Steps per second? / deg per second?
-    void setSpeed(const double& DPS);
+    void setSpeed(const float& DPS);
+
+    float getSpeed();
 
 private:
 
@@ -34,11 +39,21 @@ private:
     
     DigitalOut m_dir, m_en;
 
+    bool m_accelerationLimitOn;
+
+    float m_accelerationLimit;
+
     int m_stepsPerRevolution;
 
     int m_microStepResolution;
 
     int m_currentPeriod;
+
+    int m_configuredDirection;
+
+    int m_lastDirection;
+
+    float m_latestSpeed;
     
 };
 

src/math/Utilities.cpp

@@ -0,0 +1,20 @@
+#include "src/math/Utilities.h"
+
+#include <stdlib.h>
+
+namespace math {
+
+float constrain(float value, float range)
+{
+    if (value < -range)
+    {
+        return -range;
+    }
+    if (value > range)
+    {
+        return range;
+    }
+    return value;
+}
+    
+}

src/math/Utilities.h

@@ -0,0 +1,10 @@
+#ifndef PI
+  #define PI 3.14159265358979f
+#endif
+
+
+namespace math {
+
+float constrain(float value, float range);
+
+}

src/serialization/RCProtocol.cpp

@@ -0,0 +1,153 @@
+#include "RCProtocol.h"
+
+namespace serialization {
+
+namespace {
+
+//! Example package
+void testParser()
+{
+    static RCProtocol RCProt;
+
+    uint8_t dummy[16];
+
+    // Magic word 233573869 / 0xDEC0DED
+    dummy[0] = 0xED; // 11101101
+    dummy[1] = 0x0D; // 00001101
+    dummy[2] = 0xEC; // 11101100
+    dummy[3] = 0x0D; // 1101
+    
+
+    // Throttle int16 1
+    dummy[4] = 0x01;
+    dummy[5] = 0x00;
+
+    // Steering int16 -1
+    dummy[6] = 0xFF;
+    dummy[7] = 0xFF;
+
+    // Kp 1234 
+    dummy[8] = 0xD2;
+    dummy[9] = 0x04;
+
+    // Ki -1234 
+    dummy[10] = 0x2E;
+    dummy[11] = 0xFB;
+
+    // Kd 10000 
+    dummy[12] = 0x10;
+    dummy[13] = 0x27;
+
+    // Bool True
+    dummy[14] = 0x01;
+
+    dummy[15] = 0x00; // !?? calculate
+
+    RCProtocol::PacketIn pkt = *((RCProtocol::PacketIn*)(dummy+4));
+
+    for (int i = 0; i < 16; i++)
+    {
+        bool newPackage = RCProt.readByte(dummy[i]);
+        if (newPackage)
+        {
+            RCProtocol::PacketIn prot = RCProt.readPacket();
+        }
+    }
+}
+
+}
+
+RCProtocol::RCProtocol()
+: m_packetIn()
+, m_packetOut()
+//, m_semaphore(0)
+{
+}
+
+RCProtocol::PacketIn RCProtocol::readPacket()
+{
+    //m_semaphore.wait();
+    return m_packetIn;
+    //m_semaphore.release();
+}
+
+bool RCProtocol::readByte(uint8_t newByte)
+{
+    static uint8_t header[4]; // 4 bytes for our header
+    static uint32_t* pHeader32 = (uint32_t*)header;
+
+    static const int payloadSize = sizeof(RCProtocol::PacketIn);
+    static uint8_t payload[payloadSize];
+    static RCProtocol::PacketIn* pPacketIn = (RCProtocol::PacketIn*)payload;
+
+    static int byteCounter = 0;
+
+    if (*pHeader32 == MAGIC_WORD)
+    {
+        payload[byteCounter++] = newByte;
+
+        static uint16_t sum = 0;
+
+        if (byteCounter >= payloadSize)
+        {
+            *pHeader32 = 0; // Unvalidate the magic word
+            byteCounter = 0; // Reset the read counter
+
+            uint8_t checksum = sum & 0xFF;
+            sum = 0;
+
+            if (checksum == pPacketIn->Checksum)
+            {
+                // Create package
+                //m_semaphore.wait();
+                m_packetIn = *((PacketIn*)payload);
+                //m_semaphore.release();
+                return true; // new package available
+            }
+        }
+        sum +=newByte; // placed here not to include the checksum
+    }
+    else // Read header
+    {
+        *pHeader32 = *pHeader32 >> 8 | newByte << 8*3;
+    }
+    return false; // Continue parsing
+}
+
+uint8_t RCProtocol::writeByte(bool& done)
+{
+    static const uint8_t* pByte = (uint8_t*)&m_packetOut;
+
+    static uint8_t i = 0;
+    static const uint8_t max = sizeof(RCProtocol::PacketOut);
+
+    uint8_t outByte = pByte[i];
+    i++;
+    
+    i = i % max;
+    
+    done = (i == 0) ? true : false;
+
+    return outByte;
+}
+
+void RCProtocol::setOutput(RCProtocol::PacketOut& packetOut)
+{
+    m_packetOut = packetOut;
+
+    // Now we calculate the checksum
+    uint8_t* pByte = (uint8_t*)&m_packetOut;
+
+    uint16_t sum = 0;
+
+    for (int i = 4; i < sizeof(RCProtocol::PacketOut) - 1; i++)
+    {
+        sum += pByte[i];
+    }
+
+    uint8_t checksum = sum & 0xFF;
+
+    m_packetOut.Checksum = checksum;
+}
+
+}

src/serialization/RCProtocol.h

@@ -0,0 +1,89 @@
+#include "mbed.h"
+
+#pragma once
+
+
+namespace serialization {
+
+//! Receive commands from the Touch OSC layout through a NodeMCU
+class RCProtocol
+{
+
+public:
+
+    struct PacketIn {
+        
+        int16_t Throttle;
+        int16_t Steering;
+
+        int16_t Kp;
+        int16_t Ki;
+        int16_t Kd;
+
+        bool Enabled;
+
+        uint8_t Checksum;
+        
+        PacketIn()
+        : Throttle(0)
+        , Steering(0)
+        , Kp(0)
+        , Ki(0)
+        , Kd(0)
+        , Enabled(false)
+        , Checksum(0)
+        {
+        }
+
+    } __attribute__ ((__packed__));
+
+    struct PacketOut {
+
+        int32_t MagicWord;
+        
+        int16_t BatteryLevel;
+
+        int16_t Mode;
+
+        uint8_t Checksum;
+
+        PacketOut()
+        : MagicWord(0xDEC0DED)
+        , BatteryLevel(0)
+        , Mode(0)
+        , Checksum(0)
+        {
+        }
+
+    } __attribute__ ((__packed__));
+
+    RCProtocol();
+
+    //! Append a byte to read until we have a hole package.
+    //! Returns true when a hole package is available
+    bool readByte(uint8_t newByte);
+
+    //! Read the latest package
+    RCProtocol::PacketIn readPacket();
+
+    //! Generate a byte to write
+    //! @done is set to true when a hole packet
+    //! has been passed
+    uint8_t writeByte(bool& done);
+    
+    //! Set the data to be written
+    void setOutput(RCProtocol::PacketOut& packetOut);
+
+    //Semaphore m_semaphore;
+
+    const static uint32_t MAGIC_WORD = 0xDEC0DED;
+
+private:
+
+    RCProtocol::PacketIn m_packetIn;
+
+    RCProtocol::PacketOut m_packetOut;
+
+};
+
+}

src/targets/revo_f4/pins.h

@@ -7,23 +7,50 @@
 extern "C" {
 #endif
 
+namespace targets {
+namespace revo_f4 {
+
 typedef enum
 {
-    ledBlue = PB_5,
-    ledOrange = PB_4,
-    sensV = PC_2,
-    sensC = PC_1,
+    // Onboard leds
+    PIN_BLUE_LED   = PB_5,
+    PIN_ORANGE_LED = PB_4,
 
-    MPU_MOSI = PA_7,
-    MPU_MISO = PA_6,
-    MPU_SCLK = PA_5,
-    MPU_NSS  = PA_4,
+    // Analog sensors
+    PIN_VOLTAGE_SENSOR = PC_2,
+    PIN_CURRENT_SENSOR = PC_1,
 
-    COMPASS_SCL = PB_8,
-    COMPASS_SDA = PB_9,
+    // MPU SPI pins
+    PIN_MPU_MOSI = PA_7,
+    PIN_MPU_MISO = PA_6,
+    PIN_MPU_SCLK = PA_5,
+    PIN_MPU_NSS  = PA_4,
+
+    // Flash SPI pins
+    PIN_FLASH_MOSI = PC_12,
+    PIN_FLASH_MISO = PC_11,
+    PIN_FLASH_SCLK = PC_10,
+    PIN_FLASH_NSS  = PA_15,
+
+    // Compass i2c pins
+    PIN_COMPASS_SCL = PB_8,
+    PIN_COMPASS_SDA = PB_9,
+
+    // Flexiport - Needs fixing in m-bed
+    // PIN_USART3_RX = PB_10,
+    // PIN_USART4_TX = PB_11,
+
+    PIN_USART2_RX = PA_3, // Servo 3
+    PIN_USART2_TX = PA_2, // Servo 4
+
+    PIN_USART4_RX = PA_1, // Servo 5
+    PIN_USART4_TX = PA_0, // Servo 6
 
 } PinMap;
 
+} // namespace revo_f4
+} // namespace targets
+
 #ifdef __cplusplus
 }
 #endif