File content as of revision 8:ca92cb674004:

//@todo trimのパラメータをpcから設定，csvファイルに追加
//@todo initをクラス化

//@todo androidとの通信
//@todo xbeeとの通信

//@todo 高度計の追加

#include "mbed.h"
#include "rtos.h"
#include "Global.h"
#include "ServoManager.h"
#include "ControllerManager.h"
#include "Trim.h"
#include "Cadence.h"
#include "Airspeed.h"
#include "Altitude.h"
#include "Display.h"
#include "XBee.h"
#include <string>
#include <iostream>

#define BUFMAX 20

RawSerial pc(USBTX, USBRX);
Queue<char, BUFMAX> queue;


Display display(p9, p10);
XBee xbee(p13, p14);
Trim trim(p16, p17);
ControllerManager controllerManager(p18,p19,p20);
ServoManager servoManager(p21, p22);
Airspeed airspeed(p26, NC, NC);
Cadence cadence(p29, p30, NC);


void pc_rx(){
    while(pc.readable()==1){
        char buf = pc.getc();
        queue.put((char*)buf);
    }
}


void initializeTask(void const *pvParametersd){
    std::string strbuf;
    
    while(1){
        osEvent evt = queue.get();
        if(evt.status == osEventMessage){
            char temp = evt.value.v;
            strbuf.push_back(temp);
            if(temp == '\n'){    
                std::string::size_type spaceIndex = strbuf.find(" ");
                
                if (spaceIndex != std::string::npos) {//数値データがあるコマンド（" "がある場合）
                    char *gomi;
                    std::string command = strbuf.substr(0, spaceIndex);
                    double num = strtod(strbuf.substr(spaceIndex+1, strbuf.size()-2).c_str(), &gomi);//strbuf.size()-1には'\n'が入っている
                    
                    pc.printf("coms=%s, num=%f\n", command.c_str(), num);

                    if(command == "eleneu"){
                        Global::setneutralpitch(num);
                    }else if(command == "elemax"){
                        Global::setmaxpitch(num);
                    }else if(command == "elemin"){
                        Global::setminpitch(num);
                    }else if(command == "rudneu"){
                        Global::setneutralyaw(num);
                    }else if(command == "rudmax"){
                        Global::setmaxyaw(num);
                    }else if(command == "rudmin"){
                        Global::setminyaw(num);
                        
                    }else if(command == "eleneudeg"){
                        Global::setneutralpitchdegree(num);
                    }else if(command == "elemaxdeg"){
                        Global::setmaxpitchdegree(num);
                    }else if(command == "elemindeg"){
                        Global::setminpitchdegree(num);
                    }else if(command == "rudneudeg"){
                        Global::setneutralyawdegree(num);
                    }else if(command == "rudmaxdeg"){
                        Global::setmaxyawdegree(num);
                    }else if(command == "rudmindeg"){
                        Global::setminyawdegree(num);
                    
                    }else if(command == "elemaxplay"){
                        Global::setmaxpitchplayratio(num);
                    }else if(command == "eleminplay"){
                        Global::setminpitchplayratio(num);                        
                    }else if(command == "rudmaxplay"){
                        Global::setmaxyawplayratio(num);
                    }else if(command == "rudminplay"){
                        Global::setminyawplayratio(num);
                        
                    }else{
                        pc.printf("Invalid Input\n");
                    }
                    
                }else{//数値データがないコマンド（" "がない場合）
                
                    std::string command = strbuf.substr(0,strbuf.size()-2);
                    if(command == "eleneu"){
                        Global::setneutralpitch(Global::getpitch());
                    }else if(command == "elemax"){
                        Global::setmaxpitch(Global::getpitch());
                    }else if(command == "elemin"){
                        Global::setminpitch(Global::getpitch());
                    }else if(command == "rudneu"){
                        Global::setneutralyaw(Global::getyaw());
                    }else if(command == "rudmax"){
                        Global::setmaxyaw(Global::getyaw());
                    }else if(command == "rudmin"){
                        Global::setminyaw(Global::getyaw());
                        
                    }else if(command == "eleneudeg"){
                        Global::setneutralpitchdegree(Global::getpitchdegree());
                    }else if(command == "elemaxdeg"){
                        Global::setmaxpitchdegree(Global::getpitchdegree());
                    }else if(command == "elemindeg"){
                        Global::setminpitchdegree(Global::getpitchdegree());
                    }else if(command == "rudneudeg"){
                        Global::setneutralyawdegree(Global::getyawdegree());
                    }else if(command == "rudmaxdeg"){
                        Global::setmaxyawdegree(Global::getyawdegree());
                    }else if(command == "rudmindeg"){
                        Global::setminyawdegree(Global::getyawdegree());
                        
                    }else if(command == "show"){
                        pc.printf("------Servo Parameter------\n");
                        pc.printf("elevatorNeutral = %f\n", Global::getneutralpitch());
                        pc.printf("elevatorMax     = %f\n", Global::getmaxpitch());
                        pc.printf("elevatorMin     = %f\n", Global::getminpitch());
                        pc.printf("rudderNeutral   = %f\n", Global::getneutralyaw());
                        pc.printf("rudderMax       = %f\n", Global::getmaxyaw());
                        pc.printf("rudderMin       = %f\n", Global::getminyaw());
                        pc.printf("----Controller Parameter----\n");
                        pc.printf("elevatorNeutral = %f\n", Global::getneutralpitchdegree());
                        pc.printf("elevatorMax     = %f\n", Global::getmaxpitchdegree());
                        pc.printf("elevatorMin     = %f\n", Global::getminpitchdegree());
                        pc.printf("rudderNeutral   = %f\n", Global::getneutralyawdegree());
                        pc.printf("rudderMax       = %f\n", Global::getmaxyawdegree());
                        pc.printf("rudderMin       = %f\n", Global::getminyawdegree());
                        pc.printf("------Controller Play------\n");
                        pc.printf("elevatorMaxPlay = %f\n", Global::getmaxpitchplayratio());
                        pc.printf("elevatorMinPlay = %f\n", Global::getminpitchplayratio());
                        pc.printf("rudderMaxPlay   = %f\n", Global::getmaxyawplayratio());
                        pc.printf("rudderMinPlay   = %f\n", Global::getminyawplayratio());
                        
                    }else{
                        pc.printf("Invalid Input\n");
                    }
                    //ここで，ニュートラル情報をLocalFileに保存
                    Global::filewrite();    
                }
                strbuf.clear();
            }
            Thread::wait(10);
            if(strbuf.size() >= BUFMAX){
                pc.printf("\nInput Error ... ");
                strbuf.clear();
                pc.printf("Buffer clear\n");
            }
        }
    }
}


void controlTask(void const *pvParameters){
    while(1){
        trim.update();
        controllerManager.update();
        servoManager.update();
        Thread::wait(50);
    }
}

void airspeedTask(void const *pvParameters){
    while(1){
        airspeed.update();
        Thread::wait(200);
    }
}
    
void cadenceTask(void const *pvParameters){
    while(1){
        cadence.update();
        Thread::wait(200);
    }
}

void displayTask(void const *pvParameters){
    while(1){
        display.update();
        Thread::wait(50);
    }   
}

void xbeeTask(void const *pvParameters){
    while(1){
        xbee.update();
        Thread::wait(50);
    }
}


int main(void){
    printf("start\n");
    
    pc.attach(pc_rx,Serial::RxIrq);

    Global::initialize();
    Thread InitializeTask(initializeTask);
    Thread ControlTask(controlTask);
    Thread AirspeedTask(airspeedTask);
    Thread CadenceTask(cadenceTask);
    Thread DisplayTask(displayTask);
    Thread XbeeTask(xbeeTask);
    printf("Task end\n");
    
    Thread::wait(osWaitForever);   
}