From e5eafe8bf5e9bbd26f4275a2fe56ddafe0aeb556 Mon Sep 17 00:00:00 2001 From: Matthew Watson Date: Fri, 9 Nov 2012 10:09:58 +0000 Subject: [PATCH] Added timer, config file reader, plus a lot of bug fixes --- AHRS.cpp | 63 ++++++++++++------ AHRS.h | 54 +++++++++------ CommandLineInterface.cpp | 55 ++++++++++++++-- CommandLineInterface.h | 7 ++ ConfigFile.cpp | 119 ++++++++++++++++++++++++++++++++++ ConfigFile.h | 78 ++++++++++++++++++++++ Control.cpp | 28 ++++++++ Control.h | 24 +++++++ I2CInterface.cpp | 21 ++++-- I2CInterface.h | 3 +- MPU6050.cpp | 21 +++--- MPU6050.h | 18 ++--- PICInterface.cpp | 41 ++++++++---- PICInterface.h | 73 ++++++++++++--------- Timer.cpp | 75 ++++++++++++++------- Timer.h | 8 +++ main.cpp | 16 ++--- main.h | 4 ++ nbproject/Makefile-Release.mk | 12 ++++ 19 files changed, 579 insertions(+), 141 deletions(-) create mode 100644 ConfigFile.cpp create mode 100644 ConfigFile.h create mode 100644 Control.cpp create mode 100644 Control.h diff --git a/AHRS.cpp b/AHRS.cpp index edf5738..5eb53ae 100644 --- a/AHRS.cpp +++ b/AHRS.cpp @@ -7,43 +7,66 @@ #include "AHRS.h" -AHRS* AHRS::p_instance_ = NULL; //Initialise pointer -s_rawData rawData; -s_calibratedData calibratedData; +AHRSClass AHRS; -AHRS* AHRS::Instance() +AHRSClass::AHRSClass() { - if(!p_instance_) - p_instance_ = new AHRS; } -AHRS::AHRS() +AHRSClass::AHRSClass(const AHRSClass& orig) { + readConfig(); } -AHRS::AHRS(const AHRS& orig) +AHRSClass::~AHRSClass() { } -AHRS::~AHRS() +void AHRSClass::update() { + getSensors_(); + calibrateData_(); + calcAccelAngles_(); + filter_(); +} + +void AHRSClass::readConfig() +{ + zeroPoints_.x = Config.getValueOfKey("zero_x"); + zeroPoints_.y = Config.getValueOfKey("zero_y"); + zeroPoints_.z = Config.getValueOfKey("zero_z"); + zeroPoints_.p = Config.getValueOfKey("zero_p"); + zeroPoints_.q = Config.getValueOfKey("zero_q"); + zeroPoints_.r = Config.getValueOfKey("zero_r"); } //Collect data from all the sensors. Currently only MPU is collected. -void AHRS::getSensors(s_rawData* rawData) +void AHRSClass::getSensors_() { - MPU6050Interface.getSensors(rawData); + MPU6050Interface.getSensors(&rawData_); } -void AHRS::calibrateData(s_rawData* rawData, s_calibratedData* calibratedData) //Need to add scales and zero points + any required inversions +void AHRSClass::calibrateData_() //Need to add scales and zero points + any required inversions { - calibratedData->x = (rawData->x - 0) * 0; - calibratedData->y = (rawData->y - 0) * 0; - calibratedData->z = (rawData->z - 0) * 0; - calibratedData->temp = (rawData->temp - 0) * 0; - calibratedData->p = (rawData->p - 0) * 0; - calibratedData->q = (rawData->q - 0) * 0; - calibratedData->r = (rawData->r - 0) * 0; + calibratedData.x = (rawData_.x - zeroPoints_.x) * (9.81 / 2048.0); + calibratedData.y = (rawData_.y - zeroPoints_.y) * (9.81 / 2048.0); + calibratedData.z = (rawData_.z - zeroPoints_.z) * (9.81 / 2048.0); + calibratedData.temp = (rawData_.temp + 521) / 340.0; + calibratedData.p = (rawData_.p - zeroPoints_.p) / 16.4; + calibratedData.q = -(rawData_.q - zeroPoints_.q) / 16.4; + calibratedData.r = (rawData_.r - zeroPoints_.r) / 16.4; } - +void AHRSClass::calcAccelAngles_() +{ + accelAngles.phi = (180/pi)*atan2(calibratedData.y,sqrt(pow(calibratedData.x,2) + pow(calibratedData.z,2))); + accelAngles.psi = (180/pi)*atan2(calibratedData.x,sqrt(pow(calibratedData.y,2) + pow(calibratedData.z,2))); +} + +void AHRSClass::filter_() +{ + #define a 0.99 + orientation.phi = (orientation.phi + calibratedData.p * Timer.dt)*a + accelAngles.phi*(1-a); + orientation.psi = (orientation.psi + calibratedData.q * Timer.dt)*a + accelAngles.psi*(1-a); + orientation.theta += calibratedData.r * Timer.dt; +} \ No newline at end of file diff --git a/AHRS.h b/AHRS.h index 5436b7f..071c45f 100644 --- a/AHRS.h +++ b/AHRS.h @@ -10,37 +10,53 @@ #include #include +#include +#include "Timer.h" #include "MPU6050.h" +#include "ConfigFile.h" - -extern s_rawData rawData; +#define pi 3.14159265358979 struct s_calibratedData { - float x; - float y; - float z; - float temp; - float p; - float q; - float r; + double x; + double y; + double z; + double temp; + double p; + double q; + double r; }; -extern s_calibratedData calibratedData; -class AHRS +struct s_euler +{ + double phi; + double psi; + double theta; +}; + +class AHRSClass { public: - static AHRS* Instance(); - AHRS(const AHRS& orig); - virtual ~AHRS(); - void getSensors(s_rawData* rawData); - void calibrateData(s_rawData* rawData, s_calibratedData* calibratedData); -protected: - AHRS(); + AHRSClass(); + AHRSClass(const AHRSClass& orig); + virtual ~AHRSClass(); + void update(); + void readConfig(); + s_calibratedData calibratedData; + s_rawData rawData_; + s_euler orientation; + s_euler accelAngles; private: - static AHRS* p_instance_; + void getSensors_(); + void calibrateData_(); + void filter_(); + void calcAccelAngles_(); + s_rawData zeroPoints_; }; +extern AHRSClass AHRS; + #endif /* AHRS_H */ diff --git a/CommandLineInterface.cpp b/CommandLineInterface.cpp index 2c935e9..07ed415 100644 --- a/CommandLineInterface.cpp +++ b/CommandLineInterface.cpp @@ -6,6 +6,7 @@ */ #include "CommandLineInterface.h" +#include "PICInterface.h" CLI_class CLI; @@ -16,6 +17,7 @@ CLI_class::CLI_class() CLI_class::CLI_class(const CLI_class& orig) { + std::string stringbuf_[6] = {0}; } CLI_class::~CLI_class() @@ -24,37 +26,76 @@ CLI_class::~CLI_class() void CLI_class::open() { - for (std::string line; std::cout << "Picopter > " && std::getline(std::cin, line);) + while (1) { + std::string line; + std::cout << "Picopter > "; + std::cout.flush(); + std::getline(std::cin, line); std::stringstream stream(line); int i = 0; while (std::getline(stream, stringbuf_[i], ' ')) { i++; } - + switch (lineMap_[stringbuf_[0]]) { case en_stringNotDefined: - std::cout << "Invalid command" << std::endl; + std::cout << stringbuf_[0] << " isn't a valid command" << std::endl; break; + case en_openlog: std::cout << "Opening log at " << stringbuf_[1] << std::endl; LogMan.open(stringbuf_[1].c_str()); break; + case en_writelog: std::cout << "Writing " << stringbuf_[1] << " to log" << std::endl; Log << stringbuf_[1]; break; + case en_starttimer: Timer.start(); break; + + case en_readconfig: + std::cout << Config.getValueOfKey (stringbuf_[1]) << std::endl; + break; + + case en_dumpsensors: + AHRS.update(); + std::cout << AHRS.calibratedData.x << "\t" << AHRS.calibratedData.y << "\t" << AHRS.calibratedData.z << "\t" << AHRS.calibratedData.p << "\t" << AHRS.calibratedData.q << "\t" << AHRS.calibratedData.r << "\t" << AHRS.calibratedData.temp << std::endl; + break; + + case en_dumprawsensors: + AHRS.update(); + std::cout << AHRS.rawData_.x << "\t" << AHRS.rawData_.y << "\t" << AHRS.rawData_.z << "\t" << AHRS.rawData_.p << "\t" << AHRS.rawData_.q << "\t" << AHRS.rawData_.r << std::endl; + break; + + case en_dumprx: + PICInterface.getRX(); + std::cout << PICInterface.rxWidths.pitch << ", " << PICInterface.rxWidths.roll << ", " << PICInterface.rxWidths.throttle << ", " << PICInterface.rxWidths.yaw << ", " << PICInterface.rxWidths.sw1 << ", " << PICInterface.rxWidths.sw2 << ", " << std::endl; + break; + + case en_resetmpu: + MPU6050Interface.initialise(); + std::cout << "mpu reset" << std::endl; + break; + + case en_readregister: + unsigned char buf[32]; + I2CInterface.readRegister(static_cast(atoi(stringbuf_[1].c_str())), static_cast(atoi(stringbuf_[2].c_str())), buf, static_cast(atoi(stringbuf_[3].c_str()))); + std::cout << buf << std::endl; + break; + + case en_exit: exit(1); break; default: - std::cout << "Invalid command" << std::endl; + std::cout << stringbuf_[0] << " isn't a valid command" << std::endl; break; } } @@ -66,5 +107,11 @@ void CLI_class::initialiseMap_() lineMap_["openlog"] = en_openlog; lineMap_["writelog"] = en_writelog; lineMap_["starttimer"] = en_starttimer; + lineMap_["readconfig"] = en_readconfig; + lineMap_["dr"] = en_dumprawsensors; + lineMap_["ds"] = en_dumpsensors; + lineMap_["drx"] = en_dumprx; + lineMap_["resetmpu"] = en_resetmpu; + lineMap_["rr"] = en_readregister; lineMap_["exit"] = en_exit; } diff --git a/CommandLineInterface.h b/CommandLineInterface.h index 2152665..a8410ea 100644 --- a/CommandLineInterface.h +++ b/CommandLineInterface.h @@ -19,6 +19,7 @@ #include "Logger.h" #include "Timer.h" +#include "ConfigFile.h" enum lineString @@ -27,6 +28,12 @@ enum lineString en_openlog, en_writelog, en_starttimer, + en_readconfig, + en_dumpsensors, + en_dumprawsensors, + en_dumprx, + en_resetmpu, + en_readregister, en_exit }; diff --git a/ConfigFile.cpp b/ConfigFile.cpp new file mode 100644 index 0000000..db6efe0 --- /dev/null +++ b/ConfigFile.cpp @@ -0,0 +1,119 @@ +/* + * File: ConfigFile.cpp + * Author: matt + * + * Created on 05 November 2012, 14:41 + */ + +#include "ConfigFile.h" + +std::map contents; +std::string fName; + +ConfigFile Config("picopter.config"); + +void ConfigFile::removeComment(std::string &line) const +{ + if (line.find(';') != line.npos) + line.erase(line.find(';')); +} + +bool ConfigFile::onlyWhitespace(const std::string &line) const +{ + return (line.find_first_not_of(' ') == line.npos); +} + +bool ConfigFile::validLine(const std::string &line) const +{ + std::string temp = line; + temp.erase(0, temp.find_first_not_of("\t ")); + if (temp[0] == '=') + return false; + + for (size_t i = temp.find('=') + 1; i < temp.length(); i++) + if (temp[i] != ' ') + return true; + + return false; +} + +void ConfigFile::extractKey(std::string &key, size_t const &sepPos, const std::string &line) const +{ + key = line.substr(0, sepPos); + if (key.find('\t') != line.npos || key.find(' ') != line.npos) + key.erase(key.find_first_of("\t ")); +} + +void ConfigFile::extractValue(std::string &value, size_t const &sepPos, const std::string &line) const +{ + value = line.substr(sepPos + 1); + value.erase(0, value.find_first_not_of("\t ")); + value.erase(value.find_last_not_of("\t ") + 1); +} + +void ConfigFile::extractContents(const std::string &line) +{ + std::string temp = line; + temp.erase(0, temp.find_first_not_of("\t ")); + size_t sepPos = temp.find('='); + + std::string key, value; + extractKey(key, sepPos, temp); + extractValue(value, sepPos, temp); + + if (!keyExists(key)) + contents.insert(std::pair (key, value)); + else + std::cout << "CFG: Can only have unique key names!\n" << std::endl; +} + +void ConfigFile::parseLine(const std::string &line, size_t const lineNo) +{ + if (line.find('=') == line.npos) + std::cout << "CFG: Couldn't find separator on line: " << T_to_string(lineNo) << "\n" << std::endl; + + if (!validLine(line)) + std::cout << "CFG: Bad format for line: " << T_to_string(lineNo) << "\n" << std::endl; + + extractContents(line); +} + +void ConfigFile::ExtractKeys() +{ + std::ifstream file; + file.open(fName.c_str()); + if (!file) + std::cout << "CFG: File " << fName << " couldn't be found!\n" << std::endl; + + std::string line; + size_t lineNo = 0; + while (std::getline(file, line)) + { + lineNo++; + std::string temp = line; + + if (temp.empty()) + continue; + + removeComment(temp); + if (onlyWhitespace(temp)) + continue; + + parseLine(temp, lineNo); + } + + file.close(); +} + +ConfigFile::ConfigFile(const std::string &fName) +{ + this->fName = fName; + ExtractKeys(); +} + +bool ConfigFile::keyExists(const std::string &key) const +{ + return contents.find(key) != contents.end(); +} + + diff --git a/ConfigFile.h b/ConfigFile.h new file mode 100644 index 0000000..dc9fcd2 --- /dev/null +++ b/ConfigFile.h @@ -0,0 +1,78 @@ +/* + * File: ConfigFile.h + * Author: matt + * + * Created on 05 November 2012, 14:41 + */ + +#ifndef CONFIGFILE_H +#define CONFIGFILE_H + +#include +#include +#include +#include +#include +#include + +template +static std::string T_to_string(T const &val) +{ + std::ostringstream ostr; + ostr << val; + + return ostr.str(); +} + +template +static T string_to_T(std::string const &val) +{ + std::istringstream istr(val); + T returnVal; + if (!(istr >> returnVal)) + std::cout << "CFG: Not a valid " << (std::string)typeid (T).name() << " received!\n" << std::endl; + + return returnVal; +} + +template <> +std::string string_to_T(std::string const &val) +{ + return val; +} + +class ConfigFile +{ +private: + std::map contents; + std::string fName; + void removeComment(std::string &line) const; + bool onlyWhitespace(const std::string &line) const; + bool validLine(const std::string &line) const; + void extractKey(std::string &key, size_t const &sepPos, const std::string &line) const; + void extractValue(std::string &value, size_t const &sepPos, const std::string &line) const; + void extractContents(const std::string &line); + void parseLine(const std::string &line, size_t const lineNo); + void ExtractKeys(); +public: + ConfigFile(const std::string &fName); + bool keyExists(const std::string &key) const; + template + ValueType getValueOfKey(const std::string &key, ValueType const &defaultValue = ValueType()) const; + + +}; + +template +ValueType ConfigFile::getValueOfKey(const std::string &key, ValueType const &defaultValue) const +{ + if (!keyExists(key)) + return defaultValue; + + return string_to_T (contents.find(key)->second); +} + +extern ConfigFile Config; + +#endif /* CONFIGFILE_H */ + diff --git a/Control.cpp b/Control.cpp new file mode 100644 index 0000000..28e32a7 --- /dev/null +++ b/Control.cpp @@ -0,0 +1,28 @@ +/* + * File: Control.cpp + * Author: matt + * + * Created on 08 November 2012, 16:05 + */ + +#include "Control.h" +#include "PICInterface.h" + +ControlClass::ControlClass() +{ +} + +ControlClass::ControlClass(const ControlClass& orig) +{ +} + +ControlClass::~ControlClass() +{ +} + +void ControlClass::update() +{ + PICInterface.getRX(); +} + +//void ControlClass::getRanges \ No newline at end of file diff --git a/Control.h b/Control.h new file mode 100644 index 0000000..d178f81 --- /dev/null +++ b/Control.h @@ -0,0 +1,24 @@ +/* + * File: Control.h + * Author: matt + * + * Created on 08 November 2012, 16:05 + */ + +#ifndef CONTROL_H +#define CONTROL_H + +class ControlClass +{ +public: + ControlClass(); + ControlClass(const ControlClass& orig); + virtual ~ControlClass(); + + void update(); +private: + +}; + +#endif /* CONTROL_H */ + diff --git a/I2CInterface.cpp b/I2CInterface.cpp index 717d44d..794b5fc 100644 --- a/I2CInterface.cpp +++ b/I2CInterface.cpp @@ -22,22 +22,35 @@ I2CInterfaceClass::~I2CInterfaceClass() void I2CInterfaceClass::openInterface() { + pthread_mutex_lock (&I2Cmutex_); if ((file_ = open(filename_, O_RDWR)) < 0) { std::cout << "Failed to open the i2c bus at path " << filename_ << std::endl; } + pthread_mutex_unlock (&I2Cmutex_); } -bool I2CInterfaceClass::writeRegister(unsigned char address, unsigned char registerAddress, unsigned char buf[], unsigned char len) + +bool I2CInterfaceClass::writeRegister(unsigned char slaveAddress, unsigned char registerAddress, unsigned char* buf, unsigned char len) { - setSlaveAddress_(address); - i2c_smbus_write_block_data(file_, registerAddress, len, buf); + pthread_mutex_lock (&I2Cmutex_); + setSlaveAddress_(slaveAddress); + if (i2c_smbus_write_i2c_block_data(file_, registerAddress, len, buf) < 0) + { + std::cout << "I2C write failed" << std::endl; + } + pthread_mutex_unlock (&I2Cmutex_); } bool I2CInterfaceClass::readRegister(unsigned char slaveAddress, unsigned char registerAddress, unsigned char* buf, unsigned char len) { + pthread_mutex_lock (&I2Cmutex_); setSlaveAddress_(slaveAddress); - i2c_smbus_read_i2c_block_data(file_, registerAddress, len, buf); + if (i2c_smbus_read_i2c_block_data(file_, registerAddress, len, buf) != len) + { + std::cout << "Incorrect number of bytes read" << std::endl; + } + pthread_mutex_unlock (&I2Cmutex_); } void I2CInterfaceClass::setSlaveAddress_(unsigned char address) diff --git a/I2CInterface.h b/I2CInterface.h index d54bb6f..808261e 100644 --- a/I2CInterface.h +++ b/I2CInterface.h @@ -27,7 +27,7 @@ class I2CInterfaceClass public: I2CInterfaceClass(); virtual ~I2CInterfaceClass(); - bool writeRegister(unsigned char address, unsigned char registerAddress, unsigned char buf[], unsigned char len); + bool writeRegister(unsigned char slaveAddress, unsigned char registerAddress, unsigned char* buf, unsigned char len); bool readRegister(unsigned char slaveAddress, unsigned char registerAddress, unsigned char* buf, unsigned char len); void openInterface(); @@ -35,6 +35,7 @@ class I2CInterfaceClass void setSlaveAddress_(unsigned char address); int file_; char filename_[20]; + pthread_mutex_t I2Cmutex_; }; extern I2CInterfaceClass I2CInterface; diff --git a/MPU6050.cpp b/MPU6050.cpp index b46f8bd..57b9e5f 100644 --- a/MPU6050.cpp +++ b/MPU6050.cpp @@ -11,7 +11,7 @@ MPU6050Class MPU6050Interface; MPU6050Class::MPU6050Class() { - initialise_(); + initialise(); } MPU6050Class::~MPU6050Class() @@ -47,35 +47,40 @@ bool MPU6050Class::checkConnection() } } -void MPU6050Class::initialise_() +void MPU6050Class::initialise() { - setPowerManagement1_(MPU6050_CLOCK_PLL_ZGYRO); //Clear sleep bit and set clock source + setPowerManagement1_(MPU6050_CLOCK_PLL_XGYRO); //Clear sleep bit and set clock source setSampleRateDivider_(0); setGyroConfig_(MPU6050_GYRO_FS_2000); setAccelConfig_(MPU6050_ACCEL_FS_16); } -bool MPU6050Class::setSampleRateDivider_(unsigned char value) +int MPU6050Class::setSampleRateDivider_(unsigned char value) { return(I2CInterface.writeRegister(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, &value, 1)); } -bool MPU6050Class::setDLPFConfig_(unsigned char cutoff) +int MPU6050Class::setDLPFConfig_(unsigned char cutoff) { return(I2CInterface.writeRegister(MPU6050_ADDRESS, MPU6050_RA_CONFIG, &cutoff, 1)); } -bool MPU6050Class::setGyroConfig_(unsigned char config) +int MPU6050Class::setGyroConfig_(unsigned char config) { return(I2CInterface.writeRegister(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, &config, 1)); } -bool MPU6050Class::setAccelConfig_(unsigned char config) +int MPU6050Class::setAccelConfig_(unsigned char config) { return(I2CInterface.writeRegister(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, &config, 1)); } -bool MPU6050Class::setPowerManagement1_(unsigned char config) +int MPU6050Class::setPowerManagement1_(unsigned char config) { return(I2CInterface.writeRegister(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, &config, 1)); +} + +int MPU6050Class::setPowerManagement2_(unsigned char config) +{ + return(I2CInterface.writeRegister(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, &config, 1)); } \ No newline at end of file diff --git a/MPU6050.h b/MPU6050.h index 6745ff4..db4265d 100644 --- a/MPU6050.h +++ b/MPU6050.h @@ -26,17 +26,19 @@ struct s_rawData class MPU6050Class { public: - bool getSensors(s_rawData* rawData); - bool checkConnection(); MPU6050Class(); virtual ~MPU6050Class(); + + bool getSensors(s_rawData* rawData); + bool checkConnection(); + void initialise(); private: - void initialise_(); - bool setSampleRateDivider_(unsigned char value); - bool setDLPFConfig_(unsigned char cutoff); - bool setGyroConfig_(unsigned char config); - bool setAccelConfig_(unsigned char config); - bool setPowerManagement1_(unsigned char config); + int setSampleRateDivider_(unsigned char value); + int setDLPFConfig_(unsigned char cutoff); + int setGyroConfig_(unsigned char config); + int setAccelConfig_(unsigned char config); + int setPowerManagement1_(unsigned char config); + int setPowerManagement2_(unsigned char config); unsigned char buf_[32]; }; diff --git a/PICInterface.cpp b/PICInterface.cpp index 972fb30..7fc8565 100644 --- a/PICInterface.cpp +++ b/PICInterface.cpp @@ -21,21 +21,38 @@ PICInterfaceClass::~PICInterfaceClass() void PICInterfaceClass::setPWM(uint16_t widths[]) { - uint8_t widthsChar[sizeof(widths)*2]; - for (int x = 0; x < sizeof(widths); x++) + uint8_t widthsChar[sizeof (widths)*2]; + for (int x = 0; x < sizeof (widths); x++) { - widthsChar[x*2] = static_cast (0xFF & (widths[x] >> 8)); - widthsChar[(x*2)+1] = static_cast (0xFF & widths[x]); + widthsChar[x * 2] = static_cast (0xFF & (widths[x] >> 8)); + widthsChar[(x * 2) + 1] = static_cast (0xFF & widths[x]); } - I2CInterface.writeRegister(PIC_ADDRESS, REG_PWM1H, widthsChar, sizeof(widthsChar)); + I2CInterface.writeRegister(PIC_ADDRESS, REG_PWM1H, widthsChar, sizeof (widthsChar)); + unsigned char temp = 0x01; + I2CInterface.writeRegister(PIC_ADDRESS, REG_PWMFIRE, &temp, 1); } -void PICInterfaceClass::getRX(uint16_t widths[]) +void PICInterfaceClass::getRX() { - uint8_t widthsChar[NUMBER_RX_CHANNELS * 2]; - I2CInterface.readRegister(PIC_ADDRESS, REG_RX1H, widthsChar, NUMBER_RX_CHANNELS * 2); - for (int x = 0; x < NUMBER_RX_CHANNELS; x++) - { - widths[x] = static_cast ((widthsChar[x * 2] << 8) | widthsChar[(x * 2) + 1]); - } + uint8_t widthsChar[12] = {0}; + + // I2CInterface.readRegister(PIC_ADDRESS, REG_RX1H, widthsChar, sizeof(widthsChar)); + I2CInterface.readRegister(PIC_ADDRESS, REG_RX1H, &widthsChar[0], 2); + I2CInterface.readRegister(PIC_ADDRESS, REG_RX2H, &widthsChar[2], 2); + I2CInterface.readRegister(PIC_ADDRESS, REG_RX3H, &widthsChar[4], 2); + I2CInterface.readRegister(PIC_ADDRESS, REG_RX4H, &widthsChar[6], 2); + I2CInterface.readRegister(PIC_ADDRESS, REG_RX5H, &widthsChar[8], 2); + I2CInterface.readRegister(PIC_ADDRESS, REG_RX6H, &widthsChar[10], 2); + rxWidths.roll = make16_(widthsChar[0], widthsChar[1]); + rxWidths.pitch = make16_(widthsChar[2], widthsChar[3]); + rxWidths.throttle = make16_(widthsChar[4], widthsChar[5]); + rxWidths.yaw = make16_(widthsChar[6], widthsChar[7]); + rxWidths.sw1 = make16_(widthsChar[8], widthsChar[9]); + rxWidths.sw2 = make16_(widthsChar[10], widthsChar[11]); + +} + +inline uint16_t PICInterfaceClass::make16_(uint8_t H, uint8_t L) +{ + return ((static_cast (H) << 8) | static_cast (L)); } \ No newline at end of file diff --git a/PICInterface.h b/PICInterface.h index c9cc17a..85170d2 100644 --- a/PICInterface.h +++ b/PICInterface.h @@ -14,44 +14,57 @@ #include "I2CInterface.h" const uint8_t PIC_ADDRESS = 0x50; -const uint8_t NUMBER_PWM_CHANNELS = 6; -const uint8_t NUMBER_RX_CHANNELS = 6; //Simulated register names and addresses -const uint8_t REG_PWM1H = 0; -const uint8_t REG_PWM1L = 1; -const uint8_t REG_PWM2H = 2; -const uint8_t REG_PWM2L = 3; -const uint8_t REG_PWM3H = 4; -const uint8_t REG_PWM3L = 5; -const uint8_t REG_PWM4H = 6; -const uint8_t REG_PWM4L = 7; -const uint8_t REG_PWM5H = 8; -const uint8_t REG_PWM5L = 9; -const uint8_t REG_PWM6H = 10; -const uint8_t REG_PWM6L = 11; -const uint8_t REG_PWMFIRE = 12; -const uint8_t REG_RX1H = 13; -const uint8_t REG_RX1L = 14; -const uint8_t REG_RX2H = 15; -const uint8_t REG_RX2L = 16; -const uint8_t REG_RX3H = 17; -const uint8_t REG_RX3L = 18; -const uint8_t REG_RX4H = 19; -const uint8_t REG_RX4L = 20; -const uint8_t REG_RX5H = 21; -const uint8_t REG_RX5L = 22; -const uint8_t REG_RX6H = 23; -const uint8_t REG_RX6L = 24; +const uint8_t REG_PWM1H = 0x00; +const uint8_t REG_PWM1L = 0x01; +const uint8_t REG_PWM2H = 0x02; +const uint8_t REG_PWM2L = 0x03; +const uint8_t REG_PWM3H = 0x04; +const uint8_t REG_PWM3L = 0x05; +const uint8_t REG_PWM4H = 0x06; +const uint8_t REG_PWM4L = 0x07; +const uint8_t REG_PWM5H = 0x08; +const uint8_t REG_PWM5L = 0x09; +const uint8_t REG_PWM6H = 0x0a; +const uint8_t REG_PWM6L = 0x0b; +const uint8_t REG_PWMFIRE = 0x0c; +const uint8_t REG_RX1H = 0x0d; +const uint8_t REG_RX1L = 0x0e; +const uint8_t REG_RX2H = 0x0f; +const uint8_t REG_RX2L = 0x10; +const uint8_t REG_RX3H = 0x11; +const uint8_t REG_RX3L = 0x12; +const uint8_t REG_RX4H = 0x13; +const uint8_t REG_RX4L = 0x14; +const uint8_t REG_RX5H = 0x15; +const uint8_t REG_RX5L = 0x16; +const uint8_t REG_RX6H = 0x17; +const uint8_t REG_RX6L = 0x18; + +struct s_rxWidths +{ + uint16_t roll; + uint16_t pitch; + uint16_t throttle; + uint16_t yaw; + uint16_t sw1; + uint16_t sw2; +}; class PICInterfaceClass { public: - void setPWM(uint16_t widths[]); - void getRX(uint16_t widths[]); PICInterfaceClass(); virtual ~PICInterfaceClass(); -private: + + void setPWM(uint16_t widths[]); + void getRX(); + + s_rxWidths rxWidths; + +private: + uint16_t make16_(uint8_t H, uint8_t L); }; diff --git a/Timer.cpp b/Timer.cpp index 839ee7b..2d227f8 100644 --- a/Timer.cpp +++ b/Timer.cpp @@ -6,29 +6,12 @@ */ #include "Timer.h" +#include "PICInterface.h" TimerClass Timer; TimerClass::TimerClass() { -} - -TimerClass::TimerClass(const TimerClass& orig) -{ -} - -TimerClass::~TimerClass() -{ -} - -void TimerClass::start() -{ - timer_t timerId; /* id for the posix timer */ - struct itimerspec timeToSet; /* time to be set */ - struct timespec timeValue; /* timer expiration value */ - struct timespec timeInterval; /* timer period */ - struct sigaction signalAction; /* signal action handler struct */ - /* Intialize sigaction struct */ signalAction.sa_handler = &sig_handler_; @@ -38,27 +21,73 @@ void TimerClass::start() /* Initialize timer period */ timeInterval.tv_sec = 0; - timeInterval.tv_nsec = 2500000; + timeInterval.tv_nsec = 0; /* Set the time to be set value */ timeToSet.it_value = timeValue; timeToSet.it_interval = timeInterval; - /* Connect a signal handler routine to the SIGALRM siganl */ + /* Connect a signal handler routine to the SIGALRM signal */ sigaction(SIGALRM, &signalAction, NULL); /* Allocate a timer */ timer_create(CLOCK_REALTIME, NULL, &timerId); +} +TimerClass::TimerClass(const TimerClass& orig) +{ +} + +TimerClass::~TimerClass() +{ +} + +void TimerClass::start() +{ timer_settime(timerId, 0, &timeToSet, NULL); } void TimerClass::sig_handler_(int signum) { + //uint16_t widths[] = {100,200,400,800,1600,3200}; + //PICInterface.setPWM(widths); + static timespec oldtime; static timespec time; + static timespec iterationtime; oldtime = time; - clock_gettime (CLOCK_REALTIME, &time); - MPU6050Interface.getSensors(&rawData); - Log << (time.tv_nsec - oldtime.tv_nsec)/1000000.0 << "\t\t" << rawData.x << "\t" << rawData.y << "\t" << rawData.z << std::endl; + clock_gettime(CLOCK_MONOTONIC, &time); + Timer.dt = ((time.tv_sec * 1000000000 + time.tv_nsec) - (oldtime.tv_sec * 1000000000 + oldtime.tv_nsec)) / 1000000000.0; + + AHRS.update(); + Log.precision(3); + Log.setf(std::fstream::fixed, std::fstream::floatfield); + Log << Timer.dt * 1000 << ", " + << AHRS.calibratedData.x << ", " + << AHRS.calibratedData.y << ", " + << AHRS.calibratedData.z << ", " + << AHRS.calibratedData.p << ", " + << AHRS.calibratedData.q << ", " + << AHRS.calibratedData.r << ", " + << AHRS.calibratedData.temp << ", " + << AHRS.accelAngles.phi << ", " + << AHRS.accelAngles.psi << ", " + << AHRS.orientation.phi << ", " + << AHRS.orientation.psi << ", " + << AHRS.orientation.theta << std::endl; + //Log.flush(); + + + +// Log << AHRS.rawData_.p << ", " +// << AHRS.rawData_.q << ", " +// << AHRS.rawData_.r << ", " < #include #include +#include #include "MPU6050.h" #include "AHRS.h" @@ -28,6 +29,13 @@ class TimerClass virtual ~TimerClass(); void start(); + float dt; + timer_t timerId; /* id for the posix timer */ + struct itimerspec timeToSet; /* time to be set */ + struct timespec timeValue; /* timer expiration value */ + struct timespec timeInterval; /* timer period */ + struct sigaction signalAction; /* signal action handler struct */ + private: static void sig_handler_(int signum); diff --git a/main.cpp b/main.cpp index eb1c621..693bce9 100644 --- a/main.cpp +++ b/main.cpp @@ -5,14 +5,8 @@ * Created on 21 October 2012, 22:26 */ -#include #include "main.h" -#include "CommandLineInterface.h" -#include "Timer.h" - -using namespace std; - /* * */ @@ -24,20 +18,18 @@ void* thunk(void* p) return 0; } + int main(int argc, char** argv) { - if (!MPU6050Interface.checkConnection()) exit(1); - + MPU6050Interface.initialise(); //Create CLI thread + open pthread_t CLIthread; pthread_create(&CLIthread, NULL, thunk, &CLI); - - - + //Timer.start(); while (1) { + sleep(1); } - return 0; } diff --git a/main.h b/main.h index 309e908..da614a0 100644 --- a/main.h +++ b/main.h @@ -12,6 +12,9 @@ #include "MPU6050.h" #include "Logger.h" #include "AHRS.h" +#include "CommandLineInterface.h" +#include "Timer.h" +#include "ConfigFile.h" #include @@ -21,3 +24,4 @@ #include #endif /* MAIN_H */ + diff --git a/nbproject/Makefile-Release.mk b/nbproject/Makefile-Release.mk index ec76891..0a53d7d 100644 --- a/nbproject/Makefile-Release.mk +++ b/nbproject/Makefile-Release.mk @@ -38,8 +38,10 @@ OBJECTFILES= \ ${OBJECTDIR}/CommandLineInterface.o \ ${OBJECTDIR}/main.o \ ${OBJECTDIR}/I2CInterface.o \ + ${OBJECTDIR}/Control.o \ ${OBJECTDIR}/PICInterface.o \ ${OBJECTDIR}/MPU6050.o \ + ${OBJECTDIR}/ConfigFile.o \ ${OBJECTDIR}/Logger.o \ ${OBJECTDIR}/Timer.o @@ -88,6 +90,11 @@ ${OBJECTDIR}/I2CInterface.o: nbproject/Makefile-${CND_CONF}.mk I2CInterface.cpp ${RM} $@.d $(COMPILE.cc) -O2 -I/opt/raspberrypi/tools/arm-bcm2708/arm-bcm2708hardfp-linux-gnueabi/arm-bcm2708hardfp-linux-gnueabi/sysroot/usr/include -MMD -MP -MF $@.d -o ${OBJECTDIR}/I2CInterface.o I2CInterface.cpp +${OBJECTDIR}/Control.o: nbproject/Makefile-${CND_CONF}.mk Control.cpp + ${MKDIR} -p ${OBJECTDIR} + ${RM} $@.d + $(COMPILE.cc) -O2 -I/opt/raspberrypi/tools/arm-bcm2708/arm-bcm2708hardfp-linux-gnueabi/arm-bcm2708hardfp-linux-gnueabi/sysroot/usr/include -MMD -MP -MF $@.d -o ${OBJECTDIR}/Control.o Control.cpp + ${OBJECTDIR}/PICInterface.o: nbproject/Makefile-${CND_CONF}.mk PICInterface.cpp ${MKDIR} -p ${OBJECTDIR} ${RM} $@.d @@ -98,6 +105,11 @@ ${OBJECTDIR}/MPU6050.o: nbproject/Makefile-${CND_CONF}.mk MPU6050.cpp ${RM} $@.d $(COMPILE.cc) -O2 -I/opt/raspberrypi/tools/arm-bcm2708/arm-bcm2708hardfp-linux-gnueabi/arm-bcm2708hardfp-linux-gnueabi/sysroot/usr/include -MMD -MP -MF $@.d -o ${OBJECTDIR}/MPU6050.o MPU6050.cpp +${OBJECTDIR}/ConfigFile.o: nbproject/Makefile-${CND_CONF}.mk ConfigFile.cpp + ${MKDIR} -p ${OBJECTDIR} + ${RM} $@.d + $(COMPILE.cc) -O2 -I/opt/raspberrypi/tools/arm-bcm2708/arm-bcm2708hardfp-linux-gnueabi/arm-bcm2708hardfp-linux-gnueabi/sysroot/usr/include -MMD -MP -MF $@.d -o ${OBJECTDIR}/ConfigFile.o ConfigFile.cpp + ${OBJECTDIR}/Logger.o: nbproject/Makefile-${CND_CONF}.mk Logger.cpp ${MKDIR} -p ${OBJECTDIR} ${RM} $@.d