Quadcopter software. Flying - Contact me for more details or a copy of the PC ground station code
Dependencies: ConfigFile PID PPM MODSERIAL mbed-rtos mbed MaxbotixDriver TinyGPS filter
hardware.h
- Committer:
- joe4465
- Date:
- 2015-02-22
- Revision:
- 9:7b194f83e567
- Parent:
- 7:bc5822aa8878
File content as of revision 9:7b194f83e567:
#include "mbed.h" #include "rtos.h" #include "FreeIMU.h" #include "PID.h" #include "ConfigFile.h" #include "PPM.h" #include <sstream> #include <TinyGPS.h> #include "sonar.h" #include "MODSERIAL.h" #include "filter.h" #ifndef HARDWARE_H #define HARDWARE_H //Global constants #define IMU_YAW_ANGLE_MAX 180 #define IMU_YAW_ANGLE_MIN -180 #define IMU_ROLL_ANGLE_MAX 90 #define IMU_ROLL_ANGLE_MIN -90 #define IMU_PITCH_ANGLE_MAX 90 #define IMU_PITCH_ANGLE_MIN -90 #define IMU_YAW_RATE_MAX 360 #define IMU_YAW_RATE_MIN -360 #define IMU_ROLL_RATE_MAX 360 #define IMU_ROLL_RATE_MIN -360 #define IMU_PITCH_RATE_MAX 360 #define IMU_PITCH_RATE_MIN -360 #define RC_CHANNELS 8 #define RC_THROTTLE_CHANNEL 3 #define RC_IN_MAX 1900 #define RC_IN_MIN 1000 #define RC_OUT_MAX 1 #define RC_OUT_MIN 0 #define RC_YAW_RATE_MAX 180 #define RC_YAW_RATE_MIN -180 #define RC_ROLL_RATE_MAX 90 #define RC_ROLL_RATE_MIN -90 #define RC_PITCH_RATE_MAX 90 #define RC_PITCH_RATE_MIN -90 #define RC_ROLL_ANGLE_MAX 20 #define RC_ROLL_ANGLE_MIN -20 #define RC_PITCH_ANGLE_MAX 20 #define RC_PITCH_ANGLE_MIN -20 #define RC_THRUST_MAX 1 #define RC_THRUST_MIN 0 #define MOTORS_OFF 0 #define MOTORS_ARMED 1000 #define MOTORS_MIN 1060 #define MOTORS_MAX 1860 #define RATE_PID_CONTROLLER_OUTPUT_MAX 100 #define RATE_PID_CONTROLLER_OUTPUT_MIN -100 #define FLIGHT_CONTROLLER_FREQUENCY 500 //Global Functions //void ZeroPitchRoll(); void Arm(); void Disarm(); void WriteSettingsToConfig(); void ConvertToCharArray(float number); void ConvertToCharArray(int number); float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax); //Global Variables float _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD; float _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD; float _zeroValues[3] = {0,0,0}; //Yaw, pitch, roll float _oldZeroValues[3] = {0,0,0}; //Yaw, pitch, roll float _rcCommands[8] = {0,0,0,0,0,0,0,0}; float _rcMappedCommands[4] = {0,0,0,0}; //Yaw, pitch, roll, thrust double _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed bool _gpsConnected = false; bool _armed = false; bool _rate = false; bool _stab = true; bool _initialised = false; float _motorPower [4] = {0,0,0,0}; float _gyroRate[3] = {0,0,0}; // Yaw, Pitch, Roll float _ypr[3] = {0,0,0}; // Yaw, pitch, roll float _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll float _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll bool _levelOffset = false; int _commsMode = 0; int _batt = 0; float _yawTarget = 0; float _maxBotixPingAltitude = 0; float _barometerAltitude = 0; //PID controllers PID *_yawRatePIDController; PID *_pitchRatePIDController; PID *_rollRatePIDController; PID *_yawStabPIDController; PID *_pitchStabPIDController; PID *_rollStabPIDController; //Threads Thread *_statusThread; Thread *_serialPortMonitorThread; Thread *_flightControllerThread; Thread *_rcCommandMonitorThread; Thread *_altitudeMonitorThread; //Config file LocalFileSystem local("local"); ConfigFile _configFile; char* _str = new char[1024]; //RC filters medianFilter *_yawMedianFilter; medianFilter *_pitchMedianFilter; medianFilter *_rollMedianFilter; medianFilter *_thrustMedianFilter; //HARDWARE//////////////////////////////////////////////////////////////////////////////////// // M1 M2 // \ / // \/ // /\ // / \ // M3 M4 //Motors PwmOut _motor1(p21); PwmOut _motor2(p22); PwmOut _motor3(p23); PwmOut _motor4(p24); //USB serial MODSERIAL _wiredSerial(USBTX, USBRX); //Wireless Serial MODSERIAL _wirelessSerial(p9, p10); //GPS Serial MODSERIAL _gps(p13, p14); TinyGPS _tinyGPS; //PPM in PPM *_ppm; InterruptIn *_interruptPin = new InterruptIn(p8); //Onboard LED's DigitalOut _led1(LED1); DigitalOut _led2(LED2); DigitalOut _led3(LED3); DigitalOut _led4(LED4); //IMU FreeIMU _freeIMU; //Buzzer DigitalOut _buzzer(p20); //MaxBotix Ping sensor Timer _maxBotixTimer; Sonar _maxBotixSensor(p15, _maxBotixTimer); //Unused analog pins DigitalOut _spare1(p16); DigitalOut _spare2(p17); DigitalOut _spare3(p18); DigitalOut _spare4(p19); //Functions/////////////////////////////////////////////////////////////////////////////////////////////// //Zero gyro and arm void Arm() { //Don't arm unless throttle is equal to 0 and the transmitter is connected if(_rcMappedCommands[3] < (RC_THRUST_MIN + 0.2) && _rcMappedCommands[3] != -1 && _armed == false) { //Zero gyro _freeIMU.zeroGyro(); //Set armed to true _armed = true; } } //Disarm void Disarm() { if(_armed == true) { //Set armed to false _armed = false; //Disable modes _levelOffset = false; //Save settings WriteSettingsToConfig(); } } //Zero pitch and roll /*void ZeroPitchRoll() { printf("Zeroing pitch and roll\r\n"); //Zero pitch and roll float totalPitch = 0; float totalRoll = 0; float ypr[3] = {0,0,0}; // Yaw, pitch, roll for(int i = 0; i < 500; i++) { _freeIMU.getYawPitchRoll(ypr); totalPitch += ypr[1]; totalRoll += ypr[2]; } _zeroValues[1] = totalPitch/500; _zeroValues[2] = totalRoll/500; printf("Pitch %f\r\n", _zeroValues[1]); printf("Roll %f\r\n", _zeroValues[2]); } */ //Saves settings to config file void WriteSettingsToConfig() { _wiredSerial.printf("Writing settings to config file\n\r"); if(_armed == false) //Not flying { _freeIMU.sample(false); //Write values ConvertToCharArray(_yawRatePIDControllerP); if (!_configFile.setValue("_yawRatePIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r"); } ConvertToCharArray(_yawRatePIDControllerI); if (!_configFile.setValue("_yawRatePIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r"); } ConvertToCharArray(_yawRatePIDControllerD); if (!_configFile.setValue("_yawRatePIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r"); } ConvertToCharArray(_pitchRatePIDControllerP); if (!_configFile.setValue("_pitchRatePIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r"); } ConvertToCharArray(_pitchRatePIDControllerI); if (!_configFile.setValue("_pitchRatePIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r"); } ConvertToCharArray(_pitchRatePIDControllerD); if (!_configFile.setValue("_pitchRatePIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r"); } ConvertToCharArray(_rollRatePIDControllerP); if (!_configFile.setValue("_rollRatePIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r"); } ConvertToCharArray(_rollRatePIDControllerI); if (!_configFile.setValue("_rollRatePIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r"); } ConvertToCharArray(_rollRatePIDControllerD); if (!_configFile.setValue("_rollRatePIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r"); } ConvertToCharArray(_yawStabPIDControllerP); if (!_configFile.setValue("_yawStabPIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r"); } ConvertToCharArray(_yawStabPIDControllerI); if (!_configFile.setValue("_yawStabPIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r"); } ConvertToCharArray(_yawStabPIDControllerD); if (!_configFile.setValue("_yawStabPIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r"); } ConvertToCharArray(_pitchStabPIDControllerP); if (!_configFile.setValue("_pitchStabPIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r"); } ConvertToCharArray(_pitchStabPIDControllerI); if (!_configFile.setValue("_pitchStabPIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r"); } ConvertToCharArray(_pitchStabPIDControllerD); if (!_configFile.setValue("_pitchStabPIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r"); } ConvertToCharArray(_rollStabPIDControllerP); if (!_configFile.setValue("_rollStabPIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r"); } ConvertToCharArray(_rollStabPIDControllerI); if (!_configFile.setValue("_rollStabPIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r"); } ConvertToCharArray(_rollStabPIDControllerD); if (!_configFile.setValue("_rollStabPIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r"); } ConvertToCharArray(_zeroValues[1]); if (!_configFile.setValue("_zeroPitch", _str)) { _wiredSerial.printf("Failed to write value for zero pitch\n\r"); } ConvertToCharArray(_zeroValues[2]); if (!_configFile.setValue("_zeroRoll", _str)) { _wiredSerial.printf("Failed to write value for zero roll\n\r"); } if (!_configFile.write("/local/config.cfg")) { _wiredSerial.printf("Failure to write settings to configuration file.\n\r"); } else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r"); _freeIMU.sample(true); } else { _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r"); } } //Converts float to char array void ConvertToCharArray(float number) { sprintf(_str, "%1.8f", number ); } //Converts integer to char array void ConvertToCharArray(int number) { sprintf(_str, "%d", number ); } float Map(float input, float inputMin, float inputMax, float outputMin, float outputMax) { return (input - inputMin) * (outputMax - outputMin) / (inputMax - inputMin) + outputMin; } #endif