unknown work 1

Just write back the serial data that comes in

.vscode/settings.json

@@ -3,6 +3,22 @@
     //"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",
+        "__string": "cpp",
+        "string_view": "cpp",
+        "__split_buffer": "cpp"
     }
 }

Makefile

@@ -645,6 +645,18 @@ OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
 OBJECTS += ./src/drivers/stepper.o
 OBJECTS += ./src/drivers/servo.o
+OBJECTS += ./src/drivers/SerialFlash/M25PDevice.o
+OBJECTS += ./src/drivers/SerialFlash/M25PDeviceImpl.o
+OBJECTS += ./src/drivers/SerialFlash/MbedSPICommand.o
+OBJECTS += ./src/drivers/SerialFlash/SerialFlashDeviceCreator.o
+OBJECTS += ./src/drivers/SerialFlash/SST25Device.o
+OBJECTS += ./src/drivers/SerialFlash/SST25DeviceImpl.o
+OBJECTS += ./src/drivers/M25P16.o
+OBJECTS += ./src/drivers/SpiFlash25.o
+OBJECTS += ./src/control/ImuFusion.o
+OBJECTS += ./src/control/PID.o
+OBJECTS += ./src/math/Utilities.o
+
 
 
 INCLUDE_PATHS += -I../

main.cpp

@@ -1,50 +1,169 @@
 #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"
+#include "src/drivers/M25P16.h"
+// Control
 #include "src/control/lpf.h"
+#include "src/control/PID.h"
+#include "src/control/ImuFusion.h"
+// Flash Memory - TODO: This component is not correctly implemented yet
+#include "src/drivers/SerialFlash/MbedSPICommand.h"
+#include "src/drivers/SerialFlash/SerialFlashDeviceCreator.h"
+#include "src/drivers/SpiFlash25.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;
 
 EventQueue queue;
 
-Ticker queueReader;
-
+// Serial port (Servo Outputs)
+Serial serialMotorOutputs(PA_2, PA_3, 115200);
 
 // 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
+SPI spiImu(PA_7, PA_6, PA_5);   //define the SPI (mosi, miso, sclk). Default frequency is 1Mhz
+mpu6000_spi imu(spiImu,PA_4);   //define the mpu6000 object
 
-PwmOut led1(D4); //PB_5
+// Flash setup
+//SPI spiFlash(PC_12, PC_11, PC_10, PA_15);
+//SPI spiFlash(PC_12, PC_11, PC_10, PB_3);
+//M25P16 memory(spiFlash, PB_3);
+//DigitalOut radioCS(PA_15);
+
+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, 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);
 
+Timer timer;
+
+// Throttle PI Controller with some acceptable values
+controllerPI throttleControl(0.0025, 0.01, 5, 0); // 0.065, 0.05, 12, 40
+
+// TODO: Figure out some good values
+controllerPD angleControl(10.0, 146.0, 400);
+// Alternatively try controllerPD2 which has another Dterm implementation
+
 // Draft of some control function
-void controlFunc(int a)
+// this runs in the context of eventThread and is triggered by the gyro ISR pin
+void controlFunc()
 {
-    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);
+
+    // Calculate dT in sec
+    float dT = timer.read_us()/(float)1000000.0;
+    timer.reset();
+
+    // Retrieve IMU angle (Only x-axis implemented for now)
+    float angleX = imuFusion.getAngle(dT);
+    float forceX = sin(angleX/180*PI);
+    // 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 (abs(angleX) > (float)50.0f && !disabled)
+    {
+        controlOutput = 0.0f; // rinse integral
+        disabled = true;
+        motorL.disable();
+        motorR.disable();
+    }
+    else if (abs(angleX) < (float)50.0f && disabled)
+    {
+        disabled = false;
+        motorL.enable();
+        motorR.enable();
+    }
+
+    /* --------------------------
+    Calculate estimated groundspeed 
+    of the robot. 
+    TODO: Inject correct input (compensate for robot rotation)
+    -------------------------- */
+    // Calculate filtered groundspeed of the robot (From deg/s to m/s)
+    float estimatedSpeed((motorL.getSpeed() + motorR.getSpeed())/2);
+    static incrementalLPF speedFilter;
+    float filteredEstSpeed(speedFilter.filter(estimatedSpeed));
+    float speedScale((1.0f/360.0f)*WHEEL_SIZE*PI);
+    float groundSpeed(filteredEstSpeed*speedScale);
+
+    /* --------------------------
+    Calculate the setpoint for 
+    the main control (control the angle)
+    through this throttle control loop (PI)
+    TODO: TEMP turned off the throttle 
+    control to tune the PD angle control
+    -------------------------- */
+    float throttle(0.0f); // TODO: This will be the input from remote
+    float angleSP = 0.0f; // Temporarily emulate an output
+    //float angleSP = throttleControl.run(dT, groundSpeed, throttle);
+
+    
+    /* --------------------------
+    The last control loop. Angle (PD)
+    TODO:
+    -------------------------- */
+    // Integrating the output to obtain acceleration
+    if (!disabled)
+    {
+        controlOutput += angleControl.run(dT, forceX, angleSP);
+
+        controlOutput = constrain(controlOutput, 3000.0f);
+    }
+    
+    /* --------------------------
+     Lastly the steering is added
+     straight on the output
+     TODO: Activate when implemented
+    -------------------------- */
+    float steering(0.0f); // This will come from remote
+    motorL.setSpeed(controlOutput - steering);
+    motorR.setSpeed(controlOutput + steering);
+
+    // Blink LED at 1hz
+    static int i = 0;
+    i++;
+    if (i > 100)
+    {
+        i = 0;
+        ledOrg = !ledOrg;
+    }
+}
+
+void readSerialContext()
+{
+    while (true)
+    {
+        while (serialMotorOutputs.readable()) 
+        {
+            serialMotorOutputs.putc(serialMotorOutputs.getc());
+        }
+    }
+    Thread::wait(1000);
 }
 
 // This context just pulses the blue LED
@@ -60,85 +179,181 @@ 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()
+void testFlash()
 {
-    //queue.dispatch(0);
-}
-
-// Interrupt driven function
-// At the moment its running at 100hz
-void ISRGYRO() 
-{
-    static int i = 0;
-    i++;
-    if (i > 100)
+    ISPICommand* pSPICommand = new MbedSPICommand(PC_12, PC_11, PC_10, PB_3);   // MOSI, MISO, SCK, CS
+    ISerialFlashDevice* pDevice = SerialFlashDeviceCreator::Create(pSPICommand);
+    
+    if (pDevice == NULL)
     {
-        i = 0;
-        ledOrg = !ledOrg;
+        printf("Unsupported device");
+        return;
     }
-    // Report that there is new data to read
-    // TODO: This hack has to be done in a better way
-    queue.call(controlFunc, 0);
+
+    // std::string deviceName = pDevice->GetDeviceName();
+    // printf("%s\n", deviceName);
+    int capacity = pDevice->GetCapacity() / 1024;
+    printf("%dK x 8\n", capacity);
+
+    // int *a[3];
+    // *a[0] = 1;
+    // *a[1] = 2;
+    // *a[2] = 3;
+
+    // int *b[3];
+
+    int result;
+
+    // result = pDevice->Write(100, a, sizeof(a));
+
+    // result = pDevice->Read(100, b, sizeof(b));
+
+    uint8_t c(-1), d(0);
+
+    result = pDevice->Read(200, &c, sizeof(c));
+
+    wait(1);
+
+    c += 4;
+
+    result = pDevice->Write(200, &c, sizeof(c));
+
+    wait(1);
+
+    result = pDevice->Read(200, &d, sizeof(d));
+
 }
 
-int i = 0;
+void testFlash2()
+{
+    SpiFlash25 flash(PC_12, PC_11, PC_10, PB_3);
+
+    char* k = flash.read_id();
+
+    char f = flash.read_status();
+
+    char a[] = "Hej och ha";
+
+    bool result;
+
+    result = flash.write(0, strlen(a), a);
+
+    char b[20];
+
+    result = flash.read(0, strlen(a), b);
+
+    printf("result");
+
+
+}
 
 // main() runs in its own thread
 int main() {
 
-    // Start sweeping the arm
-    servo.sweep(0.0, 1, 2);
+    serialMotorOutputs.baud(115200);
+    // Test thread for serial
+    Thread serialThread;
+    serialThread.start(callback(&readSerialContext));
 
-    Thread ledPulseThread;
-    ledPulseThread.start(callback(&pulseLedContext));
+    // Init memory
+    DigitalOut radioCS(PA_15);
+    radioCS = 1; // Disable radio chip. May not be necessary
+    //SPI spiFlash(PC_12, PC_11, PC_10);
+    
+    testFlash2();
 
-    // Register a event queue reader at set interval
-    queueReader.attach(&readQueue, 0.001);
+    // M25P16 memory(spiFlash, PB_3);
+    // bool memoryUp = memory.init();
 
-    // Enable motor controllers (Will power the motors with no movement)
-    motor1.enable();
-    motor2.enable();
+    //printf("Initialization of onboard memory " + memoryUp ? "succeded" : "failed");
 
-    // Set motor 2 to a fixed speed 
-    // (To test that timers don't interfeer with each other)
-    motor2.setSpeed(90.0);
+    //memory.chipErase();
+    int start = 0;
 
-    // Function to attach to gyro interrupt
-    gyroINT.rise(&ISRGYRO);
+    int values[3];
+    //memory.read(start, &values, sizeof(values));
+    
 
-    // MPU startup
+    values[0]++;
+    //values[1]++;
+    values[2]++;
+
+    //memory.write(start, values, sizeof(values));
+    
+
+    // int values2[3];
+    // memory.read(start, values2, sizeof(values2));
+
+    // memory.read(0, values2, sizeof(values2));
+
+    // values[0]++;
+    // values[1]++;
+    // values[2]++;
+
+    // start += sizeof(values);
+    // memory.write(0, values, sizeof(values));
+
+    // memory.read(0, values2, sizeof(values2));
+
+
+
+
+    // 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();
+
     // 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
+
+    /*-------------- Visible start sequence ------------*/
+    // Start sweeping the arm
+    servo.sweep(0.0, 1, 2);
+
+    // Start the pulsing blue led
+    Thread ledPulseThread;
+    ledPulseThread.start(callback(&pulseLedContext));
+
+    // Create realtime eventhandler for control loop
+    Thread eventThread(osPriorityRealtime);
+    eventThread.start(callback(&queue, &EventQueue::dispatch_forever));
+
+    // Attach gyro interrupt to add a control event
+    gyroINT.rise(queue.event(&controlFunc));
+
+    // Enable motor controllers (Will power the motors with no movement)
+    motorL.enable();
+    motorR.enable();
+    motorL.setDirection(1);
+    motorR.setDirection(-1);
 
     Thread::wait(1000);
 
+    // Servo Nod to tell us that we are done
     servo.nod();
 
-    Thread::wait(1000);
+    servo.setPosition(-0.2);
 
-    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/PID.cpp

@@ -0,0 +1,103 @@
+#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(100.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_kP*error);
+
+    // Accumulate to the integrator
+    float iTerm(constrain(m_integrator + m_kI*error*dT, m_iTermSaturation));
+
+    // Store
+    m_integrator = iTerm;
+
+    float output(constrain(pTerm + iTerm, m_saturation));
+
+    return output;
+}
+
+}

src/control/PID.h

@@ -0,0 +1,63 @@
+#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);
+
+private:
+    float m_kP;
+    float m_kI;
+    float m_saturation;
+    float m_iTermSaturation;
+    float m_integrator;
+};
+
+}

src/control/lpf.cpp

@@ -1,17 +1,39 @@
 #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;
 }
 
+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,29 @@ public:
 
     incrementalLPF();
 
-    double filter(double latestValue);
+    float filter(float latestValue);
 
 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/M25P16.cpp

@@ -0,0 +1,259 @@
+#include "M25P16.h"
+
+namespace drivers {
+
+M25P16::M25P16(SPI& spi, PinName cs)
+: m_spi(spi)
+, m_cs(cs)
+, m_property()
+{
+    m_property.deviceName = "M25P16";
+    m_property.capacity = 16384 * 1024 / 8;
+    m_property.blockProtectionMask = 0xe3;
+}
+
+bool M25P16::init()
+{
+    //clearBlockProtection();
+
+    int manufacturerId;
+    int memoryType;
+    int memoryCapacity;
+
+    readId(manufacturerId, memoryType, memoryCapacity);
+
+    clearBlockProtection();
+    
+    // If we get the expected values from the bus then we are good
+    return    manufacturerId == 0x20
+           && memoryType     == 0x20
+           && memoryCapacity == 0x15;
+}
+
+void M25P16::readId(int& manufacturerId, int& memoryType, int& memoryCapacity)
+{
+    m_spi.frequency(46000000);
+
+    unsigned char id[3];
+    readSPI(0x9f, id, sizeof(id));
+    manufacturerId = id[0];
+    memoryType = id[1];
+    memoryCapacity = id[2];
+}
+
+int M25P16::read(int address, void* buffer, int length)
+{
+    if (address >= getCapacity())
+    {
+        return 0;
+    }
+    if (address + length > getCapacity())
+    {
+        length = getCapacity() - address;
+    }
+    if (length == 0)
+    {
+        return 0;
+    }
+
+    char param[] = {0, 0, 0, 0xff};
+    fillAddress(param, address);
+    readSPI(0x0b, param, sizeof(param), buffer, length);
+    return length;
+}
+
+void M25P16::fillAddress(char* pBuffer, int address)
+{
+    *(pBuffer + 0) = (address & 0xff0000) >> 16;
+    *(pBuffer + 1) = (address & 0x00ff00) >> 8;
+    *(pBuffer + 2) = (address & 0x0000ff);
+}
+
+int M25P16::write(int address, const void* buffer, int length)
+{
+    if (address >= getCapacity())
+    {
+        return 0;
+    }
+    if (address + length > getCapacity())
+    {
+        length = getCapacity() - address;
+    }
+    if (length == 0)
+    {
+        return 0;
+    }
+
+    int result = length;
+
+    const int pageSize = 256;
+    const int pageSizeMask = pageSize - 1;
+    const int pageAddressMask = ~pageSizeMask;
+
+    const unsigned char* p = static_cast<const unsigned char*>(buffer);
+    if ((address & pageSizeMask) != 0)
+    {
+        int readLen = address & pageSizeMask;
+        int copyLen = pageSize - readLen;
+        if (copyLen > length)
+        {
+            copyLen = length;
+        }
+        char buf[pageSize];
+        int writeAddress = address & pageAddressMask;
+        read(writeAddress, buf, readLen);
+        memcpy(&buf[address & pageSizeMask], buffer, copyLen);
+        pageProgram(writeAddress, buf);
+        p += readLen;
+        address += pageSize - readLen;
+        length -= copyLen;
+    }
+    while (length >= pageSize)
+    {
+        pageProgram(address, p);
+        address += pageSize;
+        p += pageSize;
+        length -= pageSize;
+    }
+    if (length != 0)
+    {
+        char buf[pageSize];
+        memcpy(buf, p, length);
+        memset(&buf[length], 0xff, pageSize - length);
+        pageProgram(address, buf);
+    }
+    return result;
+}
+
+void M25P16::select()
+{
+    m_cs = 0;
+}
+
+void M25P16::deselect()
+{
+    m_cs = 1;
+}
+
+unsigned int M25P16::whoami()
+{
+    return 0;
+}
+
+int M25P16::getCapacity() const
+{
+    return m_property.capacity;
+}
+
+std::string M25P16::getDeviceName() const
+{
+    return m_property.deviceName;
+}
+
+int M25P16::readStatusRegister(void)
+{
+    unsigned char status;
+    readSPI(0x05, &status, 1);
+    return status;
+}
+
+void M25P16::writeStatusRegister(int value)
+{
+    char vb = static_cast<char>(value);
+    writeSPI(0x01, &vb, 1);
+}
+
+void M25P16::writeEnable()
+{
+    writeSPI(0x06);
+}
+
+void M25P16::writeDisable()
+{
+    writeSPI(0x04);
+}
+
+void M25P16::clearBlockProtection()
+{
+    writeEnable();
+
+    int status = readStatusRegister();
+    status &= m_property.blockProtectionMask;
+    writeStatusRegister(status);
+}
+
+void M25P16::writeSPI(int command)
+{
+    m_cs = 0;
+    m_spi.write(command);
+    m_cs = 1;
+}
+
+void M25P16::writeSPI(int command, const void* pParam, int length)
+{
+    m_cs = 0;
+    m_spi.write(command);
+
+    const char*p = static_cast<const char*>(pParam);
+    for (int i = 0; i < length; ++i)
+    {
+        m_spi.write(*p++);
+    }
+    m_cs = 1;
+}
+
+void M25P16::readSPI(int command, void* pRead, int readLength)
+{
+    m_cs = 0;
+    m_spi.write(command);
+    
+        char* pReadByte = static_cast<char*>(pRead);
+    for (int i = 0; i < readLength; ++i)
+    {
+        *pReadByte++ = m_spi.write(0xff);
+    }
+    m_cs = 1;
+}
+
+void M25P16::readSPI(int command, const void* pParam, int paramLength, void* pReaded, int readLength)
+{
+    m_cs = 0;
+    m_spi.write(command);
+    
+    const char* pParamByte = static_cast<const char*>(pParam);
+    for (int i = 0; i < paramLength; ++i)
+    {
+        m_spi.write(*pParamByte++);
+    }
+    char* pReadedByte = static_cast<char*>(pReaded);
+    for (int i = 0; i < readLength; ++i)
+    {
+        *pReadedByte++ = m_spi.write(0xff);
+    }
+    m_cs = 1;
+}
+
+void M25P16::chipErase()
+{
+    writeEnable();
+
+    writeSPI(0xc7);
+
+    while (readStatusRegister() & 0x01);
+
+    clearBlockProtection();
+}
+
+void M25P16::pageProgram(int address, const void* buffer)
+{
+    writeEnable();
+
+    char param[256 + 3];
+    fillAddress(param, address);
+    memcpy(param + 3, buffer, 256);
+    writeSPI(0x02, param, sizeof(param));
+
+    while (readStatusRegister() & 0x01);
+}
+
+} // namespace drivers

src/drivers/M25P16.h

@@ -0,0 +1,74 @@
+#include "mbed.h"
+
+#include <string>
+
+#pragma once
+
+namespace drivers {
+
+class M25P16 {
+
+public:
+
+    M25P16(SPI& spi, PinName cs);
+
+    bool init();
+
+    int read(int address, void* buffer, int length);
+
+    int write(int address, const void* buffer, int length);
+
+    void chipErase();
+
+    unsigned int whoami();
+
+private:
+
+    SPI& m_spi;
+    DigitalOut m_cs;
+
+    struct DeviceProperty
+    {
+        std::string deviceName;
+        int  capacity;
+        int  blockProtectionMask;
+    } m_property;
+
+    // Returns the size of the memory
+    int getCapacity() const;
+
+    void readId(int& manufacturerId, int& memoryType, int& memoryCapacity);
+
+    std::string getDeviceName() const;
+
+    void fillAddress(char* pBuffer, int address);
+
+    int readStatusRegister();
+
+    void writeStatusRegister(int value);
+
+    void writeEnable();
+
+    void writeDisable();
+
+    void clearBlockProtection();
+
+    void select();
+
+    void deselect();
+
+    void writeSPI(int command);
+
+    void writeSPI(int command, const void* pParam, int length);
+
+    void readSPI(int command, void* pRead, int readLength);
+
+    void readSPI(int command, const void* pParam, int paramLength, void* pReaded, int readLength);
+
+    void pageProgram(int address, const void* buffer);
+
+};
+
+#define MYBIT 0x00
+
+} // namespace drivers

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/SPICommand/main.cpp

@@ -0,0 +1,26 @@
+#include "mbed.h"
+#include "MbedSPICommand.h"
+#include "SerialFlashDeviceCreator.h"
+
+DigitalOut myled(LED1);
+
+int main()
+{
+    ISPICommand* pSPICommand = new MbedSPICommand(p11, p12, p13, p14);   // MOSI, MISO, SCK, CS
+
+    ISerialFlashDevice* pDevice = SerialFlashDeviceCreator::Create(pSPICommand);
+    if (pDevice == NULL)
+    {
+        printf("Unsupported device");
+        return;
+    }
+    printf("%s\n", pDevice->GetDeviceName());
+    printf("%dK x 8\n", pDevice->GetCapacity() / 1024);
+
+    while(1) {
+        myled = 1;
+        wait(0.2);
+        myled = 0;
+        wait(0.2);
+    }
+}

src/drivers/SPICommand/mbed.bld

@@ -0,0 +1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6473597d706e

src/drivers/SerialFlash/ISPICommand.h

@@ -0,0 +1,14 @@
+#pragma once
+
+class ISPICommand
+{
+public:
+
+    virtual ~ISPICommand() {}
+    virtual void SetMaxFrequency(int frequency) = 0;
+    
+    virtual void Write(int command) = 0;
+    virtual void Write(int command, const void* pParam, int length) = 0;
+    virtual void Read(int command, void* pReaded, int readLength) = 0;
+    virtual void Read(int command, const void* pParam, int paramLength, void* pReaded, int readLength) = 0;
+};

src/drivers/SerialFlash/ISerialFlashDevice.h

@@ -0,0 +1,14 @@
+#pragma once
+
+#include <string>
+
+class ISerialFlashDevice
+{
+public:
+    virtual ~ISerialFlashDevice(void) { }
+    virtual std::string GetDeviceName(void) const = 0;
+    virtual int GetCapacity(void) const = 0;
+    virtual void ChipErase(void) = 0;
+    virtual int Read(int address, void* buffer, int length) = 0;
+    virtual int Write(int address, const void* buffer, int length) = 0;
+};

src/drivers/SerialFlash/M25PDevice.cpp

@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include "M25PDeviceImpl.h"
+#include "M25PDevice.h"
+
+bool M25PDevice::IsSupported(ISPICommand* pSPICommand)
+{
+    return M25PDeviceImpl::IsSupported(pSPICommand);
+}
+
+M25PDevice* M25PDevice::Create(ISPICommand* pSPICommand)
+{
+    M25PDeviceImpl* pImpl = M25PDeviceImpl::Create(pSPICommand);
+    if (pImpl == NULL)
+    {
+        return NULL;
+    }
+    return new M25PDevice(pImpl);
+}
+
+
+M25PDevice::M25PDevice(M25PDeviceImpl* pImpl)
+ : _pImpl(pImpl)
+{
+
+}
+
+string M25PDevice::GetDeviceName(void) const
+{
+    return _pImpl->GetDeviceName();
+}
+
+int M25PDevice::GetCapacity(void) const
+{
+    return _pImpl->GetCapacity();
+}
+
+void M25PDevice::ChipErase(void)
+{
+    _pImpl->BulkErase();
+}
+
+int M25PDevice::Read(int address, void* buffer, int length)
+{
+    return _pImpl->Read(address, buffer, length);
+}
+
+int M25PDevice::Write(int address, const void* buffer, int length)
+{
+    return _pImpl->Write(address, buffer, length);
+}

src/drivers/SerialFlash/M25PDevice.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include "ISerialFlashDevice.h"
+
+class ISPICommand;
+class M25PDeviceImpl;
+
+class M25PDevice : public ISerialFlashDevice
+{
+public:
+    static M25PDevice* Create(ISPICommand* pSPICommand);
+    static bool IsSupported(ISPICommand* pSPICommand);
+
+    virtual std::string GetDeviceName(void) const;
+    virtual int GetCapacity(void) const;
+    virtual void ChipErase(void);
+    virtual int Read(int address, void* buffer, int length);
+    virtual int Write(int address, const void* buffer, int length);
+
+private:
+    std::auto_ptr<M25PDeviceImpl> _pImpl;
+    M25PDevice(M25PDeviceImpl* pImpl);
+};

src/drivers/SerialFlash/M25PDeviceImpl.cpp

@@ -0,0 +1,235 @@
+#include "ISPICommand.h"
+#include <string.h>
+#include "M25PDeviceImpl.h"
+
+const M25PDeviceImpl::DeviceProperty* M25PDeviceImpl::findMatchDevice(ISPICommand* pSPICommand)
+{
+    int manufacturerId;
+    int memoryType;
+    int memoryCapacity;
+    readId(pSPICommand, manufacturerId, memoryType, memoryCapacity);
+
+    struct SupportedDevice
+    {
+        int manufacturerId;
+        int memoryType;
+        int memoryCapacity;
+        DeviceProperty property;
+    } static const supportedDevices[] =
+    {
+        //M25P16 16MBit (2MB)
+        { 0x20, 0x20, 0x15,
+            {
+              "M25P16",
+              16384 * 1024 / 8,
+              0xe3,
+            },
+        },
+        //M25P80 8MBit (1MB)
+        { 0x20, 0x20, 0x14,
+            {
+              "M25P80",
+              8192 * 1024 / 8,
+              0xe3,
+           },
+        },
+    };
+    int count = sizeof(supportedDevices) / sizeof(supportedDevices[0]);
+    for (int i = 0; i < count; ++i)
+    {
+        const SupportedDevice& device = supportedDevices[i];
+        if (device.manufacturerId == manufacturerId && device.memoryType == memoryType && device.memoryCapacity == memoryCapacity)
+        {
+            return &device.property;
+        }
+    }
+    return NULL;
+}
+
+bool M25PDeviceImpl::IsSupported(ISPICommand* pSPICommand)
+{
+    return findMatchDevice(pSPICommand) != NULL;
+}
+
+M25PDeviceImpl* M25PDeviceImpl::Create(ISPICommand* pSPICommand)
+{
+    const DeviceProperty* property = findMatchDevice(pSPICommand);
+    if (property == NULL)
+    {
+        return NULL;
+    }
+    return new M25PDeviceImpl(pSPICommand, *property);
+}
+
+M25PDeviceImpl::M25PDeviceImpl(ISPICommand* pSPICommand, const DeviceProperty& property)
+ : _pSPICommand(pSPICommand)
+ , _property(property)
+{
+    clearBlockProtection();
+}
+
+M25PDeviceImpl::~M25PDeviceImpl(void)
+{
+
+}
+
+void M25PDeviceImpl::readId(ISPICommand* pSPICommand, int& manufacturerId, int& memoryType, int& memoryCapacity)
+{
+    const static int DefaultOperationFrequency = 20000000;
+    pSPICommand->SetMaxFrequency(DefaultOperationFrequency);
+
+    unsigned char id[3];
+    pSPICommand->Read(0x9f, id, sizeof(id));
+    manufacturerId = id[0];
+    memoryType = id[1];
+    memoryCapacity = id[2];
+}
+
+int M25PDeviceImpl::readStatusRegister(void)
+{
+    unsigned char status;
+    _pSPICommand->Read(0x05, &status, 1);
+    return status;
+}
+
+void M25PDeviceImpl::writeStatusRegister(int value)
+{
+    char vb = static_cast<char>(value);
+    _pSPICommand->Write(0x01, &vb, 1);
+}
+
+void M25PDeviceImpl::writeEnable()
+{
+    _pSPICommand->Write(0x06);
+}
+
+void M25PDeviceImpl::writeDisable()
+{
+    _pSPICommand->Write(0x04);
+}
+
+void M25PDeviceImpl::clearBlockProtection(void)
+{
+    writeEnable();
+
+    int status = readStatusRegister();
+    status &= _property.blockProtectionMask;
+    writeStatusRegister(status);
+}
+
+std::string M25PDeviceImpl::GetDeviceName(void) const
+{
+    return _property.deviceName;
+}
+
+int M25PDeviceImpl::GetCapacity(void) const
+{
+    return _property.capacity;
+}
+
+int M25PDeviceImpl::Read(int address, void* buffer, int length)
+{
+    if (address >= GetCapacity())
+    {
+        return 0;
+    }
+    if (address + length > GetCapacity())
+    {
+        length = GetCapacity() - address;
+    }
+    if (length == 0)
+    {
+        return 0;
+    }
+
+    char param[] = { 0, 0, 0, 0xff };
+    fillAddress(param, address);
+    _pSPICommand->Read(0x0b, param, sizeof(param), buffer, length);
+    return length;
+}
+
+void M25PDeviceImpl::fillAddress(char* pBuffer, int address)
+{
+    *(pBuffer + 0) = (address & 0xff0000) >> 16;
+    *(pBuffer + 1) = (address & 0x00ff00) >> 8;
+    *(pBuffer + 2) = (address & 0x0000ff);
+}
+
+int M25PDeviceImpl::Write(int address, const void* buffer, int length)
+{
+    if (address >= GetCapacity())
+    {
+        return 0;
+    }
+    if (address + length > GetCapacity())
+    {
+        length = GetCapacity() - address;
+    }
+    if (length == 0)
+    {
+        return 0;
+    }
+
+    int result = length;
+
+    const int pageSize = 256;
+    const int pageSizeMask = pageSize - 1;
+    const int pageAddressMask = ~pageSizeMask;
+
+    const unsigned char* p = static_cast<const unsigned char*>(buffer);
+    if ((address & pageSizeMask) != 0)
+    {
+        int readLen = address & pageSizeMask;
+        int copyLen = pageSize - readLen;
+        if (copyLen > length)
+        {
+            copyLen = length;
+        }
+        char buf[pageSize];
+        int writeAddress = address & pageAddressMask;
+        Read(writeAddress, buf, readLen);
+        memcpy(&buf[address & pageSizeMask], buffer, copyLen);
+        pageProgram(writeAddress, buf);
+        p += readLen;
+        address += pageSize - readLen;
+        length -= copyLen;
+    }
+    while (length >= pageSize)
+    {
+        pageProgram(address, p);
+        address += pageSize;
+        p += pageSize;
+        length -= pageSize;
+    }
+    if (length != 0)
+    {
+        char buf[pageSize];
+        memcpy(buf, p, length);
+        memset(&buf[length], 0xff, pageSize - length);
+        pageProgram(address, buf);
+    }
+    return result;
+}
+
+void M25PDeviceImpl::BulkErase()
+{
+    writeEnable();
+
+    _pSPICommand->Write(0xc7);
+
+    while (readStatusRegister() & 0x01);
+
+    clearBlockProtection();
+}
+
+void M25PDeviceImpl::pageProgram(int address, const void* buffer)
+{
+    writeEnable();
+
+    char param[256 + 3];
+    fillAddress(param, address);
+    memcpy(param + 3, buffer, 256);
+    _pSPICommand->Write(0x02, param, sizeof(param));
+
+    while (readStatusRegister() & 0x01);
+}

src/drivers/SerialFlash/M25PDeviceImpl.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include <string>
+
+class ISPICommand;
+
+class M25PDeviceImpl
+{
+public:
+    static bool IsSupported(ISPICommand* pSPICommand);
+    static M25PDeviceImpl* Create(ISPICommand* pSPICommand);
+    ~M25PDeviceImpl(void);
+    
+    std::string GetDeviceName() const;
+    int GetCapacity() const;
+    int Read(int address, void* buffer, int length);
+    int Write(int address, const void* buffer, int length);
+    void BulkErase(void);
+
+private:
+    ISPICommand* _pSPICommand;
+    int _operationFrequency;
+    struct DeviceProperty
+    {
+        std::string deviceName;
+        int  capacity;
+        int  blockProtectionMask;
+    } const& _property;
+    static const DeviceProperty* findMatchDevice(ISPICommand* pSPI);
+
+    M25PDeviceImpl(ISPICommand* pSPICommand, const DeviceProperty& property);
+    static void readId(ISPICommand* pSPICommand, int& manufacturerId, int& memoryType, int& memoryCapacity);
+    static void fillAddress(char* pBuffer, int address);
+
+    int readStatusRegister(void);
+    void clearBlockProtection(void);
+    void writeStatusRegister(int value);
+    void writeEnable(void);
+    void writeDisable(void);
+    void pageProgram(int address, const void* buffer);
+};

src/drivers/SerialFlash/MbedSPICommand.cpp

@@ -0,0 +1,71 @@
+#include "MbedSPICommand.h"
+
+MbedSPICommand::MbedSPICommand(PinName mosi, PinName miso, PinName sck, PinName cs)
+ : _spi(mosi, miso, sck)
+ , _cs(cs)
+{
+    //CS inactive
+    _cs = 1;
+}
+
+void MbedSPICommand::SetMaxFrequency(int frequency)
+{
+    static const int LPC1768MaxFrequency = 46000000;
+
+    if (frequency > LPC1768MaxFrequency)
+    {
+        frequency = LPC1768MaxFrequency;
+    }
+    _spi.frequency(frequency);
+}
+
+void MbedSPICommand::Write(int command)
+{
+    _cs = 0;
+    _spi.write(command);
+    _cs = 1;
+}
+
+void MbedSPICommand::Write(int command, const void* pParam, int length)
+{
+    _cs = 0;
+    _spi.write(command);
+
+    const char*p = static_cast<const char*>(pParam);
+    for (int i = 0; i < length; ++i)
+    {
+        _spi.write(*p++);
+    }
+    _cs = 1;
+}
+
+void MbedSPICommand::Read(int command, void* pReaded, int readLength)
+{
+    _cs = 0;
+    _spi.write(command);
+    
+        char* pReadedByte = static_cast<char*>(pReaded);
+    for (int i = 0; i < readLength; ++i)
+    {
+        *pReadedByte++ = _spi.write(0xff);
+    }
+    _cs = 1;
+}
+
+void MbedSPICommand::Read(int command, const void* pParam, int paramLength, void* pReaded, int readLength)
+{
+    _cs = 0;
+    _spi.write(command);
+    
+    const char* pParamByte = static_cast<const char*>(pParam);
+    for (int i = 0; i < paramLength; ++i)
+    {
+        _spi.write(*pParamByte++);
+    }
+    char* pReadedByte = static_cast<char*>(pReaded);
+    for (int i = 0; i < readLength; ++i)
+    {
+        *pReadedByte++ = _spi.write(0xff);
+    }
+    _cs = 1;
+}

src/drivers/SerialFlash/MbedSPICommand.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include "ISPICommand.h"
+#include "mbed.h"
+
+class MbedSPICommand : public ISPICommand
+{
+public:
+    MbedSPICommand(PinName mosi, PinName miso, PinName sck, PinName cs);
+
+    virtual void SetMaxFrequency(int frequency);
+
+    virtual void Write(int command);
+    virtual void Write(int command, const void* pParam, int length);
+    virtual void Read(int command, void* pReaded, int readLength);
+    virtual void Read(int command, const void* pParam, int paramLength, void* pReaded, int readLength);
+
+private:
+    SPI _spi;
+    DigitalOut _cs;
+};
+

src/drivers/SerialFlash/SST25Device.cpp

@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include "SST25DeviceImpl.h"
+#include "SST25Device.h"
+
+bool SST25Device::IsSupported(ISPICommand* pSPICommand)
+{
+    return SST25DeviceImpl::IsSupported(pSPICommand);
+}
+
+SST25Device* SST25Device::Create(ISPICommand* pSPICommand)
+{
+    SST25DeviceImpl* pImpl = SST25DeviceImpl::Create(pSPICommand);
+    if (pImpl == NULL)
+    {
+        return NULL;
+    }
+    return new SST25Device(pImpl);
+}
+
+
+SST25Device::SST25Device(SST25DeviceImpl* pImpl)
+ : _pImpl(pImpl)
+{
+
+}
+
+string SST25Device::GetDeviceName(void) const
+{
+    return _pImpl->GetDeviceName();
+}
+
+int SST25Device::GetCapacity(void) const
+{
+    return _pImpl->GetCapacity();
+}
+
+void SST25Device::ChipErase(void)
+{
+    _pImpl->ChipErase();
+}
+
+int SST25Device::Read(int address, void* buffer, int length)
+{
+    return _pImpl->Read(address, buffer, length);
+}
+
+int SST25Device::Write(int address, const void* buffer, int length)
+{
+    return _pImpl->Write(address, buffer, length);
+}

src/drivers/SerialFlash/SST25Device.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <memory>
+#include <string>
+#include "ISerialFlashDevice.h"
+
+class ISPICommand;
+class SST25DeviceImpl;
+
+class SST25Device : public ISerialFlashDevice
+{
+public:
+    static SST25Device* Create(ISPICommand* pSPICommand);
+    static bool IsSupported(ISPICommand* pSPICommand);
+
+    virtual std::string GetDeviceName(void) const;
+    virtual int GetCapacity(void) const;
+    virtual void ChipErase(void);
+    virtual int Read(int address, void* buffer, int length);
+    virtual int Write(int address, const void* buffer, int length);
+
+private:
+    std::auto_ptr<SST25DeviceImpl> _pImpl;
+    SST25Device(SST25DeviceImpl* pImpl);
+};

src/drivers/SerialFlash/SST25DeviceImpl.cpp

@@ -0,0 +1,444 @@
+#include "ISPICommand.h"
+#include <string.h>
+#include "SST25DeviceImpl.h"
+
+const SST25DeviceImpl::DeviceProperty* SST25DeviceImpl::findMatchDevice(ISPICommand* pSPICommand)
+{
+    int manufacturersId;
+    int deviceId;
+    readId(pSPICommand, manufacturersId, deviceId);
+
+    struct SupportedDevice
+    {
+        int manufacturersId;
+        int deviceId;
+        DeviceProperty property;
+    } static const supportedDevices[] =
+    {
+        //SST25xF512A 512KBit (64KByte)
+        { 0xbf, 0x48,
+            {
+              "SST25xF512A",
+              512 * 1024 / 8,
+              0xf3,
+              1,
+              &SST25DeviceImpl::writeAAI,
+              33000000
+            },
+        },
+        //SST25xF010A 1MBit (128KByte)
+        { 0xbf, 0x49,
+            {
+              "SST25xF010A",
+              1024 * 1024 / 8,
+              0xf3,
+              1,
+              &SST25DeviceImpl::writeAAI,
+              33000000
+           },
+        },
+        //SST25xF020A 2MBit (256KByte)
+        { 0xbf, 0x43,
+            {
+              "SST25xF020A",
+              2048 * 1024 / 8,
+              0xf3,
+              1,
+              &SST25DeviceImpl::writeAAI,
+              33000000
+           },
+        },
+        //SST25xF040B 4MBit (512KByte)
+        { 0xbf, 0x8d,
+            {
+              "SST25xF040B",
+              4096 * 1024 / 8,
+              0xe3,
+              1,
+              &SST25DeviceImpl::writeAAIWord,
+              50000000
+            },
+        },
+        //SST25xF080B 8MBit (1MByte)
+        { 0xbf, 0x8e,
+            {
+              "SST25xF080B",
+              8192 * 1024 / 8,
+              0xe3,
+              1,
+              &SST25DeviceImpl::writeAAIWord,
+              50000000
+            },
+        },
+        //SST25xF016B 16MBit (2MByte)
+        { 0xbf, 0x41,
+            {
+              "SST25xF016B",
+              16384 * 1024 / 8,
+              0xe3,
+              1,
+              &SST25DeviceImpl::writeAAIWord,
+              50000000
+            },
+        },
+        //SST25xF032B 32MBit (4MByte)
+        { 0xbf, 0x4a,
+            {
+              "SST25xF032B",
+              32768 * 1024 / 8,
+              0xc3,
+              1,
+              &SST25DeviceImpl::writeAAIWord,
+              50000000
+            },
+        },
+        //SST25xF064C 64MBit (8MByte)
+        { 0xbf, 0x4b,
+            {
+              "SST25xF064C",
+              65536 * 1024 / 8,
+              0xc3,              
+              256,
+              &SST25DeviceImpl::writePage,
+              50000000
+            },
+        }
+    };
+    int count = sizeof(supportedDevices) / sizeof(supportedDevices[0]);
+    for (int i = 0; i < count; ++i)
+    {
+        const SupportedDevice& device = supportedDevices[i];
+        if (device.manufacturersId == manufacturersId && device.deviceId == deviceId)
+        {
+            return &device.property;
+        }
+    }
+    return NULL;
+}
+
+bool SST25DeviceImpl::IsSupported(ISPICommand* pSPICommand)
+{
+    return findMatchDevice(pSPICommand) != NULL;
+}
+
+SST25DeviceImpl* SST25DeviceImpl::Create(ISPICommand* pSPICommand)
+{
+    const DeviceProperty* property = findMatchDevice(pSPICommand);
+    if (property == NULL)
+    {
+        return NULL;
+    }
+    return new SST25DeviceImpl(pSPICommand, *property);
+}
+
+SST25DeviceImpl::SST25DeviceImpl(ISPICommand* pSPICommand, const DeviceProperty& property)
+ : _pSPICommand(pSPICommand)
+ , _property(property)
+{
+    _pSPICommand->SetMaxFrequency(_property.operationClkHz);
+}
+
+SST25DeviceImpl::~SST25DeviceImpl(void)
+{
+
+}
+
+void SST25DeviceImpl::readId(ISPICommand* pSPICommand, int& manufacturersId, int& deviceId)
+{
+    const static int DefaultOperationFrequency = 20000000;
+    pSPICommand->SetMaxFrequency(DefaultOperationFrequency);
+
+    unsigned char read[2];
+    char param[3];
+    fillAddress(param, 0x000000);
+    pSPICommand->Read(0x90, param, sizeof(param), read, sizeof(read));
+    manufacturersId = read[0];
+    deviceId = read[1];
+}
+
+int SST25DeviceImpl::readStatusRegister(void)
+{
+    unsigned char read;
+    _pSPICommand->Read(0x05, &read, 1);
+    return read;
+}
+
+void SST25DeviceImpl::writeStatusRegister(int value)
+{
+    _pSPICommand->Write(0x50);
+
+    char vb = static_cast<char>(value);
+    _pSPICommand->Write(0x01, &vb, sizeof(vb));
+}
+
+void SST25DeviceImpl::writeEnable()
+{
+    int status = readStatusRegister();
+    status &= _property.blockProtectionMask;
+    writeStatusRegister(status);
+    _pSPICommand->Write(0x06);
+}
+
+void SST25DeviceImpl::writeDisable()
+{
+    _pSPICommand->Write(0x04);
+}
+
+void SST25DeviceImpl::waitForReady()
+{
+    while (readStatusRegister() & 0x01)
+        ;
+}
+
+std::string SST25DeviceImpl::GetDeviceName(void) const
+{
+    return _property.deviceName;
+}
+
+int SST25DeviceImpl::GetCapacity(void) const
+{
+    return _property.capacity;
+}
+
+int SST25DeviceImpl::Read(int address, void* buffer, int length)
+{
+    if (address >= GetCapacity())
+    {
+        return 0;
+    }
+    if (address + length > GetCapacity())
+    {
+        length = GetCapacity() - address;
+    }
+    if (length == 0)
+    {
+        return 0;
+    }
+
+    _pSPICommand->SetMaxFrequency(_property.operationClkHz);
+
+    char param[] = { 0, 0, 0, 0xff };
+    fillAddress(param, address);
+    _pSPICommand->Read(0x0b, param, sizeof(param), buffer, length);
+    return length;
+}
+
+int SST25DeviceImpl::Write(int address, const void* buffer, int length)
+{
+    if (address >= GetCapacity())
+    {
+        return 0;
+    }
+    if (address + length > GetCapacity())
+    {
+        length = GetCapacity() - address;
+    }
+    if (length == 0)
+    {
+        return 0;
+    }
+    (this->*_property.pfnWriter)(address, buffer, length);
+    return length;
+}
+
+void SST25DeviceImpl::ChipErase()
+{
+    _pSPICommand->SetMaxFrequency(_property.operationClkHz);
+
+    writeEnable();
+    _pSPICommand->Write(0x60);
+    waitForReady();
+}
+
+void SST25DeviceImpl::byteProgram(int address, int value)
+{
+    _pSPICommand->SetMaxFrequency(_property.operationClkHz);
+
+    writeEnable();
+
+    char param[] = { 0, 0, 0, value };
+    fillAddress(param, address);
+    _pSPICommand->Write(0x02, param, sizeof(param));
+
+    waitForReady();
+}
+
+void SST25DeviceImpl::beginAAIProgram(int address, int data)
+{
+    _pSPICommand->SetMaxFrequency(_property.operationClkHz);
+
+    writeEnable();
+    
+    char param[] = { 0, 0, 0, data };
+    fillAddress(param, address);
+    _pSPICommand->Write(0xaf, param, sizeof(param));
+
+    waitForReady();
+}
+
+void SST25DeviceImpl::nextAAIProgram(int data)
+{
+    char param = static_cast<char>(data);
+    _pSPICommand->Write(0xaf, &param, 1);
+
+    waitForReady();
+}
+
+void SST25DeviceImpl::endAAIProgram(void)
+{
+    _pSPICommand->Write(0x04);
+    waitForReady();
+}
+
+void SST25DeviceImpl::beginAAIWordProgram(int address, int data)
+{
+    _pSPICommand->SetMaxFrequency(_property.operationClkHz);
+
+    writeEnable();
+    char param[] = { 0, 0, 0, (data & 0xff00) >> 8, data & 0xff };
+    fillAddress(param, address);
+    _pSPICommand->Write(0xad, param, sizeof(param));
+    waitForReady();
+}
+
+void SST25DeviceImpl::nextAAIWordProgram(int data)
+{
+    char param[] = { (data & 0xff00) >> 8, data & 0xff };
+    _pSPICommand->Write(0xad, param, sizeof(param));
+    waitForReady();
+}
+
+void SST25DeviceImpl::fillAddress(char* pBuffer, int address)
+{
+    *(pBuffer + 0) = (address & 0xff0000) >> 16;
+    *(pBuffer + 1) = (address & 0x00ff00) >> 8;
+    *(pBuffer + 2) = (address & 0x0000ff);
+}
+
+void SST25DeviceImpl::pageProgram(int address, const void* buffer)
+{
+    _pSPICommand->SetMaxFrequency(_property.operationClkHz);
+
+    writeEnable();
+
+    char param[256 + 3];
+    fillAddress(param, address);
+    memcpy(param + 3, buffer, 256);
+    _pSPICommand->Write(0x02, param, sizeof(param));    
+
+    waitForReady();
+}
+
+void SST25DeviceImpl::writePage(int address, const void* buffer, int length)
+{   
+    int pageSize = _property.pageSize;
+    int pageSizeMask = pageSize - 1;
+    int pageAddressMask = ~pageSizeMask;
+
+    if (length <= 0)
+    {
+        return;
+    }
+    const unsigned char* p = static_cast<const unsigned char*>(buffer);
+    if ((address & pageSizeMask) != 0)
+    {
+        int readLen = address & pageSizeMask;
+        int copyLen = pageSize - readLen;
+        if (copyLen > length)
+        {
+            copyLen = length;
+        }
+        char buf[pageSize];
+        int writeAddress = address & pageAddressMask;
+        Read(writeAddress, buf, readLen);
+        memcpy(&buf[address & pageSizeMask], buffer, copyLen);
+        pageProgram(writeAddress, buf);
+        p += readLen;
+        address += pageSize - readLen;
+        length -= copyLen;
+    }
+    while (length >= pageSize)
+    {
+        pageProgram(address, p);
+        address += pageSize;
+        p += pageSize;
+        length -= pageSize;
+    }
+    if (length != 0)
+    {
+        char buf[pageSize];
+        memcpy(buf, p, length);
+        memset(&buf[length], 0xff, pageSize - length);
+        pageProgram(address, buf);
+    }
+}
+
+void SST25DeviceImpl::writeBytes(int address, const void* buffer, int length)
+{
+    const unsigned char* p = static_cast<const unsigned char*>(buffer);
+    while (length-- >= 0)
+    {
+        byteProgram(address++, *p++);
+    }
+}
+
+void SST25DeviceImpl::writeAAI(int address, const void* buffer, int length)
+{
+    if (length <= 0)
+    {
+        return;
+    }
+
+    const unsigned char* p = static_cast<const unsigned char*>(buffer);
+    if (length < 2)
+    {
+        byteProgram(address, *p);
+        return;
+    }
+
+    beginAAIProgram(address, *p++);
+    while (--length != 0)
+    {
+        nextAAIProgram(*p++);
+    }
+    endAAIProgram();
+}
+
+void SST25DeviceImpl::writeAAIWord(int address, const void* buffer, int length)
+{
+    if (length <= 0)
+    {
+        return;
+    }
+
+    const unsigned char* p = static_cast<const unsigned char*>(buffer);
+    if ((address & 0x1) != 0)
+    {
+        byteProgram(address++, *p++);
+        length--;
+    }
+
+    if (length < 4)
+    {
+        writeBytes(address, p, length);
+        return;
+    }
+
+    beginAAIWordProgram(address, (*p << 8) | *(p + 1)); 
+    address += length & ~0x1;
+    p += 2;
+    length -= 2;
+
+    do
+    {
+        nextAAIWordProgram((*p << 8) | *(p + 1));
+        p += 2;
+        length -= 2;
+    } while (length >= 2);
+    endAAIProgram();
+
+    if (length != 0)
+    {
+        byteProgram(address, *p);
+    }
+}

src/drivers/SerialFlash/SST25DeviceImpl.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include <string>
+
+class ISPICommand;
+
+class SST25DeviceImpl
+{
+public:
+    static bool IsSupported(ISPICommand* pSPICommand);
+    static SST25DeviceImpl* Create(ISPICommand* pSPICommand);
+    ~SST25DeviceImpl(void);
+    
+    std::string GetDeviceName() const;
+    int GetCapacity() const;
+    int Read(int address, void* buffer, int length);
+    int Write(int address, const void* buffer, int length);
+    void ChipErase(void);
+
+private:
+    ISPICommand* const _pSPICommand;
+    int _operationFrequency;
+    struct DeviceProperty
+    {
+        std::string deviceName;
+        int  capacity;
+        int  blockProtectionMask;
+        int  pageSize;
+        void (SST25DeviceImpl::*pfnWriter)(int address, const void* buffer, int length);
+
+        int  operationClkHz;        
+    } const& _property;
+    static const DeviceProperty* findMatchDevice(ISPICommand* pSPICommand);
+
+    SST25DeviceImpl(ISPICommand* pSPICommand, const DeviceProperty& property);
+    static void readId(ISPICommand* pSPICommand, int& manufacturersId, int& deviceId);
+    static void fillAddress(char* pBuffer, int address);
+
+    int readStatusRegister(void);
+    void writeStatusRegister(int value);
+    void writeEnable(void);
+    void writeDisable(void);
+    void waitForReady(void);
+
+    void byteProgram(int address, int value);
+    void pageProgram(int address, const void* buffer);
+    void beginAAIProgram(int address, int data);
+    void nextAAIProgram(int data);
+    void endAAIProgram(void);
+    void beginAAIWordProgram(int address, int data);
+    void nextAAIWordProgram(int data);
+
+    void writeBytes(int address, const void* buffer, int length);
+    void writeAAI(int address, const void* buffer, int length);
+    void writeAAIWord(int address, const void* buffer, int length);
+    void writePage(int address, const void* buffer, int length);
+};

src/drivers/SerialFlash/SerialFlashDeviceCreator.cpp

@@ -0,0 +1,17 @@
+#include "ISerialFlashDevice.h"
+#include "SST25Device.h"
+#include "M25PDevice.h"
+#include "SerialFlashDeviceCreator.h"
+
+ISerialFlashDevice* SerialFlashDeviceCreator::Create(ISPICommand* pSPICommand)
+{
+    if (M25PDevice::IsSupported(pSPICommand))
+    {
+        return M25PDevice::Create(pSPICommand);
+    }
+    if (SST25Device::IsSupported(pSPICommand))
+    {
+        return SST25Device::Create(pSPICommand);
+    }
+    return NULL;
+}

src/drivers/SerialFlash/SerialFlashDeviceCreator.h

@@ -0,0 +1,11 @@
+#pragma once
+
+#include "ISerialFlashDevice.h"
+
+class ISPICommand;
+
+class SerialFlashDeviceCreator
+{
+public:
+    static ISerialFlashDevice* Create(ISPICommand* pSPICommand);
+};

src/drivers/SpiFlash25.cpp

@@ -0,0 +1,189 @@
+/* Library for SPI flash 25* devices.
+ * Copyright (c) 2014 Multi-Tech Systems
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+ 
+#include "SpiFlash25.h"
+ 
+SpiFlash25::SpiFlash25(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName W, PinName HOLD, int page_size)
+:   _spi(mosi, miso, sclk),
+    _cs(cs),
+    _page_size(page_size)
+{
+ 
+    wakeup();
+ 
+    _cs.write(1);
+    _spi.format(8, 3);
+    _spi.frequency(75000000);
+ 
+    if (W != NC) {
+        _w = new DigitalOut(W);
+        _w->write(1);
+    }
+    if (HOLD != NC) {
+        _hold = new DigitalOut(HOLD);
+        _hold->write(1);
+    }
+ 
+    _cs.write(0);
+    _spi.write(READ_IDENTIFICATION);
+    _id[ID_MANUFACTURER] = _spi.write(0x00);
+    _id[ID_MEM_TYPE] = _spi.write(0x00);
+    _id[ID_MEM_SIZE] = _spi.write(0x00);
+    _cs.write(1);
+ 
+    _mem_size = 1 << _id[ID_MEM_SIZE];
+}
+ 
+void SpiFlash25::format(int bits, int mode) {
+    _spi.format(bits, mode);
+}
+ 
+void SpiFlash25::frequency(int hz) {
+    _spi.frequency(hz);
+}
+ 
+bool SpiFlash25::read(int addr, int len, char* data) {
+    if (addr + len > _mem_size) {
+        return false;
+    }
+ 
+    enable_write();
+ 
+    _cs.write(0);
+    _spi.write(READ_DATA);
+    _spi.write(high_byte(addr));
+    _spi.write(mid_byte(addr));
+    _spi.write(low_byte(addr));
+ 
+    for (int i = 0; i < len; i++) {
+        data[i] = _spi.write(0x00);
+    }
+ 
+    _cs.write(1);
+ 
+    return true;
+}
+ 
+bool SpiFlash25::write(int addr, int len, const char* data) {
+    if (addr + len > _mem_size) {
+        return false;
+    }
+ 
+    int written = 0;
+    int write_size = 0;
+ 
+    while (written < len) {
+        write_size = _page_size - ((addr + written) % _page_size);
+        if (written + write_size > len) {
+            write_size = len - written;
+        }
+ 
+        if (! write_page(addr + written, write_size, data + written)) {
+            return false;
+        }
+ 
+        written += write_size;
+    }
+ 
+    return true;
+}
+ 
+char* SpiFlash25::read_id() {
+    return _id;
+}
+ 
+char SpiFlash25::read_status() {
+    char status;
+ 
+    _cs.write(0);
+    _spi.write(READ_STATUS);
+    status = _spi.write(0x00);
+    _cs.write(1);
+ 
+    return status;
+}
+ 
+void SpiFlash25::clear_sector(int addr) {
+    enable_write();
+ 
+    _cs.write(0);
+    _spi.write(SECTOR_ERASE);
+    _spi.write(high_byte(addr));
+    _spi.write(mid_byte(addr));
+    _spi.write(low_byte(addr));
+    _cs.write(1);
+ 
+    wait_for_write();
+}
+ 
+void SpiFlash25::clear_mem() {
+    enable_write();
+ 
+    _cs.write(0);
+    _spi.write(BULK_ERASE);
+    _cs.write(1);
+ 
+    wait_for_write();
+}
+ 
+bool SpiFlash25::write_page(int addr, int len, const char* data) {
+    enable_write();
+ 
+    _cs.write(0);
+    _spi.write(PAGE_PROGRAM);
+    _spi.write(high_byte(addr));
+    _spi.write(mid_byte(addr));
+    _spi.write(low_byte(addr));
+ 
+    for (int i = 0; i < len; i++) {
+        _spi.write(data[i]);
+    }
+ 
+    _cs.write(1);
+    wait_for_write();
+ 
+    return true;
+}
+ 
+void SpiFlash25::enable_write() {
+    _cs.write(0);
+    _spi.write(WRITE_ENABLE);
+    _cs.write(1);
+}
+ 
+void SpiFlash25::wait_for_write() {
+    while (read_status() & STATUS_WIP) {
+        wait_us(10);
+    }
+}
+ 
+void SpiFlash25::deep_power_down() {
+    _cs.write(0);
+    _spi.write(DEEP_POWER_DOWN);
+    _cs.write(1);
+}
+ 
+void SpiFlash25::wakeup() {
+    _cs.write(0);
+    _spi.write(DEEP_POWER_DOWN_RELEASE);
+    _cs.write(1);
+}

src/drivers/SpiFlash25.h

@@ -0,0 +1,107 @@
+/* Library for SPI flash 25* devices.
+ * Copyright (c) 2014 Multi-Tech Systems
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+ 
+#ifndef SPIFLASH25_H
+#define SPIFLASH25_H
+ 
+#include "mbed.h"
+ 
+class SpiFlash25 {
+    public:
+        SpiFlash25(PinName mosi, PinName miso, PinName sclk, PinName cs, PinName W = NC, PinName HOLD = NC, int page_size = 256);
+ 
+        /* Set the page size (default 256) */
+        void set_page_size(int size);
+ 
+        /* Set up the internal SPI object */
+        void format(int bits, int mode);
+        void frequency(int hz);
+ 
+        /* Reads and writes can be across page boundaries */
+        bool read(int addr, int len, char* data);
+        bool write(int addr, int len, const char* data);
+ 
+        /* Read ID and status registers */
+        char* read_id();
+        char read_status();
+ 
+        /* Erase methods */
+        void clear_sector(int addr);
+        void clear_mem();
+ 
+        void deep_power_down();
+        void wakeup();
+ 
+    private:
+        enum {
+            WRITE_ENABLE                = 0x06,
+            WRITE_DISABLE               = 0x04,
+            READ_IDENTIFICATION         = 0x9F,
+            READ_STATUS                 = 0x05,
+            WRITE_STATUS                = 0x01,
+            READ_DATA                   = 0x03,
+            READ_DATA_FAST              = 0x0B,
+            PAGE_PROGRAM                = 0x02,
+            SECTOR_ERASE                = 0xD8,
+            BULK_ERASE                  = 0xC7,
+            DEEP_POWER_DOWN             = 0xB9,
+            DEEP_POWER_DOWN_RELEASE     = 0xAB,
+        };
+ 
+        enum {
+            STATUS_SRWD                 = 0x80,     // 0b 1000 0000
+            STATUS_BP2                  = 0x10,     // 0b 0001 0000
+            STATUS_BP1                  = 0x08,     // 0b 0000 1000
+            STATUS_BP0                  = 0x04,     // 0b 0000 0100
+            STATUS_WEL                  = 0x02,     // 0b 0000 0010
+            STATUS_WIP                  = 0x01,     // 0b 0000 0001
+        };
+ 
+        enum {
+            ID_MANUFACTURER             = 0,
+            ID_MEM_TYPE                 = 1,
+            ID_MEM_SIZE                 = 2,
+        };
+ 
+        bool write_page(int addr, int len, const char* data);
+        void enable_write();
+        void wait_for_write();
+ 
+        static inline char high_byte(int addr) {
+            return ((addr & 0xff0000) >> 16);
+        }
+        static inline char mid_byte(int addr) {
+            return ((addr & 0xff00) >> 8);
+        }
+        static inline char low_byte(int addr) {
+            return (addr & 0xff);
+        }
+ 
+        SPI _spi;
+        DigitalOut _cs;
+        DigitalOut* _w;
+        DigitalOut* _hold;
+        int _mem_size;
+        int _page_size;
+        char _id[3];
+};
+#endif

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(50.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/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