File content as of revision 1:d8c9f6b16279:

#include "QK_StateMachine.h"
using namespace std;

QK_StateMachine::QK_StateMachine(float Ts) :thread(osPriorityNormal, 4096), dout2(PB_3)
{
    CS = INIT;
    this->Ts = Ts;
    bool flightmode_changed = false;
    disarm_flag = false;
    timer.reset();
    timer.start();
}

// ---------------- Controller_Loops::init_loops -------------------
void QK_StateMachine::start_loop(void){
    printf("StateMachine Thread started\r\n");
    thread.start(callback(this, &QK_StateMachine::loop));
    ticker.attach(callback(this, &QK_StateMachine::sendSignal), Ts);

}
// ------

void QK_StateMachine::loop(void)
{
uint8_t kk=0;
while(1){
    thread.signal_wait(signal);
    dout2.write(1);
    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, CS: %d CH1: %1.2f CH2: %1.2f CH3: %1.2f CH4: %1.2f\r\n",local_FM,CS,myRC.PM1[0],myRC.PM1[1],myRC.PM1[2],myRC.PM1[3]);
            data.sm_FM=(float)local_FM;
            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
                for(uint8_t k=0;k<copter.nb_motors;k++)
                    motor_pwms[k].pulsewidth_us(850);
                timer.reset(); 
                pc.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_loop(); // start controller loop here (do not conrol yet!!!)
                // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * 
                if(local_FM == RTL)    // HACK!: do logging while not armed
//                    my_datalogger.continue_logging();
                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_datalogger.continue_logging();
                timer.reset();
                pc.printf("ARMING\r\n");
                
                }
            break;      // CS: WAIT_ARMING
        case ARMING:                      // 
            if((myRC.PM1[0] >= -.9) || (myRC.PM1[2]<.9f)){
                CS = WAIT_ARMING;
                timer.reset();
                }
            else if(dt > 1.0f){   // after 1 sec armed
                CS = START_MOTORS;
                timer.reset();
                }
            break;      // CS: ARMING
        case START_MOTORS:   // just wai, do nothing yet 
            if (dt > 1.5f){
                CS = FLIGHT;
                timer.reset();
                controller.reset_all();
                pc.printf("FLIGHT\r\n");
                switch(local_FM){
                case STABILIZED:         // STABILIZED: Control RP-Angles with Sticks, Yaw-Rate with Sticks, Thrust with Sticks
                    controller.enable_stabilized();
                    break;      // flm_loc: STABILIZED
                case ACRO:       // ACRO: Control Rates with sticks, Thrust with Sticks
                    controller.enable_acro();
                    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);
                        controller.enable_alt_hold();
                        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){
                        controller.vel_cntrl[0].reset(0.0);
                        controller.vel_cntrl[1].reset(0.0);
                        controller.enable_vel_of_z_pos();
                        flightmode_changed = false;
                        }
                    break;      // flm_loc: VEL_OF_Z_POS
                default: break;
                }       // end of switch(flm_loc)
            
                }
            break;      // CS: START_MOTORS
        case FLIGHT:
            // 2nd SWITCH-CASE
            
            if((myRC.PM1[0] > 0.8f) && (myRC.PM1[1]<-0.8f) && (myRC.PM1[3]<-0.8f) && disarm_flag == false){      // use CH1 (Roll) and CH4 (Yaw) to disarm (stick to leftright)
                timer.reset();
                CS = MOTOR_STOPPED;
                my_datalogger.pause_logging();
                controller.disable_all();
                pc.printf("UAV disarmed\r\n");
                }
            else
                disarm_flag=false;            
            break;  // case FLIGHT
        case MOTOR_STOPPED:
            if(dt>1){
                timer.reset();
                CS = WAIT_ARMING;
                }   // case MOTOR_STOPPED
            break;
        default: break;
        }           // end of switch(CS)
    dout2.write(0);
    }               // end while(1) the thread
}

// this is for realtime OS
void QK_StateMachine::sendSignal() {
    thread.signal_set(signal);
}
// ------------------- destructor -----------------
QK_StateMachine::~QK_StateMachine() {
    ticker.detach();
    }