Revision 4:9ffbf9101992, committed 2015-05-08

Comitter:

joe4465

Date:

Fri May 08 09:07:38 2015 +0000

Parent:

3:4823d6750629

Commit message:

End of FYP

Changed in this revision

--- a/BaseStation/BaseStation.cpp	Fri Apr 24 16:50:20 2015 +0000
+++ b/BaseStation/BaseStation.cpp	Fri May 08 09:07:38 2015 +0000
@@ -11,7 +11,7 @@
 }
 
 BaseStation::~BaseStation(){}
-
+l
 void BaseStation::threadStarter(void const *p)
 {
     BaseStation *instance = (BaseStation*)p;
@@ -133,15 +133,20 @@
         }
         else if (mode == Status::ALTITUDE)
         {
-            Sensors::Altitude altitude = _sensors.getAltitude();            
-            _wireless->printf("<GAlt=%1.2f:CAlt=%1.2f:BAlt=%1.2f:LAlt=%1.2f>",
-            altitude.gps, altitude.computed, altitude.barometer, altitude.lidar);
+            Sensors::Altitude altitude = _sensors.getAltitude();      
+            Imu::Acceleration acceleration = _sensors.getImuAcceleration();
+                  
+            _wireless->printf("<ZVEL=%1.2f:CAlt=%1.2f:BAlt=%1.2f:LAlt=%1.2f>",
+            acceleration.z, altitude.computed, altitude.barometer, altitude.lidar);
         }
         else if (mode == Status::VELOCITY)
         {
-            Imu::Velocity velocity = _sensors.getImuVelocity();
+            //Imu::Velocity velocity = _sensors.getImuVelocity();
+            //_wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>",
+            //velocity.x, velocity.y, velocity.z);  
+            Imu::Acceleration acceleration = _sensors.getImuAcceleration();
             _wireless->printf("<XVEL=%1.2f:YVEL=%1.2f:ZVEL=%1.2f>",
-            velocity.x, velocity.y, velocity.z);      
+            acceleration.x, acceleration.y, acceleration.z);   
         }
         else if (mode == Status::ALTITUDE_STATUS)
         {

--- a/Sensors/Imu/Imu.cpp	Fri Apr 24 16:50:20 2015 +0000
+++ b/Sensors/Imu/Imu.cpp	Fri May 08 09:07:38 2015 +0000
@@ -93,6 +93,20 @@
     return (normalAltitude * 100);
 }
 
+Imu::Acceleration Imu::getAcceleration()
+{
+    //Get raw accel data
+    float values[9];
+    _freeImu.getValues(values);
+    
+    Acceleration acceleration;
+    acceleration.x = values[0];
+    acceleration.y = values[1];
+    acceleration.z = values[2];
+    
+    return acceleration;
+}
+
 Imu::Velocity Imu::getVelocity(float time)
 {
     //Get quaternion

--- a/Sensors/Imu/Imu.h	Fri Apr 24 16:50:20 2015 +0000
+++ b/Sensors/Imu/Imu.h	Fri May 08 09:07:38 2015 +0000
@@ -35,12 +35,20 @@
         double z;
     };  
     
+    struct Acceleration
+    {
+        double x;
+        double y;
+        double z;
+    }; 
+    
     void enable(bool enable);
     
     Rate getRate();
     Angle getAngle(bool bias = true);
     Velocity getVelocity(float time);
     Velocity getVelocity();
+    Acceleration getAcceleration();
     double getAltitude();
     
     void zeroGyro();

--- a/Sensors/LidarLitePwm.lib	Fri Apr 24 16:50:20 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://developer.mbed.org/users/joe4465/code/LidarLitePwm/#0b1f4404cb21

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/LidarLitePwm/LidarLitePwm.cpp	Fri May 08 09:07:38 2015 +0000
@@ -0,0 +1,39 @@
+#include "LidarLitePwm.h"
+
+LidarLitePwm::LidarLitePwm(PinName pin) : _interrupt(pin)
+{
+    _pulseStartTime = 0;
+    _range = 0;
+    _lidarFilter = new filter(5);
+    _timer.start();
+    _interrupt.rise(this, &LidarLitePwm::pulseStart);
+    _interrupt.fall(this, &LidarLitePwm::pulseStop);
+}
+
+LidarLitePwm::~LidarLitePwm(){}
+
+int LidarLitePwm::read()
+{
+    //if(_range < 30) return 0;
+    //else return _range - 30;
+    
+    return _range - 10;
+}
+
+LidarLitePwm::operator int()
+{
+    return read();
+}
+
+void LidarLitePwm::pulseStart()
+{
+    _pulseStartTime = _timer.read_us();
+}
+
+void LidarLitePwm::pulseStop()
+{
+    int endTime = _timer.read_us();
+    if (endTime < _pulseStartTime) return; // Escape if there's been a roll over
+    int range = (endTime - _pulseStartTime) / 10; // 10uS per CM
+    _range = _lidarFilter->process(range);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/LidarLitePwm/LidarLitePwm.h	Fri May 08 09:07:38 2015 +0000
@@ -0,0 +1,39 @@
+#include "mbed.h"
+#include "filter.h"
+
+#ifndef LidarLitePwm_H
+#define LidarLitePwm_H
+ 
+class LidarLitePwm
+{
+  public:
+    LidarLitePwm(PinName input);
+    ~LidarLitePwm();
+    
+    /// Returns range in cm as int
+    int read();
+    
+    
+    /// Returns the range in CM as an int
+    operator int();
+    
+  private:
+
+    /// Inturrupt at start of pulse
+    void pulseStart();
+    /// Interrupt at end of pulse
+    void pulseStop();
+    
+    /// Interrupt driver for the input pin
+    InterruptIn _interrupt;
+    /// Timer
+    Timer _timer;
+    /// Time of the start of the current pulse
+    int _pulseStartTime;
+    /// The most recent sample
+    int _range;
+    
+    filter* _lidarFilter;
+};
+ 
+#endif
\ No newline at end of file

--- a/Sensors/Sensors.cpp	Fri Apr 24 16:50:20 2015 +0000
+++ b/Sensors/Sensors.cpp	Fri May 08 09:07:38 2015 +0000
@@ -70,6 +70,11 @@
     return _imu->getVelocity();
 }
 
+Imu::Acceleration Sensors::getImuAcceleration()
+{
+    return _imu->getAcceleration();
+}
+
 void Sensors::updateImu()
 {
     _angle = _imu->getAngle();
@@ -100,7 +105,7 @@
     
     //Compute estimated altitude
     float altitude = 0;
-    if(altitude.computed < 20 * 100) altitude = _altitude.lidar;
+    if(_altitude.computed < 20 * 100) altitude = _altitude.lidar;
     else altitude = _altitude.barometer;
     double estimatedAltitude = _altitudeKalmanFilter->update(predictedChange, altitude);
     

--- a/Sensors/Sensors.h	Fri Apr 24 16:50:20 2015 +0000
+++ b/Sensors/Sensors.h	Fri May 08 09:07:38 2015 +0000
@@ -43,6 +43,7 @@
     Imu::Velocity getImuVelocity();
     Sensors::Altitude getAltitude();
     Sensors::Position getPosition();
+    Imu::Acceleration getImuAcceleration();
     void enable(bool enable);
     void zeroBarometer();
     void updateImu();