File content as of revision 2:e7874762cc25:

#include "QK_StateMachine.h"
using namespace std;

QK_StateMachine::QK_StateMachine(float Ts) :thread(osPriorityNormal, 4096), dout2(PB_3)
{
    CS = INIT;
    this->Ts = Ts;
    local_FM = myRC.get_flightmode();
    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
    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 > 0.5f && local_FM == STABILIZED){
                CS = FLIGHT;
                timer.reset();
                controller.reset_all();
                pc.printf("FLIGHT\r\n");
                controller.enable_stabilized();
                }
            else if(dt > 0.5)
                {
                    CS = MOTOR_STOPPED;
                    timer.reset();
                    }
            break;      // CS: START_MOTORS
        case FLIGHT:
            // 2nd SWITCH-CASE
            if(myRC.flightmode_changed){            // This is triggered bei RC-PWM readout
                local_FM = get_new_flightmode();
                }   // if flightmode on RC changed   
            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:
            local_FM = STABILIZED;
            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();
    }

// -------- test if flightmode can be changed -----------------
uint8_t QK_StateMachine::get_new_flightmode(void){
    myRC.flightmode_changed = false;   // set flag back, but do nothing else! 
    uint8_t local_FM_temp = myRC.get_flightmode();
    switch(local_FM_temp){
        case VEL_OF_Z_POS:
            if(data.sens_OF[3] > 150) // test if OF quality is high enough
                {
                local_FM = local_FM_temp;
                data.sm_FM=(float)local_FM;
                flightmode_changed = true;
                controller.vel_cntrl[0].reset(0.0f);
                controller.vel_cntrl[1].reset(0.0f);
                //pc.printf("Changed FM to: %d\r\n",local_FM);
                return local_FM;
                }
            else
                return local_FM;
            break;
        case ALT_HOLD:
            if(xternal_sensors.Lidar_1_is_valid)
                {
                if(local_FM == STABILIZED)  // we come from Stabilized mode
                    {
                    controller.pos_cntrl[2].reset(data.F_Thrust);      // Set vel vntrl to thrustvalue    
                    controller.reset_des_z(data.est_xyz[2]);
                    }
                local_FM = local_FM_temp;
                data.sm_FM=(float)local_FM;
                flightmode_changed = true;
                controller.enable_alt_hold();
                pc.printf("Changed FM to: %d\r\n",local_FM);
                }
            else
                return local_FM;
            break;
        case STABILIZED:
            local_FM = local_FM_temp;
            data.sm_FM=(float)local_FM;
            flightmode_changed = true;
            controller.enable_stabilized();
            pc.printf("Changed FM to: %d\r\n",local_FM);
            break;   
        }       // switch
        
}