File content as of revision 0:0010a5abcc31:

//Includes
#include "mbed.h"
#include "rtos.h"
#include "FreeIMU.h"
#include "PID.h"
#include "ConfigFile.h"
#include "hardware.h"
#include "statusLights.h"
#include "serialPortMonitor.h"
#include "flightController.h"

//Declarations
void LoadSettingsFromConfig();
void InitialisePID();
void InitialisePWM();
void Setup();

//Loads settings from the config file
void LoadSettingsFromConfig()
{
    _wiredSerial.printf("Loading settings from config file\n\r");
    
    //_wiredSerial.printf("Loading settings from config file\n\r");
    char value[BUFSIZ];
    
    //Read a configuration file from a mbed.
    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);
        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");
    }
    
    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");
    }
}

//PID initialisation
void InitialisePID()
{
    float updateTime = 1.0 / UPDATE_FREQUENCY;
    
    _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime);
    _yawRatePIDController->setInputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
    _yawRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
    _yawRatePIDController->setMode(AUTO_MODE);
    _yawRatePIDController->setSetPoint(0.0);
    _yawRatePIDController->setBias(0);
    
    _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime);
    _pitchRatePIDController->setInputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
    _pitchRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
    _pitchRatePIDController->setMode(AUTO_MODE);
    _pitchRatePIDController->setSetPoint(0.0);
    _pitchRatePIDController->setBias(0);
    
    _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime);
    _rollRatePIDController->setInputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
    _rollRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
    _rollRatePIDController->setMode(AUTO_MODE);
    _rollRatePIDController->setSetPoint(0.0);
    _rollRatePIDController->setBias(0);
    
    _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime);
    _yawStabPIDController->setInputLimits(IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX);
    _yawStabPIDController->setOutputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
    _yawStabPIDController->setMode(AUTO_MODE);
    _yawStabPIDController->setSetPoint(0.0);
    _yawStabPIDController->setBias(0);
    
    _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime);
    _pitchStabPIDController->setInputLimits(IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX);
    _pitchStabPIDController->setOutputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
    _pitchStabPIDController->setMode(AUTO_MODE);
    _pitchStabPIDController->setSetPoint(0.0);
    _pitchStabPIDController->setBias(0);
    
    _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime);
    _rollStabPIDController->setInputLimits(IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX);
    _rollStabPIDController->setOutputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
    _rollStabPIDController->setMode(AUTO_MODE);
    _rollStabPIDController->setSetPoint(0.0);
    _rollStabPIDController->setBias(0);
}

//PWM Initialisation
void InitialisePWM()
{
    //
    float period = 1.0 / MOTOR_PWM_FREQUENCY;
    _motor1.period(period);
    _motor2.period(period);
    _motor3.period(period);
    _motor4.period(period);
    
    //Disable
    _motor1 = MOTORS_OFF;
    _motor2 = MOTORS_OFF;
    _motor2 = MOTORS_OFF;
    _motor2 = MOTORS_OFF;
}

//Setup
void Setup()
{
    //Setup wired serial coms
    _wiredSerial.baud(115200);
    
    //Setup wireless serial coms
    _wirelessSerial.baud(57600);
    
    //Read config file
    LoadSettingsFromConfig();
    
    //Set initial RC Ccommands
    _rcCommands[0] = 0;
    _rcCommands[1] = 0;
    _rcCommands[2] = 0;
    _rcCommands[3] = 0;

    //Initialise IMU
    _freeIMU.init(true);
    
    //Initialise PID
    InitialisePID();
    
    //Initialise PWM
    InitialisePWM();
    
    //Start new line
    _wiredSerial.printf("\n\r");
    
    // 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);
    }
}

//MAIN LOOP//////////////////////////////////////////////////////////////////////////
int main()
{
    Setup(); 
    
    // Wait here forever
    Thread::wait(osWaitForever); 
}