Joseph Roberts
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
Revision 6:4c207e7b1203, committed 2015-01-22
- Comitter:
- joe4465
- Date:
- Thu Jan 22 18:03:22 2015 +0000
- Parent:
- 5:7b7db24ef6eb
- Child:
- 7:bc5822aa8878
- Commit message:
- Updated Communications to PC to handle issue with long messages
Changed in this revision
--- a/FreeIMU.lib Mon Sep 22 10:16:31 2014 +0000 +++ b/FreeIMU.lib Thu Jan 22 18:03:22 2015 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/tyftyftyf/code/FreeIMU/#ea86489d606b +http://mbed.org/users/tyftyftyf/code/FreeIMU/#7d83fc674fb2
--- a/PPM.lib Mon Sep 22 10:16:31 2014 +0000 +++ b/PPM.lib Thu Jan 22 18:03:22 2015 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/joe4465/code/PPM/#083ed8cea5ff +http://mbed.org/users/joe4465/code/PPM/#fb0ab71eb0ea
--- a/beep.lib Mon Sep 22 10:16:31 2014 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/joe4465/code/DigitalOutBeep/#14aa09db92f8
--- a/flightController.h Mon Sep 22 10:16:31 2014 +0000
+++ b/flightController.h Thu Jan 22 18:03:22 2015 +0000
@@ -18,7 +18,9 @@
// A thread to control flight
void FlightControllerThread(void const *args)
-{
+{
+ printf("Flight controller thread started\r\n");
+
//Update Timer
_flightControllerUpdateTimer = new RtosTimer(FlightControllerTask, osTimerPeriodic, (void *)0);
int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
@@ -98,10 +100,10 @@
}
//Testing
- _ratePIDControllerOutputs[0] = 0; // yaw
+ //_ratePIDControllerOutputs[0] = 0; // yaw
//_ratePIDControllerOutputs[1] = 0; // pitch
//_ratePIDControllerOutputs[2] = 0; // roll
- _stabPIDControllerOutputs[0] = 0; // yaw
+ //_stabPIDControllerOutputs[0] = 0; // yaw
//_stabPIDControllerOutputs[1] = 0; // pitch
//_stabPIDControllerOutputs[2] = 0; // roll
@@ -181,8 +183,26 @@
void GetAttitude()
{
+ //Use the 2 spare channels to alter the offset
+ if(_levelOffset == true)
+ {
+ float pitchOffset =_channel7MedianFilter->process(Map(_rcCommands[6], RC_OUT_MIN, RC_OUT_MAX, -5, 5));
+ float rollOffset =_channel8MedianFilter->process(Map(_rcCommands[7], RC_OUT_MIN, RC_OUT_MAX, -5, 5));
+
+ _zeroValues[1] = _oldZeroValues[1] + pitchOffset;
+ _zeroValues[2] = _oldZeroValues[2] + rollOffset;
+ }
+ else
+ {
+ _oldZeroValues[1] = _zeroValues[1];
+ _oldZeroValues[2] = _zeroValues[2];
+ }
+
+ //_zeroValues[1] = _rcCommands[6];
+ //_zeroValues[2] = _rcCommands[7];
+
//Get raw data from IMU
- _freeIMU.getYawPitchRoll(_ypr); // try get euler to get full rotation in each axis
+ _freeIMU.getYawPitchRoll(_ypr);
_freeIMU.getRate(_gyroRate);
//Take off zero values to account for any angle inbetween the PCB level and ground
@@ -190,19 +210,20 @@
_ypr[2] = _ypr[2] - _zeroValues[2];
//Swap pitch and roll angle because IMU is mounted at a right angle to the board
+ float yaw = _ypr[0];
float pitch = _ypr[2];
- float roll = -_ypr[1]; //Needs to be negative
+ float roll = - _ypr[1]; //Needs to be negative
+ _ypr[0] = yaw;
_ypr[1] = pitch;
_ypr[2] = roll;
- //Swap pitch and roll rate because IMU is mounted at a right angle to the board
- pitch = _gyroRate[2];
- roll = -_gyroRate[1];
+ //Swap pitch, roll and yaw? rate because IMU is mounted at a right angle to the board
+ yaw = _gyroRate[2];
+ pitch = _gyroRate[0];
+ roll = _gyroRate[1];
+ _gyroRate[0] = yaw;
_gyroRate[1] = pitch;
_gyroRate[2] = roll;
-
- //Try swapping yaw
- //_ypr[0] = - _ypr[0];
}
void NotFlying()
--- a/hardware.h Mon Sep 22 10:16:31 2014 +0000
+++ b/hardware.h Thu Jan 22 18:03:22 2015 +0000
@@ -3,14 +3,14 @@
#include "FreeIMU.h"
#include "PID.h"
#include "ConfigFile.h"
-#include "beep.h"
#include "PPM.h"
+#include "filter.h"
#include <sstream>
#ifndef HARDWARE_H
#define HARDWARE_H
-//Constants
+//Global constants
#define IMU_YAW_ANGLE_MAX 180
#define IMU_YAW_ANGLE_MIN -180
#define IMU_ROLL_ANGLE_MAX 90
@@ -30,16 +30,16 @@
#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 180
-#define RC_ROLL_RATE_MIN -180
-#define RC_PITCH_RATE_MAX 180
-#define RC_PITCH_RATE_MIN -180
-#define RC_ROLL_ANGLE_MAX 45
-#define RC_ROLL_ANGLE_MIN -45
-#define RC_PITCH_ANGLE_MAX 45
-#define RC_PITCH_ANGLE_MIN -45
+#define RC_YAW_RATE_MAX 90
+#define RC_YAW_RATE_MIN -90
+#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 15
+#define RC_ROLL_ANGLE_MIN -15
+#define RC_PITCH_ANGLE_MAX 15
+#define RC_PITCH_ANGLE_MIN -15
#define RC_THRUST_MAX 1
#define RC_THRUST_MIN 0
@@ -52,35 +52,37 @@
#define RATE_PID_CONTROLLER_OUTPUT_MIN -100
#define FLIGHT_CONTROLLER_FREQUENCY 500
-#define MOTOR_PWM_FREQUENCY 500
-#define RC_COMMANDS_FREQUENCY 50
-#define SERIAL_MONITOR_FREQUENCY 10
-#define PING_FREQUENCY 10
-#define PIXY_CAM_FREQUENCY 10
-#define STATUS_LIGHTS_FREQUENCY 10
-//Shared Functions
-void ZeroPitchRoll();
+//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);
-//Shared Variables
+//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
-//float _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
+float _gpsValues[5] = {0,0,0,0,0}; //Latitude, longitude, altitude, course, speed
bool _armed = false;
bool _rate = false;
bool _stab = true;
-bool _initialised = true;
-//bool _gpsConnected = false;
+bool _initialised = false;
+bool _gpsConnected = false;
float _motorPower [4] = {0,0,0,0};
-float _gyroRate[3] ={}; // Yaw, Pitch, Roll
+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;
//PID controllers
PID *_yawRatePIDController;
@@ -99,6 +101,15 @@
//Config file
LocalFileSystem local("local");
ConfigFile _configFile;
+char* _str = new char[1024];
+
+//RC filters
+medianFilter *_yawMedianFilter;
+medianFilter *_pitchMedianFilter;
+medianFilter *_rollMedianFilter;
+medianFilter *_thrustMedianFilter;
+medianFilter *_channel7MedianFilter;
+medianFilter *_channel8MedianFilter;
//HARDWARE////////////////////////////////////////////////////////////////////////////////////
// M1 M2
@@ -121,14 +132,14 @@
Serial _wirelessSerial(p9, p10);
//GPS Serial
-//Serial _gpsSerial(p28, p27);
+//Serial _gpsSerial(p28, p27);
//PPM in
PPM *_ppm;
-InterruptIn *_interruptPin = new InterruptIn(p5);
+InterruptIn *_interruptPin = new InterruptIn(p7);
//Battery monitor
-//DigitalIn _batteryMonitor(p8);
+//DigitalIn _batteryMonitor(p8);
//Onboard LED's
DigitalOut _led1(LED1);
@@ -142,39 +153,42 @@
DigitalOut _output3(p5);
DigitalOut _output4(p6);
-//Buzzer
-Beep _buzzer(p26);
-
//IMU
FreeIMU _freeIMU;
//Functions///////////////////////////////////////////////////////////////////////////////////////////////
-//Zero gyro, zero yaw and arm
+//Zero gyro and arm
void Arm()
{
- //Zero gyro
- _freeIMU.zeroGyro();
-
- //Set armed to true
- _armed = true;
+ //Don't arm unless throttle is equal to 0 and the transmitter is connected
+ if(_rcMappedCommands[3] < (RC_THRUST_MIN + 0.1) && _rcMappedCommands[3] != -1)
+ {
+ //Zero gyro
+ _freeIMU.zeroGyro();
+
+ //Set armed to true
+ _armed = true;
+ }
}
-//Set all RC to 0 and disarm
+//Disarm
void Disarm()
-{
- //Set all RC to 0
- _rcMappedCommands[0] = 0;
- _rcMappedCommands[1] = 0;
- _rcMappedCommands[2] = 0;
- _rcMappedCommands[3] = 0;
+{
+ //Set armed to false
+ _armed = false;
- //Set armed to false
- _armed = false;
+ //Disable modes
+ _levelOffset = false;
+
+ //Save settings
+ WriteSettingsToConfig();
}
//Zero pitch and roll
-void ZeroPitchRoll()
+/*void ZeroPitchRoll()
{
+ printf("Zeroing pitch and roll\r\n");
+
//Zero pitch and roll
float totalPitch = 0;
float totalRoll = 0;
@@ -190,11 +204,167 @@
_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
\ No newline at end of file
--- a/main.cpp Mon Sep 22 10:16:31 2014 +0000
+++ b/main.cpp Thu Jan 22 18:03:22 2015 +0000
@@ -16,10 +16,10 @@
void InitialisePWM();
void Setup();
-//Loads settings from the config file
+//Loads settings from the config file - could tidy this a little if I can be assed
void LoadSettingsFromConfig()
{
- _wiredSerial.printf("Loading settings from config file\n\r");
+ _wiredSerial.printf("Starting to load settings from config file\n\r");
//_wiredSerial.printf("Loading settings from config file\n\r");
char value[BUFSIZ];
@@ -28,140 +28,114 @@
if (!_configFile.read("/local/config.cfg"))
{
_wiredSerial.printf("Config file does not exist\n\r");
- _initialised = false;
}
-
- //Get values
- if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value);
else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r");
- }
- if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r");
- }
- if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r");
- }
- if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value);
+ {
+ //Get values
+ if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value);
else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r");
- }
- if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r");
- }
- if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r");
- }
- if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r");
- }
- if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r");
- }
- if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r");
+ {
+ _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r");
+ }
+ if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r");
+ }
+ if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r");
+ }
+ if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r");
+ }
+ if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r");
+ }
+ if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r");
+ }
+ if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r");
+ }
+ if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r");
+ }
+ if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r");
+ }
+
+ if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r");
+ }
+ if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r");
+ }
+ if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r");
+ }
+ if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r");
+ }
+ if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r");
+ }
+ if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r");
+ }
+ if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r");
+ }
+ if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r");
+ }
+ if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r");
+ }
+ if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for zero pitch\n\r");
+ }
+ if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
+ else
+ {
+ _wiredSerial.printf("Failed to get value for zero roll\n\r");
+ }
}
- if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r");
- }
- if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r");
- }
- if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r");
- }
- if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r");
- }
- if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r");
- }
- if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r");
- }
- if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r");
- }
- if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r");
- }
- if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r");
- }
- if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for zero pitch\n\r");
- }
- if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
- else
- {
- _initialised = false;
- _wiredSerial.printf("Failed to get value for zero roll\n\r");
- }
-
- if(_initialised == true)
- {
- _wiredSerial.printf("Finished loading settings from config file\n\r");
- }
- else
- {
- _wiredSerial.printf("Failed to load settings from config file\n\r");
- }
+ _wiredSerial.printf("Finished loading settings from config file\n\r");
}
//PID initialisation
@@ -216,7 +190,7 @@
void InitialisePWM()
{
//500Hz
- float period = 1.0 / MOTOR_PWM_FREQUENCY;
+ float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
_motor1.period(period);
_motor2.period(period);
_motor3.period(period);
@@ -231,15 +205,22 @@
//Setup
void Setup()
-{
+{
//Setup wired serial coms
_wiredSerial.baud(115200);
+ printf("\r\n");
+ printf("*********************************************************************************\r\n");
+ printf("Starting Setup\r\n");
+ printf("*********************************************************************************\r\n");
+
//Setup wireless serial coms
_wirelessSerial.baud(57600);
+ printf("Setup wireless serial\r\n");
//Setup GPS serial comms
//_gpsSerial.baud(115200);
+ //printf("Setup GPS serial\r\n");
//Read config file
LoadSettingsFromConfig();
@@ -249,35 +230,50 @@
_rcMappedCommands[1] = 0;
_rcMappedCommands[2] = 0;
_rcMappedCommands[3] = 0;
+ printf("Setup initial RC commands\r\n");
+
+ //Setup RC median filters
+ _yawMedianFilter = new medianFilter(10);
+ _pitchMedianFilter = new medianFilter(10);
+ _rollMedianFilter = new medianFilter(10);
+ _thrustMedianFilter = new medianFilter(10);
+ _channel7MedianFilter = new medianFilter(10);
+ _channel8MedianFilter = new medianFilter(10);
+ printf("Setup RC median filters\r\n");
//Initialise PPM
_ppm = new PPM(_interruptPin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
+ printf("Setup PPM\r\n");
//Initialise IMU
+ wait(1);
_freeIMU.init(true);
+ printf("Setup IMU\r\n");
//Initialise PID
InitialisePID();
+ printf("Setup PID\r\n");
//Initialise PWM
InitialisePWM();
+ printf("Setup PWM\r\n");
+
+ //Set initialised flag
+ _initialised = true;
+
+ printf("*********************************************************************************\r\n");
+ printf("Finished Setup\r\n");
+ printf("*********************************************************************************\r\n");
- //Start new line
- _wiredSerial.printf("\n\r");
- _wiredSerial.printf("Finished initialising: %s\n\r", _initialised ? "true" : "false");
-
// Start threads
- if(_initialised == true)
- {
- _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
- _serialPortMonitorThread->set_priority(osPriorityLow);
- _statusThread = new Thread(StatusThread);
- _statusThread->set_priority(osPriorityIdle);
- _flightControllerThread = new Thread (FlightControllerThread);
- _flightControllerThread->set_priority(osPriorityRealtime);
- _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
- _rcCommandMonitorThread->set_priority(osPriorityHigh);
- }
+ _flightControllerThread = new Thread (FlightControllerThread);
+ _flightControllerThread->set_priority(osPriorityRealtime);
+ _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
+ _rcCommandMonitorThread->set_priority(osPriorityHigh);
+ _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
+ _serialPortMonitorThread->set_priority(osPriorityLow);
+ _statusThread = new Thread(StatusThread);
+ _statusThread->set_priority(osPriorityIdle);
}
int main()
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/medianfilter.lib Thu Jan 22 18:03:22 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/networker/code/medianfilter/#9bd415456089
--- a/rcCommandMonitor.h Mon Sep 22 10:16:31 2014 +0000
+++ b/rcCommandMonitor.h Thu Jan 22 18:03:22 2015 +0000
@@ -2,25 +2,10 @@
#include "rtos.h"
#include "hardware.h"
-//Declarations
-void RcCommandMonitorTask(void const *n);
-
-//Timers
-RtosTimer *_rcCommandMonitorUpdateTimer;
+//Variables
+int i;
-// A thread to control flight
-void RcCommandMonitorThread(void const *args)
-{
- //Update Timer
- _rcCommandMonitorUpdateTimer = new RtosTimer(RcCommandMonitorTask, osTimerPeriodic, (void *)0);
- int updateTime = (1.0 / RC_COMMANDS_FREQUENCY) * 1000;
- _rcCommandMonitorUpdateTimer->start(updateTime);
-
- // Wait here forever
- Thread::wait(osWaitForever);
-}
-
-//Get RC commands
+// A thread to get RC commands and convert to correct values
//Channel 1 is roll. min 1000. max 1900
//Channel 2 is pitch. min 1000. max 1900
//Channel 3 is throttle < 900 when not connected. min 1000. max 1900
@@ -29,54 +14,85 @@
//Channel 6 is mode. rate > 1800. stab < 1100
//Channel 7 is spare
//Channel 8 is spare
-void RcCommandMonitorTask(void const *n)
-{
- //Initialise array to hold channel data
- float rcCommands[8] = {0,0,0,0,0,0,0,0};
-
- //Get channel data - mapped to between 0 and 1
- _ppm->GetChannelData(rcCommands);
+void RcCommandMonitorThread(void const *args)
+{
+ printf("RC command monitor thread started\r\n");
- //Map yaw channel
- _rcMappedCommands[0] = Map(rcCommands[3], 0, 1, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX);
-
- //Map thust channel
- _rcMappedCommands[3] = Map(rcCommands[2], 0, 1, RC_THRUST_MIN, RC_THRUST_MAX);
+ //Set lost connection iterator to 0
+ i = 0;
- //Map arm channel. 0.85 = 1765
- if(rcCommands[4] > 0.85) Arm();
- else Disarm();
-
- //Map mode channel
- if(rcCommands[5] < 0.5)
+ //Main loop
+ while(true)
{
- _stab = true;
- _rate = false;
- }
- else
- {
- _stab = false;
- _rate = true;
- }
-
- //Roll and pitch mapping depends on the mode
- if(_rate == false && _stab == true)//Stability mode
- {
- //Roll
- _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX);
- //Pitch
- _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX);
- }
- else if(_rate == true && _stab == false)//Rate mode
- {
- //Roll
- _rcMappedCommands[2] = Map(rcCommands[0], 0, 1, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX);
- //Pitch
- _rcMappedCommands[1] = Map(rcCommands[2], 0, 1, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX);
- }
- else
- {
- _rcMappedCommands[1] = 0;
- _rcMappedCommands[2] = 0;
+ //Get channel data - mapped to between 0 and 1
+ _ppm->GetChannelData(_rcCommands);
+
+ //Check whether transmitter is connected
+ if(_rcCommands[2] != -1)
+ {
+ //Transmitter is connected so reset not connected iterator
+ i = 0;
+
+ //Map yaw channel
+ _rcMappedCommands[0] = - _yawMedianFilter->process(Map(_rcCommands[3], RC_OUT_MIN, RC_OUT_MAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
+
+ //Map thust channel
+ _rcMappedCommands[3] = _thrustMedianFilter->process(Map(_rcCommands[2], RC_OUT_MIN, RC_OUT_MAX, RC_THRUST_MIN, RC_THRUST_MAX));
+
+ //Map arm channel.
+ if(_rcCommands[4] > 0.5 && _initialised == true && _armed == false) Arm();
+ else if(_rcCommands[4] <= 0.5 && _armed == true)
+ {
+ Disarm();
+ }
+
+ //Map mode channel
+ if(_rcCommands[5] < 0.5)
+ {
+ _stab = true;
+ _rate = false;
+ }
+ else
+ {
+ _stab = false;
+ _rate = true;
+ }
+
+ //Roll and pitch mapping depends on the mode
+ if(_rate == false && _stab == true)//Stability mode
+ {
+ //Roll
+ _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
+ //Pitch
+ _rcMappedCommands[1] = _pitchMedianFilter->process(-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX)); //Needs to be reverse
+ }
+ else if(_rate == true && _stab == false)//Rate mode
+ {
+ //Roll
+ _rcMappedCommands[2] = _rollMedianFilter->process(Map(_rcCommands[0], RC_OUT_MIN, RC_OUT_MAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
+ //Pitch
+ _rcMappedCommands[1] = _pitchMedianFilter->process(-Map(_rcCommands[1], RC_OUT_MIN, RC_OUT_MAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX)); //Needs to be reverse
+ }
+ else
+ {
+ _rcMappedCommands[1] = 0;
+ _rcMappedCommands[2] = 0;
+ }
+ }
+ else
+ {
+ //Transmitter not connected so increase iterator
+ i++;
+
+ //Set commands to 0
+ _rcMappedCommands[0] = 0;
+ _rcMappedCommands[1] = 0;
+ _rcMappedCommands[2] = 0;
+ _rcMappedCommands[3] = -1;
+
+ //If connection has been down for 10 loops then assume the connection really is lost
+ if(i > 10) Disarm();
+ }
+ Thread::wait(20);
}
}
\ No newline at end of file
--- a/serialPortMonitor.h Mon Sep 22 10:16:31 2014 +0000
+++ b/serialPortMonitor.h Thu Jan 22 18:03:22 2015 +0000
@@ -3,99 +3,148 @@
#include "hardware.h"
//Declarations
-void SerialPortMonitorTask(void const *n);
void CheckWirelessSerialCommand();
//void CheckGPSSerialCommand();
-void WriteSettingsToConfig();
-void ConvertToCharArray(float number);
-void ConvertToCharArray(int number);
void UpdatePID();
//Variables
-char* _str = new char[1024];
char _wirelessSerialBuffer[255];
int _wirelessSerialRxPos = 0;
-char _gpsSerialBuffer[255];
-int _gpsSerialRxPos = 0;
-
-//Timers
-RtosTimer *_serialPortMonitorUpdateTimer;
+//char _gpsSerialBuffer[255];
+//int _gpsSerialRxPos = 0;
// A thread to monitor the serial ports
void SerialPortMonitorThread(void const *args)
-{
- //Update Timer
- _serialPortMonitorUpdateTimer = new RtosTimer(SerialPortMonitorTask, osTimerPeriodic, (void *)0);
- int updateTime = (1.0 / SERIAL_MONITOR_FREQUENCY) * 1000;
- _serialPortMonitorUpdateTimer->start(updateTime);
+{
+ printf("Serial port monitor thread started\r\n");
- // Wait here forever
- Thread::wait(osWaitForever);
-}
-
-void SerialPortMonitorTask(void const *n)
-{
- //Print to windows application
- int batt = 0;
- _wirelessSerial.printf("<%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%d:%d:%d:%1.6f:%1.6f:%1.6f:%1.2f:%1.2f:%1.2f:%1.2f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%d>",
- _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, 0/*_gpsValues[0]*/, 0/* _gpsValues[1]*/, 0/*_gpsValues[2]*/, 0/*_gpsValues[3]*/, 0/*_gpsValues[4]*/, 0/*_gpsConnected*/);
+ while(true)
+ {
+ //Print to windows application
+ //int batt = 0;
+ //_wirelessSerial.printf("<%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%d:%d:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f::1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f>",
+ //_motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, 0/*_gpsValues[0]*/, 0/* _gpsValues[1]*/, 0/*_gpsValues[2]*/, 0/*_gpsValues[3]*/, 0/*_gpsValues[4]*/, 0/*_gpsConnected*/, _zeroValues[1], _zeroValues[2], _oldZeroValues[1], _oldZeroValues[2]);
+ // 0 1 2 3 4 5 6 7 8 9 10 11
- //Check for wireless serial command
- if (_wirelessSerial.readable() > 0)
- {
- int c = _wirelessSerial.getc();
-
- switch (c)
+ //Check comms mode and print correct data back to PC application
+ switch(_commsMode)
{
- case 60: //
- _wirelessSerialRxPos = 0;
+ //Motor power
+ case 0:
+ _wirelessSerial.printf("<M1=%1.2f:M2=%1.2f:M3=%1.2f:M4=%1.2f>",
+ _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3]);
+ break;
+
+ //PID outputs
+ case 1:
+ _wirelessSerial.printf("<SYPID=%1.2f:SPPID=%1.2f:SRPID=%1.2f:RYPID=%1.2f:RPPID=%1.2f:RRPID=%1.2f>",
+ _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2]);
+ break;
+
+ //IMU outputs
+ case 2:
+ _wirelessSerial.printf("<SY=%1.2f:SP=%1.2f:SR=%1.2f:RY=%1.2f:RP=%1.2f:RR=%1.2f>",
+ _ypr[0], _ypr[1], _ypr[2], _gyroRate[0], _gyroRate[1], _gyroRate[2]);
break;
- case 10: // LF
- case 13: // CR
- case 62: // >
- CheckWirelessSerialCommand();
+ //Status
+ case 3:
+ _wirelessSerial.printf("<Batt=%d:Armed=%d:Init=%d:Rate=%d:Stab=%d:Lev=%d>",
+ _batt, _armed, _initialised, _rate, _stab, _levelOffset);
+ break;
+
+ //Mapped RC commands
+ case 4:
+ _wirelessSerial.printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f>",
+ _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3]);
+ break;
+
+ //PID Tuning
+ case 5:
+ _wirelessSerial.printf("<RYPIDP=%1.2f:RYPIDI=%1.2f:RYPIDD=%1.2f:RPPIDP=%1.2f:RPPIDI=%1.2f:RPPIDD=%1.2f:RRPIDP=%1.2f:RRPIDI=%1.2f:RRPIDD=%1.2f:SYPIDP=%1.2f:SYPIDI=%1.2f:SYPIDD=%1.2f:SPPIDP=%1.2f:SPPIDI=%1.2f:SPPIDD=%1.2f:SRPIDP=%1.2f:SRPIDI=%1.2f:SRPIDD=%1.2f>",
+ _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);
+ break;
+
+ //GPS
+ case 6:
+ _wirelessSerial.printf("<GLat=%1.6f:GLon=%1.6f:GAlt=%1.2f:GAng=%1.2f:GSpd=%1.2f:GInit=%d>",
+ _gpsValues[0], _gpsValues[1], _gpsValues[2], _gpsValues[3], _gpsValues[4], _gpsConnected);
+ break;
+
+ //Zero mode
+ case 7:
+ _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>",
+ _zeroValues[0], _zeroValues[1], _zeroValues[2]);
+ break;
+
+ //Raw RC Commands
+ case 8:
+ _wirelessSerial.printf("<RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>",
+ _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _rcCommands[4], _rcCommands[5], _rcCommands[6], _rcCommands[7]);
break;
default:
- _wirelessSerialBuffer[_wirelessSerialRxPos++] = c;
- if (_wirelessSerialRxPos > 200)
- {
- _wirelessSerialRxPos = 0;
- }
- break;
+ break;
}
- }
-
- //Check for GPS serial command
- /*if (_gpsSerial.readable() > 0)
- {
- int c = _gpsSerial.getc();
- //printf("%c", c);
- _wiredSerial.putc(c);
- switch (c)
+ //Check for wireless serial command
+ if (_wirelessSerial.readable() > 0)
{
- case 60: // <
- _gpsSerialRxPos = 0;
- break;
+ int c = _wirelessSerial.getc();
- case 10: // LF
- case 13: // CR
- case 62: // >
- CheckGPSSerialCommand();
- break;
+ switch (c)
+ {
+ case 60: //
+ _wirelessSerialRxPos = 0;
+ break;
- default:
- _gpsSerialBuffer[_gpsSerialRxPos++] = c;
- if (_gpsSerialRxPos > 200)
- {
+ case 10: // LF
+ case 13: // CR
+ case 62: // >
+ CheckWirelessSerialCommand();
+ break;
+
+ default:
+ _wirelessSerialBuffer[_wirelessSerialRxPos++] = c;
+ if (_wirelessSerialRxPos > 200)
+ {
+ _wirelessSerialRxPos = 0;
+ }
+ break;
+ }
+ }
+
+ //Check for GPS serial command
+ /*if (_gpsSerial.readable() > 0)
+ {
+ int c = _gpsSerial.getc();
+ //printf("%c", c);
+ _wiredSerial.putc(c);
+
+ switch (c)
+ {
+ case 60: // <
_gpsSerialRxPos = 0;
- printf("oh no \r\n");
- }
- break;
- }
- } */
+ break;
+
+ case 10: // LF
+ case 13: // CR
+ case 62: // >
+ CheckGPSSerialCommand();
+ break;
+
+ default:
+ _gpsSerialBuffer[_gpsSerialRxPos++] = c;
+ if (_gpsSerialRxPos > 200)
+ {
+ _gpsSerialRxPos = 0;
+ printf("oh no \r\n");
+ }
+ break;
+ }
+ } */
+ Thread::wait(100);
+ }
}
//Checks for a valid command from the serial port and executes it
@@ -120,20 +169,23 @@
switch (command)
{
- //Spare
+ //Start level offset mode to teach quad level
case 'a':
+ _levelOffset = true;
break;
+
//Arm disarm
case 'b':
if(_initialised == true && _armed == false)
{
Arm();
}
- else
+ else if(_armed == true)
{
Disarm();
}
break;
+
//Set mode
case 'c':
if(_rate == true)
@@ -147,118 +199,151 @@
_stab = false;
}
break;
+
//Set yaw
case 'd':
if(_armed == true) _rcMappedCommands[0] = value; //Yaw
+ else _rcMappedCommands[0] = 0;
break;
+
//Set pitch
case 'e':
if(_armed == true) _rcMappedCommands[1] = value; //Pitch
+ else _rcMappedCommands[1] = 0;
break;
+
//Set roll
case 'f':
if(_armed == true) _rcMappedCommands[2] = value; //Roll
+ else _rcMappedCommands[2] = 0;
break;
+
//Set thrust
case 'g':
if(_armed == true) _rcMappedCommands[3] = value; //Thrust
+ else _rcMappedCommands[3] = 0;
break;
+
//Set PID values
case 'h':
_yawRatePIDControllerP = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'i':
_yawRatePIDControllerI = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'j':
_yawRatePIDControllerD = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'k':
_pitchRatePIDControllerP = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'l':
_pitchRatePIDControllerI = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'm':
_pitchRatePIDControllerD = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'n':
_rollRatePIDControllerP = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'o':
_rollRatePIDControllerI = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'p':
_rollRatePIDControllerD = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'q':
_yawStabPIDControllerP = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'r':
_yawStabPIDControllerI = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 's':
_yawStabPIDControllerD = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 't':
_pitchStabPIDControllerP = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'u':
_pitchStabPIDControllerI = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'v':
_pitchStabPIDControllerD = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'w':
_rollStabPIDControllerP = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'x':
_rollStabPIDControllerI = value;
UpdatePID();
WriteSettingsToConfig();
break;
+
case 'y':
_rollStabPIDControllerD = value;
UpdatePID();
WriteSettingsToConfig();
break;
- //Zero pitch and roll
+
+ case 'z':
+ _commsMode = value;
+ break;
+
case '1':
- ZeroPitchRoll();
+ _zeroValues[0] = 0;
+ _zeroValues[1] = 0;
+ _zeroValues[2] = 0;
WriteSettingsToConfig();
break;
+
default:
break;
}
@@ -335,162 +420,6 @@
return;
}*/
-//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 );
-}
-
//Updates PID tunings
void UpdatePID()
{
--- a/statusLights.h Mon Sep 22 10:16:31 2014 +0000
+++ b/statusLights.h Thu Jan 22 18:03:22 2015 +0000
@@ -2,38 +2,22 @@
#include "rtos.h"
#include "hardware.h"
-//Declarations
-void StatusLightsTask(void const *n);
-
-//Timers
-RtosTimer *_statusLightsUpdateTimer;
-
// The status thread indicates the current system status to the user
void StatusThread(void const *args)
{
- //Update Timer
- _statusLightsUpdateTimer = new RtosTimer(StatusLightsTask, osTimerPeriodic, (void *)0);
- int updateTime = (1.0 / STATUS_LIGHTS_FREQUENCY) * 1000;
- _statusLightsUpdateTimer->start(updateTime);
+ printf("Status lights thread started\r\n");
- // Wait here forever
- Thread::wait(osWaitForever);
-}
-
-void StatusLightsTask(void const *n)
-{
int ledState = 0;
while (true)
{
ledState++;
- if (ledState > 5)
- {
- ledState = 0;
- }
+ if (ledState > 5) { ledState = 0; }
_led1 = (ledState == 0);
_led2 = (ledState == 1 || ledState == 5);
_led3 = (ledState == 2 || ledState == 4);
_led4 = (ledState == 3);
+
+ Thread::wait(100);
}
-}
\ No newline at end of file
+}