altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/QK_StateMachine.cpp@3:bc24fee36ba3, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:54:10 2019 +0000
Revision:
3:bc24fee36ba3
Parent:
2:e7874762cc25 
- with complementFilter for Yaw (Magneteometer), Lidar, AltHold running, Flow sensor reading, control sth wrong, Yaw not tested in detail

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:a479dc61e931 1 #include "QK_StateMachine.h"
altb2 0:a479dc61e931 2 using namespace std;
altb2 0:a479dc61e931 3
altb2 1:d8c9f6b16279 4 QK_StateMachine::QK_StateMachine(float Ts) :thread(osPriorityNormal, 4096), dout2(PB_3)
altb2 0:a479dc61e931 5 {
altb2 0:a479dc61e931 6 CS = INIT;
altb2 1:d8c9f6b16279 7 this->Ts = Ts;
altb2 2:e7874762cc25 8 local_FM = myRC.get_flightmode();
altb2 0:a479dc61e931 9 bool flightmode_changed = false;
altb2 1:d8c9f6b16279 10 disarm_flag = false;
altb2 0:a479dc61e931 11 timer.reset();
altb2 0:a479dc61e931 12 timer.start();
altb2 0:a479dc61e931 13 }
altb2 0:a479dc61e931 14
altb2 0:a479dc61e931 15 // ---------------- Controller_Loops::init_loops -------------------
altb2 1:d8c9f6b16279 16 void QK_StateMachine::start_loop(void){
altb2 1:d8c9f6b16279 17 printf("StateMachine Thread started\r\n");
altb2 0:a479dc61e931 18 thread.start(callback(this, &QK_StateMachine::loop));
altb2 0:a479dc61e931 19 ticker.attach(callback(this, &QK_StateMachine::sendSignal), Ts);
altb2 0:a479dc61e931 20
altb2 1:d8c9f6b16279 21 }
altb2 0:a479dc61e931 22 // ------
altb2 0:a479dc61e931 23
altb2 0:a479dc61e931 24 void QK_StateMachine::loop(void)
altb2 0:a479dc61e931 25 {
altb2 1:d8c9f6b16279 26 uint8_t kk=0;
altb2 0:a479dc61e931 27 while(1){
altb2 0:a479dc61e931 28 thread.signal_wait(signal);
altb2 1:d8c9f6b16279 29 dout2.write(1);
altb2 0:a479dc61e931 30 dt = timer.read(); // time elapsed in current state
altb2 0:a479dc61e931 31 switch(CS) {
altb2 0:a479dc61e931 32 case INIT: // at very beginning
altb2 0:a479dc61e931 33 if (dt > 3.0f && (myRC.PM1[0]<-0.9f) && (myRC.PM1[2] > 0.9f)){
altb2 0:a479dc61e931 34 CS = INIT_MOTORS; // switch to Init Motors state
altb2 1:d8c9f6b16279 35 for(uint8_t k=0;k<copter.nb_motors;k++)
altb2 1:d8c9f6b16279 36 motor_pwms[k].pulsewidth_us(850);
altb2 0:a479dc61e931 37 timer.reset();
altb2 1:d8c9f6b16279 38 pc.printf("INIT_MOTORS\r\n");
altb2 0:a479dc61e931 39 }
altb2 0:a479dc61e931 40 break;
altb2 0:a479dc61e931 41 case INIT_MOTORS: // give start pulses to motors
altb2 0:a479dc61e931 42 if(dt > 1.0f & dt < 2.0f){
altb2 1:d8c9f6b16279 43 for(uint8_t k=0;k<copter.nb_motors;k++)
altb2 0:a479dc61e931 44 motor_pwms[k].pulsewidth_us(1050);
altb2 0:a479dc61e931 45 }
altb2 0:a479dc61e931 46 if(dt > 2.0f){
altb2 1:d8c9f6b16279 47 for(uint8_t k=0;k<copter.nb_motors;k++)
altb2 0:a479dc61e931 48 motor_pwms[k].pulsewidth_us(0);
altb2 0:a479dc61e931 49 CS = WAIT_ARMING;
altb2 0:a479dc61e931 50 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
altb2 1:d8c9f6b16279 51 controller.init_loop(); // start controller loop here (do not conrol yet!!!)
altb2 0:a479dc61e931 52 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
altb2 1:d8c9f6b16279 53 if(local_FM == RTL) // HACK!: do logging while not armed
altb2 2:e7874762cc25 54 my_datalogger.continue_logging();
altb2 1:d8c9f6b16279 55 timer.reset();
altb2 0:a479dc61e931 56 } // if dt > 2
altb2 0:a479dc61e931 57 break; // CS: INIT_MOTORS
altb2 0:a479dc61e931 58 // -------------------------------------------------------- go to CS states above only at very beginning!
altb2 0:a479dc61e931 59 case WAIT_ARMING: // after disarming fall back to here!!!
altb2 0:a479dc61e931 60 if((myRC.PM1[0] < -.9) && (myRC.PM1[2]>0.9f) ){ // use CH1 (Roll) and CH4 (Yaw) to arm (stick to middle)
altb2 0:a479dc61e931 61 CS = ARMING;
altb2 1:d8c9f6b16279 62 my_datalogger.continue_logging();
altb2 1:d8c9f6b16279 63 timer.reset();
altb2 1:d8c9f6b16279 64 pc.printf("ARMING\r\n");
altb2 1:d8c9f6b16279 65
altb2 0:a479dc61e931 66 }
altb2 0:a479dc61e931 67 break; // CS: WAIT_ARMING
altb2 0:a479dc61e931 68 case ARMING: //
altb2 0:a479dc61e931 69 if((myRC.PM1[0] >= -.9) || (myRC.PM1[2]<.9f)){
altb2 0:a479dc61e931 70 CS = WAIT_ARMING;
altb2 1:d8c9f6b16279 71 timer.reset();
altb2 0:a479dc61e931 72 }
altb2 0:a479dc61e931 73 else if(dt > 1.0f){ // after 1 sec armed
altb2 0:a479dc61e931 74 CS = START_MOTORS;
altb2 1:d8c9f6b16279 75 timer.reset();
altb2 0:a479dc61e931 76 }
altb2 0:a479dc61e931 77 break; // CS: ARMING
altb2 0:a479dc61e931 78 case START_MOTORS: // just wai, do nothing yet
altb2 2:e7874762cc25 79 if (dt > 0.5f && local_FM == STABILIZED){
altb2 0:a479dc61e931 80 CS = FLIGHT;
altb2 1:d8c9f6b16279 81 timer.reset();
altb2 0:a479dc61e931 82 controller.reset_all();
altb2 1:d8c9f6b16279 83 pc.printf("FLIGHT\r\n");
altb2 2:e7874762cc25 84 controller.enable_stabilized();
altb2 0:a479dc61e931 85 }
altb2 2:e7874762cc25 86 else if(dt > 0.5)
altb2 2:e7874762cc25 87 {
altb2 2:e7874762cc25 88 CS = MOTOR_STOPPED;
altb2 2:e7874762cc25 89 timer.reset();
altb2 2:e7874762cc25 90 }
altb2 1:d8c9f6b16279 91 break; // CS: START_MOTORS
altb2 1:d8c9f6b16279 92 case FLIGHT:
altb2 1:d8c9f6b16279 93 // 2nd SWITCH-CASE
altb2 2:e7874762cc25 94 if(myRC.flightmode_changed){ // This is triggered bei RC-PWM readout
altb2 2:e7874762cc25 95 local_FM = get_new_flightmode();
altb2 2:e7874762cc25 96 } // if flightmode on RC changed
altb2 1:d8c9f6b16279 97 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)
altb2 1:d8c9f6b16279 98 timer.reset();
altb2 0:a479dc61e931 99 CS = MOTOR_STOPPED;
altb2 1:d8c9f6b16279 100 my_datalogger.pause_logging();
altb2 1:d8c9f6b16279 101 controller.disable_all();
altb2 0:a479dc61e931 102 pc.printf("UAV disarmed\r\n");
altb2 0:a479dc61e931 103 }
altb2 1:d8c9f6b16279 104 else
altb2 1:d8c9f6b16279 105 disarm_flag=false;
altb2 0:a479dc61e931 106 break; // case FLIGHT
altb2 0:a479dc61e931 107 case MOTOR_STOPPED:
altb2 2:e7874762cc25 108 local_FM = STABILIZED;
altb2 0:a479dc61e931 109 if(dt>1){
altb2 1:d8c9f6b16279 110 timer.reset();
altb2 0:a479dc61e931 111 CS = WAIT_ARMING;
altb2 0:a479dc61e931 112 } // case MOTOR_STOPPED
altb2 0:a479dc61e931 113 break;
altb2 0:a479dc61e931 114 default: break;
altb2 0:a479dc61e931 115 } // end of switch(CS)
altb2 1:d8c9f6b16279 116 dout2.write(0);
altb2 0:a479dc61e931 117 } // end while(1) the thread
altb2 0:a479dc61e931 118 }
altb2 0:a479dc61e931 119
altb2 0:a479dc61e931 120 // this is for realtime OS
altb2 0:a479dc61e931 121 void QK_StateMachine::sendSignal() {
altb2 0:a479dc61e931 122 thread.signal_set(signal);
altb2 1:d8c9f6b16279 123 }
altb2 1:d8c9f6b16279 124 // ------------------- destructor -----------------
altb2 1:d8c9f6b16279 125 QK_StateMachine::~QK_StateMachine() {
altb2 1:d8c9f6b16279 126 ticker.detach();
altb2 2:e7874762cc25 127 }
altb2 2:e7874762cc25 128
altb2 2:e7874762cc25 129 // -------- test if flightmode can be changed -----------------
altb2 2:e7874762cc25 130 uint8_t QK_StateMachine::get_new_flightmode(void){
altb2 2:e7874762cc25 131 myRC.flightmode_changed = false; // set flag back, but do nothing else!
altb2 2:e7874762cc25 132 uint8_t local_FM_temp = myRC.get_flightmode();
altb2 2:e7874762cc25 133 switch(local_FM_temp){
altb2 2:e7874762cc25 134 case VEL_OF_Z_POS:
altb2 3:bc24fee36ba3 135 if(data.sens_OF[2] > 150) // test if OF quality is high enough
altb2 2:e7874762cc25 136 {
altb2 2:e7874762cc25 137 local_FM = local_FM_temp;
altb2 2:e7874762cc25 138 data.sm_FM=(float)local_FM;
altb2 2:e7874762cc25 139 flightmode_changed = true;
altb2 2:e7874762cc25 140 controller.vel_cntrl[0].reset(0.0f);
altb2 2:e7874762cc25 141 controller.vel_cntrl[1].reset(0.0f);
altb2 2:e7874762cc25 142 //pc.printf("Changed FM to: %d\r\n",local_FM);
altb2 3:bc24fee36ba3 143 controller.enable_vel_of_z_pos();
altb2 2:e7874762cc25 144 return local_FM;
altb2 2:e7874762cc25 145 }
altb2 2:e7874762cc25 146 else
altb2 2:e7874762cc25 147 return local_FM;
altb2 2:e7874762cc25 148 break;
altb2 2:e7874762cc25 149 case ALT_HOLD:
altb2 2:e7874762cc25 150 if(xternal_sensors.Lidar_1_is_valid)
altb2 2:e7874762cc25 151 {
altb2 2:e7874762cc25 152 if(local_FM == STABILIZED) // we come from Stabilized mode
altb2 2:e7874762cc25 153 {
altb2 2:e7874762cc25 154 controller.pos_cntrl[2].reset(data.F_Thrust); // Set vel vntrl to thrustvalue
altb2 2:e7874762cc25 155 controller.reset_des_z(data.est_xyz[2]);
altb2 2:e7874762cc25 156 }
altb2 2:e7874762cc25 157 local_FM = local_FM_temp;
altb2 2:e7874762cc25 158 data.sm_FM=(float)local_FM;
altb2 2:e7874762cc25 159 flightmode_changed = true;
altb2 2:e7874762cc25 160 controller.enable_alt_hold();
altb2 3:bc24fee36ba3 161 //pc.printf("Changed FM to: %d\r\n",local_FM);
altb2 2:e7874762cc25 162 }
altb2 2:e7874762cc25 163 else
altb2 2:e7874762cc25 164 return local_FM;
altb2 2:e7874762cc25 165 break;
altb2 2:e7874762cc25 166 case STABILIZED:
altb2 2:e7874762cc25 167 local_FM = local_FM_temp;
altb2 3:bc24fee36ba3 168 controller.vel_cntrl[0].reset(0.0f);
altb2 3:bc24fee36ba3 169 controller.vel_cntrl[1].reset(0.0f);
altb2 3:bc24fee36ba3 170 controller.pos_cntrl[2].reset(0.0f);
altb2 3:bc24fee36ba3 171
altb2 2:e7874762cc25 172 data.sm_FM=(float)local_FM;
altb2 2:e7874762cc25 173 flightmode_changed = true;
altb2 2:e7874762cc25 174 controller.enable_stabilized();
altb2 3:bc24fee36ba3 175 //pc.printf("Changed FM to: %d\r\n",local_FM);
altb2 2:e7874762cc25 176 break;
altb2 2:e7874762cc25 177 } // switch
altb2 2:e7874762cc25 178
altb2 2:e7874762cc25 179 }