![](/media/cache/profiles/8158fa3e4de806e614f7ff02e7b22fde.jpg.50x50_q85.png)
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
--- 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();