Matteo Terruzzi
Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
Diff: main.cpp
- Revision:
- 3:5e7476bca25f
- Parent:
- 2:7439607ccd51
- Child:
- 4:46f3c3dd5977
--- a/main.cpp Tue Oct 15 18:30:46 2013 +0000
+++ b/main.cpp Sun Oct 27 01:05:35 2013 +0000
@@ -1,16 +1,57 @@
#include "math.h"
#include "mbed.h"
#include "rtos.h"
-//#include "EthernetInterface.h"
-//#include "RefRX.h"
+#include "EthernetInterface.h"
#include "esc.h"
+#include "PID.h"
+#include "MPU6050.h"
+#include "IMUfilter.h"
+#include <string>
+
+
+//Author: Matteo Terruzzi
+
+//Target differential of time in seconds, for the various program cycles:
+ //timings as for main cycle:
+#define TARGET_IMU_DT 0.002 //every 1 step: 100%
+#define TARGET_MAIN_DT 0.016 //every 8 steps: 12.5% + 2 step start delay
+#define TARGET_TX_DT 0.040 //every 20 steps: 5% + 1 step start delay
+#define TARGET_RX_DT 0.040 //with previous
+
+//For the various dt, "average" is computed using a sort of low-pass filter of the actual value.
+//The formula (recurrence equation) is:
+// a(t) = x(t) * k - a(t-1) * k with a(0) = 0
+//where:
+// x(t) is the actual value at the time t;
+// a(t) is the average at the timepoint t;
+// a(t-1) is the average at the previous timepoint;
+// k is AVERAGE_DT_K_GAIN (macro defined below).
+//
+//So, here is the meaning of AVERAGE_DT_K_GAIN, or k: the higher it is, the higher is the cut-frequency.
+//If k == 1, then a(t) == x(t).
+//If k == 0, then a(t) == target value (the actual value is totaly ignored).
+//A good value may be k = 1/8 = 0.125.
+#define AVERAGE_DT_K_GAIN 0.0125
+
+//Blinking status leds
+#define LED_ESC LED1
+#define LED_IMU LED2
+#define LED_TXRX LED3
+#define LED_RX LED4
+
+
#include "state.h"
#include "TXRX.h"
-#include "IMU.h"
-#include "PID.h"
int main()
{
+
+ Serial pc(USBTX,USBRX);
+
+
+ //INITIALIZATION
+ //-------------------------------------------------------------------------------
+
//Setup ESCs
ESC esc1(p23);
ESC esc2(p24);
@@ -18,98 +59,155 @@
ESC esc4(p26);
//Setup status leds
- DigitalOut led1(LED1); led1 = 1; //esc
- DigitalOut led4(LED4); led4 = 0; //signal
- //DigitalOut led3(LED3); led3 = 0; //tmp signal send
- //DigitalOut led2(LED2); led2 = 0;//moved to IMU.h
+ DigitalOut led_esc(LED_ESC); led_esc = 1; //flip at esc update (fixed 0 means that the ESCs was not been initiated--the program is stucked before this point)
+ DigitalOut led_imu(LED_IMU); led_imu = 0; //flip at imu computations done (fixed 0 means that the imu sensors don't respond)
+ DigitalOut led_txrx(LED_TXRX); led_txrx = 0; //flip at remote signal exchange done (fixed 0 means that the TXRX_thread has not been started)
//Setup Ethernet
- //EthernetInterface interface;
- //interface.init("192.168.1.16","255.255.255.0","192.168.1.1");
- //interface.connect();
+ EthernetInterface interface;
+ interface.init("192.168.1.16","255.255.255.0","192.168.1.1");
+ interface.connect();
- //Setup RefRX static class for remote control
- //Thread RefRX_thread (RefRX::worker);
- //RefRX::init(2222, 2223, RefRX_thread);
- //Serial pc(USBTX, USBRX);
- //pc.baud(115200);
- //pc.printf("Serial setted up\r\n");
+ //Setup remote control
Thread TXRX_thread (TXRX_thread_routine);
- led4 = 1;
- //Thread::wait(0.5);
-
- //Setup FreeIMU
- //FreeIMU freeIMU;
- //freeIMU.init();
- Thread IMU_thread (IMU_thread_routine);
- //pc.printf("IMU thread started\r\n");
+ led_txrx = 1;
//Setup state
QuadState mainQuadState; mainQuadState.reset();
QuadState rxQuadState; rxQuadState.reset();
QuadState txQuadState; txQuadState.reset();
- //pc.printf("Main and rx states setted up\r\n");
+
+ //Setup FreeIMU
+ IMUfilter IMU_filter(TARGET_IMU_DT,40);
+ MPU6050 IMU_sensor(p28, p27);
+ IMU_sensor.setGyroRange(MPU6050_GYRO_RANGE_250);
+ IMU_sensor.setAcceleroRange(MPU6050_ACCELERO_RANGE_4G);
+ led_imu = 1;
+
+
+
- //Prepare controll loop
- //float ref1=0, ref2=0, ref3=0, ref4=0;
- double dt=0.001; //will be measured
- Timer dt_t;
- dt_t.start();
- //unsigned int state_read_i=0;
- //unsigned int state_send_i=0;
+ //MAIN CONTROL CYCLE PREPARATION
+ //-------------------------------------------------------------------------------
+
+ unsigned short int step = 0;
+
+ //Just a moment... Let the other threads begin!
+ Thread::wait(2000 /*ms*/);
+ Thread::yield();
+ //just made sure the other threads had time to begin.
+
+
+ //Setup timers
+ Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
+ Timer dt_timer; dt_timer.reset(); dt_timer.start();
+ Thread::wait(1000.0 * min(mainQuadState.target_imu_dt,mainQuadState.target_main_dt));
//pid
PID targetRoll_pid(0.1,0.0001,0.5, 0.020);
targetRoll_pid.setInputLimits(-3.15/4,3.15/4);
targetRoll_pid.setOutputLimits(0,0.5);
-
- //pc.printf("Now going to start main loop...\r\n");
-
- led1 = !led1;
- //Begins main control loop
+
+
+
+ //MAIN CONTROL CYCLE
+ //===================================================================================
while(1)
{
- //retrive refs
- //led4 ^= RefRX::get(ref1,ref2,ref3,ref4); //flip-flop-flip-flop!
- /*
- if(pc.readable()){
- *((unsigned char *)(&rxQuadState) + state_read_i) = pc.getc();
- ++state_read_i;
- if(state_read_i >= sizeof(QuadState))
- state_read_i = 0;
- led4 = 1;
+
+
+ //IMU INPUT
+ //-------------------------------------------------------------------------------
+
+ //Measure time
+ QUAD_STATE_READ_ACTUAL_DT(mainQuadState,imu,IMU_dt_timer)
+ QUAD_STATE_WAIT_DT_TARGET(mainQuadState.actual_imu_dt,mainQuadState.target_imu_dt)
+ QUAD_STATE_UPDATE_DT(mainQuadState,imu,IMU_dt_timer)
+
+
+ led_imu = 1;
+
+ //Retrieve inertial values from sensors
+ {
+ float a[3], g[3];
+ IMU_sensor.getAccelero(a);
+ IMU_sensor.getGyro(g);
+
+ //Compute IMU filters
+ IMU_filter.updateFilter(g[0], g[1], g[2], a[0], a[1], a[2]);
}
- else
- led4 = 0;
- */
- unsigned int rx_delta, rx_wait;
- if(TXRX_stateExchange(txQuadState,rxQuadState))
- if(rxQuadState.actual_rx_delta_ms >= 5000 || rxQuadState.actual_rx_wait_ms >= 5000)
- mainQuadState.ZERO_ALL_MOTORS_NOW__DANGER = true; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
+ if(mainQuadState.actual_imu_dt >= 100.0 * mainQuadState.target_imu_dt)
+ mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
+
+
+ led_imu = 0;
+
+
+
+
+if(step % 20 == 2){
+//5% frequency; step%20==2
+
+
+
+ //REMOTE INPUT
+ //-------------------------------------------------------------------------------
+ led_txrx = 1;
+
+ //Remote signal exchange
+ txQuadState = mainQuadState; //send info back
+ if(TXRX_stateExchange(txQuadState,rxQuadState)){
+ mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
+ mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
+ if(mainQuadState.actual_rx_dt >= 100.0 * mainQuadState.target_rx_dt)
+ mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = 0; //experiments only ////NOTE: FIXME: THAT'S ABSOLUTELY UNSAFE WHILE FLYING!!!
+ }
+
+
+ /*if(step % 640 == 2){
- //retrive inertial values
- IMU_filter_getYawPitchRoll(
- mainQuadState.estimated_rotation_y,
- mainQuadState.estimated_rotation_p,
- mainQuadState.estimated_rotation_r );
+ //Other tasks
+
+ if(pc.writeable()){
+ pc.printf("%s \r\n", mainQuadState.getJSON().c_str()); //the message is long => it slows down a lot!
+ }
+ }*/
+
+ led_txrx = 0;
+
+
+
+//end of 5% frequency; step%20==2
+}
+if(step % 8 == 3){
+//12.5% frequency; step%8==3
+
- //calc dt
- dt_t.stop();
- dt = (double) (1.0/1000000.0) * dt_t.read_us();
- dt_t.reset(); dt_t.start();
- mainQuadState.average_throttle_dt += dt * mainQuadState.average_throttle_dt_k;
- mainQuadState.average_throttle_dt -= mainQuadState.average_throttle_dt * mainQuadState.average_throttle_dt_k;
- mainQuadState.actual_throttle_dt = dt;
+ led_esc = 1;
+
+
+ //ELABORATION
+ //-------------------------------------------------------------------------------
+
+ //Measure time
+ QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
- //elaborate signals
- if(mainQuadState.ZERO_ALL_MOTORS_NOW__DANGER){
+ //Estimate rotation
+ IMU_filter.computeEuler();
+ mainQuadState.estimated_rotation_y = IMU_filter.getYaw();
+ mainQuadState.estimated_rotation_p = IMU_filter.getPitch();
+ mainQuadState.estimated_rotation_r = IMU_filter.getRoll();
+
+ //Elaborate inputs to determinate outputs
+ if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
+ || rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
mainQuadState.actual_throttle_1 = 0;
mainQuadState.actual_throttle_2 = 0;
mainQuadState.actual_throttle_3 = 0;
mainQuadState.actual_throttle_4 = 0;
}else{
- mainQuadState.actual_rx_delta_ms = rxQuadState.actual_rx_delta_ms;
- mainQuadState.actual_rx_wait_ms = rxQuadState.actual_rx_wait_ms;
+ mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
+ mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
@@ -117,7 +215,7 @@
float roll = 0;
if(mainQuadState.target_rotation_r_isRequired){
targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
- targetRoll_pid.setInterval(mainQuadState.average_throttle_dt);
+ targetRoll_pid.setInterval(mainQuadState.average_main_dt);
targetRoll_pid.setProcessValue(mainQuadState.estimated_rotation_r);
roll = targetRoll_pid.compute();
}else{
@@ -131,6 +229,7 @@
out -= roll;
mainQuadState.actual_throttle_1 = out;
}
+
if(rxQuadState.requested_throttle_2 >= 0 && rxQuadState.requested_throttle_2 <=1)
mainQuadState.actual_throttle_2 = rxQuadState.requested_throttle_2;
else{
@@ -150,52 +249,35 @@
}
}
- //compute new ESC values..
+
+
+ //OUTPUT
+ //-------------------------------------------------------------------------------
+
+ //set new ESC values.
esc1 = mainQuadState.actual_throttle_1;
esc2 = mainQuadState.actual_throttle_2;
esc3 = mainQuadState.actual_throttle_3;
esc4 = mainQuadState.actual_throttle_4;
- //finally output to ESCs
+ //effectively output to ESCs
esc1();
esc2();
esc3();
esc4();
- led1 = !led1;
- uint32_t w = 20-dt;
- if(w<5) w=5;
- else if(w>19) w=19;
- Thread::wait(w /*ms*/);
+ led_esc = 0;
+
+//end of 12.5% frequency; step%8==3
+}
- //send info back
- txQuadState = mainQuadState;
- /*
- for(int i=0; i<sizeof(QuadState); ++i){
- while(!pc.writeable())
- led3=0;
- pc.putc((int) *((unsigned char *) (&mainQuadState) + i));
- }*/
- /*
- if(state_send_i >= 7){
- state_send_i = 0;
- time_t sec = time(NULL);
- pc.printf("\033[2J\033[1;1H");
- pc.printf("Time: %s", ctime(&sec));
- pc.printf("Actual thrust: \t\t %8.2f %8.2f %8.2f %8.2f\n",
- mainQuadState.actual_throttle_1, mainQuadState.actual_throttle_2,
- mainQuadState.actual_throttle_3, mainQuadState.actual_throttle_4 );
- pc.printf("IMU yaw pitch roll: \t %10.1f° %10.1f° %10.1f°\n", mainQuadState.estimated_rotation_y*180.0/3.141592653589793,
- mainQuadState.estimated_rotation_p*180.0/3.141592653589793, mainQuadState.estimated_rotation_r*180.0/3.141592653589793 );
- pc.printf("Avg/actual delta time: \t %10.2f ms %10.2f ms \n", mainQuadState.average_throttle_dt*1000.0, mainQuadState.actual_throttle_dt*1000.0);
- led3=pc.writeable();
- }
- ++state_send_i;*/
- }
+ ++step;
+ }//End of main control loop
+ //It must not get here.
return 0;
}