File content as of revision 0:a479dc61e931:

#include "QK_StateMachine.h"
using namespace std;

QK_StateMachine::QK_StateMachine(float Ts)
{
    CS = INIT;
    bool flightmode_changed = false;
    timer.reset();
    timer.start();
}




// ---------------- Controller_Loops::init_loops -------------------
void QK_StateMachine::init_loops(float TS){
    thread.start(callback(this, &QK_StateMachine::loop));
    ticker.attach(callback(this, &QK_StateMachine::sendSignal), Ts);


// ------

void QK_StateMachine::loop(void)
{
while(1){
    thread.signal_wait(signal);
    old_dt = dt;
    dt  = timer.read();              // time elapsed in current state
    if(myRC.flightmode_changed){            // This is triggered bei RC-PWM readout
        local_FM_temp = myRC.get_flightmode();
        if(local_FM_temp == VEL_OF_Z_POS & data.sens_OF[4] < 150) // do not change to that flightmode, if OF quality ist too low
            {
             myRC.flightmode_changed = false;   // set flag back, but do nothing else! 
            }
        else{
            local_FM = local_FM_temp;
//            pc.printf("Changed FM to %d \r\n",flm_loc);
//            my_logger.data_vector[23]=(float)flm_loc;
            myRC.flightmode_changed = false;
            flightmode_changed = true;
            }
        }   // if flightmode on RC changed   
    switch(CS)  {
        case INIT:                      // at very beginning
            if (dt > 3.0f && (myRC.PM1[0]<-0.9f) && (myRC.PM1[2] > 0.9f)){
                CS = INIT_MOTORS;              // switch to Init Motors state
                motor_pwms[k].pulsewidth_us(850);
                timer.reset(); 
                printf("INIT_MOTORS\r\n");
                }
                break;     
        case INIT_MOTORS:  // give start pulses to motors
            if(dt > 1.0f & dt < 2.0f){
                for(uint8_t k=0;k<copter_nb_motors;k++)
                    motor_pwms[k].pulsewidth_us(1050);
                }
            if(dt > 2.0f){
                for(uint8_t k=0;k<copter_nb_motors;k++)
                    motor_pwms[k].pulsewidth_us(0);
                CS = WAIT_ARMING;
                // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
                controller.init_loops(); // start controller loop here (do not conrol yet!!!)
                // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
                if(flm_loc == RTL)    // HACK!: do logging while not armed
                    my_logger.continue_logging();
                global_timer.reset();
                }   // if dt > 2
            break;      // CS: INIT_MOTORS
// -------------------------------------------------------- go to CS states above only at very beginning!
        case WAIT_ARMING:  // after disarming fall back to here!!!
            if((myRC.PM1[0] < -.9) && (myRC.PM1[2]>0.9f) ){      // use CH1 (Roll) and CH4 (Yaw) to arm (stick to middle)
                CS = ARMING;
                my_logger.continue_logging();
                global_timer.reset();
                }
            break;      // CS: WAIT_ARMING
        case ARMING:                      // 
            if((myRC.PM1[0] >= -.9) || (myRC.PM1[2]<.9f)){
                CS = WAIT_ARMING;
                global_timer.reset();
                }
            else if(dt > 1.0f){   // after 1 sec armed
                CS = START_MOTORS;
                global_timer.reset();
                }
            break;      // CS: ARMING
        case START_MOTORS:   // just wai, do nothing yet 
            if (dt > 1.5f){
                CS = FLIGHT;
                global_timer.reset();
                controller.reset_all();
                }
            break;      // CS: START_MOTORS
        case FLIGHT:
            // 2nd SWITCH-CASE
            switch(local_FM){
                case STABILIZED:         // STABILIZED: Control RP-Angles with Sticks, Yaw-Rate with Sticks, Thrust with Sticks
                    Rate_controller_running = true;
                    Attitude_controller_running = true;
                    break;      // flm_loc: STABILIZED
                case ACRO:       // ACRO: Control Rates with sticks, Thrust with Sticks
                    Rate_controller_running = true;
                    Attitude_controller_running = false;
                    break;      // flm_loc: ACRO
                case ALT_HOLD:   // Altitde Hold 
                    if(flightmode_changed){
                        controller.reset_des_z(data.est_xyz[2]);        // set desired value to current value
                        controller.vel_cntrl[2].reset(data.F_Thrust);
                        Rate_controller_running = true;
                        Attitude_controller_running = true;
                        Althold_Controller_Running = true;
                        flightmode_changed = false;
                        }
                    break;      // flm_loc: ALT_HOLD
                case VEL_OF_Z_POS:       // This is the velocity mode with optical flow sensor
                    if(flightmode_changed){
                        vel_cntrl[0].reset(0.0);
                        vel_cntrl[1].reset(0.0);
                        Rate_Controller_Running = true;
                        Attitude_Controller_Running = true;
                        Althold_Controller_Running = true;
                        Vel_Controller_Running = true;
                        flightmode_changed = false;
                        }
                    break;      // flm_loc: VEL_OF_Z_POS
                default: break;
                }       // end of switch(flm_loc)
            // ... continue FLIGHT case
            if((myRC.PM1[1] > 0.8f) && (myRC.PM1[2]<-0.8f) && (myRC.PM1[3]<-0.8f) && disarm_flag == false){      // use CH1 (Roll) and CH4 (Yaw) to disarm (stick to leftright)
                disarm_flag=true;
                global_timer.reset();
                }
            else 
                disarm_flag=false;            
            if((myRC.PM1[1] > 0.8f) && (myRC.PM1[2]<-0.8f) && (myRC.PM1[3]<-0.8f) && disarm_flag == false && dt > 1){
                disarm_flag=false;
                global_timer.reset();
                CS = MOTOR_STOPPED;
                my_logger.pause_logging();
                pc.printf("UAV disarmed\r\n");
                }
            break;  // case FLIGHT
        case MOTOR_STOPPED:
            for(uint8_t m=1;m <= copter.nb_motors;m++)
                motorspeed[m]=0.0f;
            if(dt>1){
                global_timer.reset();
                CS = WAIT_ARMING;
                }   // case MOTOR_STOPPED
            break;
        default: break;
        }           // end of switch(CS)
    }               // end while(1) the thread
}

// this is for realtime OS
void QK_StateMachine::sendSignal() {
    thread.signal_set(signal);
}