Joseph Roberts
New version of quadcopter software written to OO principles
Dependencies: mbed MODSERIAL filter mbed-rtos ConfigFile PID PPM FreeIMU_external_magnetometer TinyGPS
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
diff -r 4823d6750629 -r 9ffbf9101992 BaseStation/BaseStation.cpp
--- 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)
{
diff -r 4823d6750629 -r 9ffbf9101992 Sensors/Imu/Imu.cpp
--- 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
diff -r 4823d6750629 -r 9ffbf9101992 Sensors/Imu/Imu.h
--- 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();
diff -r 4823d6750629 -r 9ffbf9101992 Sensors/LidarLitePwm.lib --- 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
diff -r 4823d6750629 -r 9ffbf9101992 Sensors/LidarLitePwm/LidarLitePwm.cpp
--- /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
diff -r 4823d6750629 -r 9ffbf9101992 Sensors/LidarLitePwm/LidarLitePwm.h
--- /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
diff -r 4823d6750629 -r 9ffbf9101992 Sensors/Sensors.cpp
--- 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);
diff -r 4823d6750629 -r 9ffbf9101992 Sensors/Sensors.h
--- 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();