altb_pmic
branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Diff: Sources/QK_StateMachine.cpp
- Revision:
- 1:d8c9f6b16279
- Parent:
- 0:a479dc61e931
--- a/Sources/QK_StateMachine.cpp Wed Oct 02 15:31:12 2019 +0000
+++ b/Sources/QK_StateMachine.cpp Wed Oct 09 13:47:43 2019 +0000
@@ -1,30 +1,31 @@
#include "QK_StateMachine.h"
using namespace std;
-QK_StateMachine::QK_StateMachine(float Ts)
+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::init_loops(float TS){
+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);
- old_dt = dt;
+ 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();
@@ -34,8 +35,8 @@
}
else{
local_FM = local_FM_temp;
-// pc.printf("Changed FM to %d \r\n",flm_loc);
-// my_logger.data_vector[23]=(float)flm_loc;
+ 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;
}
@@ -44,116 +45,113 @@
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);
+ for(uint8_t k=0;k<copter.nb_motors;k++)
+ motor_pwms[k].pulsewidth_us(850);
timer.reset();
- printf("INIT_MOTORS\r\n");
+ 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++)
+ 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++)
+ 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!!!)
+ controller.init_loop(); // 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(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_logger.continue_logging();
- global_timer.reset();
+ 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;
- global_timer.reset();
+ timer.reset();
}
else if(dt > 1.0f){ // after 1 sec armed
CS = START_MOTORS;
- global_timer.reset();
+ timer.reset();
}
break; // CS: ARMING
case START_MOTORS: // just wai, do nothing yet
if (dt > 1.5f){
CS = FLIGHT;
- global_timer.reset();
+ timer.reset();
controller.reset_all();
- }
- break; // CS: START_MOTORS
- case FLIGHT:
- // 2nd SWITCH-CASE
- switch(local_FM){
+ pc.printf("FLIGHT\r\n");
+ 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;
+ controller.enable_stabilized();
break; // flm_loc: STABILIZED
case ACRO: // ACRO: Control Rates with sticks, Thrust with Sticks
- Rate_controller_running = true;
- Attitude_controller_running = false;
+ 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);
- Rate_controller_running = true;
- Attitude_controller_running = true;
- Althold_Controller_Running = true;
+ 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){
- 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;
+ 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)
- // ... 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();
+ 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_logger.pause_logging();
+ my_datalogger.pause_logging();
+ controller.disable_all();
pc.printf("UAV disarmed\r\n");
}
+ else
+ disarm_flag=false;
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();
+ 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);
-}
\ No newline at end of file
+}
+// ------------------- destructor -----------------
+QK_StateMachine::~QK_StateMachine() {
+ ticker.detach();
+ }
\ No newline at end of file