Matteo Terruzzi
Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
Diff: main.cpp
- Revision:
- 5:33abcc31b0aa
- Parent:
- 4:46f3c3dd5977
- Child:
- 6:4ddd68260f76
--- a/main.cpp Sat May 03 13:22:30 2014 +0000
+++ b/main.cpp Mon May 12 22:58:06 2014 +0000
@@ -1,22 +1,18 @@
#include "math.h"
#include "mbed.h"
#include "rtos.h"
-#include "EthernetInterface.h"
#include "esc.h"
-#include "PID.h"
-#include "MPU6050.h"
-#include "IMUfilter.h"
+#include "FreeIMU.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
+#define TARGET_IMU_DT 0.002 //500Hz on main thread, every step of the cycle.
+#define TARGET_MAIN_DT 0.016 //62.5Hz on main thread, every 8 steps + 7 step delay.
+#define TARGET_TX_DT 0.050 //20Hz on other thread. Exchanged with main every 24 steps + 21 step delay (dt=48ms, 20.8333Hz)
+#define TARGET_RX_DT 0.050 //waited with previous: this dt is only measured, not waited.
//For the various dt, "average" is computed using a sort of low-pass filter of the actual value.
//The formula (recurrence equation) is:
@@ -34,58 +30,57 @@
#define AVERAGE_DT_K_GAIN 0.0125
//Blinking status leds
-#define LED_ESC LED1
-#define LED_IMU LED2
+#define LED_IMU LED1
+#define LED_ESC LED2
#define LED_TXRX LED3
-#define LED_RX LED4
+#define LED_RX LED4
+#define FAST_FLASH_ON(led,times) for(int i=0;i<times;++i){Thread::wait(25); led = 0; Thread::wait(50); led = 1; Thread::wait(25); }
+#define FAST_FLASH_OFF(led,times) for(int i=0;i<times;++i){Thread::wait(25); led = 1; Thread::wait(50); led = 0; Thread::wait(25); }
+#define SLOW_FLASH_ON(led,times) for(int i=0;i<times;++i){Thread::wait(250); led = 0; Thread::wait(500); led = 1; Thread::wait(250);}
#include "state.h"
-#include "IMU.h"
-#include "TXRX.h"
+#include "TXRX_nomagic.h"
+FreeIMU IMU_imu;
+
int main()
{
-
- Serial pc(USBTX,USBRX);
//INITIALIZATION
//-------------------------------------------------------------------------------
//Setup ESCs
- ESC esc1(p23);
- ESC esc2(p24);
- ESC esc3(p25);
- ESC esc4(p26);
+ ESC esc1(p23); esc1=0;
+ ESC esc2(p24); esc1=0;
+ ESC esc3(p25); esc1=0;
+ ESC esc4(p26); esc1=0;
//Setup status leds
- 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.2.16","255.255.255.0","192.168.2.1");
- interface.connect();
+ DigitalOut led_imu(LED_IMU); led_imu = 0;
+ DigitalOut led_esc(LED_ESC); led_esc = 0;
+ DigitalOut led_txrx(LED_TXRX); led_txrx = 0;
+ FAST_FLASH_ON(led_esc,5);
//Setup remote control
- Thread TXRX_thread (TXRX_thread_routine);
- led_txrx = 1;
+ FAST_FLASH_OFF(led_txrx,2);
+ Thread TXRX_thread(TXRX_thread_routine);
+ Thread::wait(8000);
+ FAST_FLASH_ON(led_txrx,5);
+
+ //Setup IMU
+ Thread::wait(2000);
+ FAST_FLASH_OFF(led_imu,2);
+ IMU_imu.init(true);
+ Thread::wait(8000);
+ FAST_FLASH_ON(led_imu,5);
//Setup state
QuadState mainQuadState; mainQuadState.reset();
QuadState rxQuadState; rxQuadState.reset();
QuadState txQuadState; txQuadState.reset();
- //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;
-
-
//MAIN CONTROL CYCLE PREPARATION
@@ -94,128 +89,80 @@
unsigned short int step = 0;
//Just a moment... Let the other threads begin!
- Thread::wait(2000 /*ms*/);
Thread::yield();
+ Thread::wait(1000.0 * (TARGET_MAIN_DT + TARGET_IMU_DT + TARGET_TX_DT + TARGET_RX_DT) * 10.0);
//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));
+ Timer IMU_dt_timer; IMU_dt_timer.reset(); IMU_dt_timer.start();
+ Thread::wait(1000.0 * TARGET_IMU_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);*/
-
+ //Some flashing animation
+ for(int i=100;i>10;i=(75*i)/100){
+ led_txrx = 0;led_esc = 0;led_imu = 0; Thread::wait(i);
+ led_txrx = 1;led_esc = 1;led_imu = 1; Thread::wait(i);
+ }
+
+
//MAIN CONTROL CYCLE
//===================================================================================
while(1)
{
-
-
- //IMU INPUT
- //-------------------------------------------------------------------------------
-
- //Measure the time and sleep to correct the interval
+
+
+ //measure the time for IMU cycle
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)
+ wait_us(1000);
+
+
+if(step % 8 != 7){//see step%8==7; prevent double sensor refresh
+
+
+ //IMU INPUT
+ //-------------------------------------------------------------------------------
+
+ led_imu = 1;
+ IMU_imu.getQ(NULL);
+ led_imu = 0;
- led_imu = 1;
+}//end of 7/8 steps
+if(step % 8 == 7){
+
- //Retrieve inertial values from sensors
+ //IMU INPUT AND ANGLES COMPUTATION
+ //-------------------------------------------------------------------------------
+
{
- float a[3], g[3];
- IMU_sensor.getAccelero(a);
- IMU_sensor.getGyro(g);
-
- mainQuadState.sensor_accel_x += a[0]/4 - mainQuadState.sensor_accel_x/4;
- mainQuadState.sensor_accel_y += a[1]/4 - mainQuadState.sensor_accel_y/4;
- mainQuadState.sensor_accel_z += a[2]/4 - mainQuadState.sensor_accel_z/4;
- mainQuadState.sensor_gyro_r = g[0];
- mainQuadState.sensor_gyro_p = g[1];
- mainQuadState.sensor_gyro_y = g[2];
-
- //Compute IMU filters
- IMU_filter.updateFilter(g[0], g[1], g[2], a[0], a[1], a[2]);
+ float ypr[3];
+ led_imu = 1;
+ IMU_imu.getYawPitchRoll(ypr);
+ mainQuadState.estimated_position_z = IMU_imu.getBaroAlt();
+ IMU_imu.baro->getTemperature();
+ mainQuadState.estimated_rotation_y = ypr[0];
+ mainQuadState.estimated_rotation_p = ypr[1];
+ mainQuadState.estimated_rotation_r = ypr[2];
+ led_imu = 0;
}
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
- txQuadState.timestamp = time(NULL);
- if(TXRX_stateExchange(txQuadState,rxQuadState)){
- mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
- mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
- mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER;
- 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){
-
- //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
-
-
- led_esc = 1;
//ELABORATION
//-------------------------------------------------------------------------------
-
- //Measure the time
+
+ //Measure the time for main control cycle
QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
- //Get estimated 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();
- IMU_filter.getCorrectedEuler(
- mainQuadState.estimated_rotation_r,
- mainQuadState.estimated_rotation_p,
- mainQuadState.estimated_rotation_y );
- IMU_filter.getQuaternion(
- mainQuadState.estimated_rotation_q1,
- mainQuadState.estimated_rotation_q2,
- mainQuadState.estimated_rotation_q3,
- mainQuadState.estimated_rotation_q4);
+ //Elaborate inputs to determinate outputs
+ led_esc = 1;
- //Elaborate inputs to determinate outputs
{//Roll PID
float previous = mainQuadState.pid_rotation_r_error;
mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r;
@@ -225,7 +172,7 @@
+ mainQuadState.pid_rotation_r_kI * mainQuadState.pid_rotation_r_error_integrative
+ mainQuadState.pid_rotation_r_kD * mainQuadState.pid_rotation_r_error_derivative;
if(mainQuadState.pid_rotation_r_error_integrative > 100) mainQuadState.pid_rotation_r_error_integrative = 100;
- else if(mainQuadState.pid_rotation_r_error_integrative > -100) mainQuadState.pid_rotation_r_error_integrative = -100;
+ else if(mainQuadState.pid_rotation_r_error_integrative < -100) mainQuadState.pid_rotation_r_error_integrative = -100;
}{//Pitch PID
float previous = mainQuadState.pid_rotation_p_error;
mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p;
@@ -235,7 +182,7 @@
+ mainQuadState.pid_rotation_p_kI * mainQuadState.pid_rotation_p_error_integrative
+ mainQuadState.pid_rotation_p_kD * mainQuadState.pid_rotation_p_error_derivative;
if(mainQuadState.pid_rotation_p_error_integrative > 100) mainQuadState.pid_rotation_p_error_integrative = 100;
- else if(mainQuadState.pid_rotation_p_error_integrative > -100) mainQuadState.pid_rotation_p_error_integrative = -100;
+ else if(mainQuadState.pid_rotation_p_error_integrative < -100) mainQuadState.pid_rotation_p_error_integrative = -100;
}{//Yaw PID
float previous = mainQuadState.pid_rotation_y_error;
mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y;
@@ -245,7 +192,7 @@
+ mainQuadState.pid_rotation_y_kI * mainQuadState.pid_rotation_y_error_integrative
+ mainQuadState.pid_rotation_y_kD * mainQuadState.pid_rotation_y_error_derivative;
if(mainQuadState.pid_rotation_y_error_integrative > 100) mainQuadState.pid_rotation_y_error_integrative = 100;
- else if(mainQuadState.pid_rotation_y_error_integrative > -100) mainQuadState.pid_rotation_y_error_integrative = -100;
+ else if(mainQuadState.pid_rotation_y_error_integrative < -100) mainQuadState.pid_rotation_y_error_integrative = -100;
}
//Compute actual throttle values
if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
@@ -264,16 +211,6 @@
mainQuadState.reference_throttle_2 = rxQuadState.reference_throttle_2;
mainQuadState.reference_throttle_3 = rxQuadState.reference_throttle_3;
mainQuadState.reference_throttle_4 = rxQuadState.reference_throttle_4;
-
- /*float roll = 0;
- if(mainQuadState.target_rotation_r_isRequired){
- targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
- targetRoll_pid.setInterval(mainQuadState.average_main_dt);
- targetRoll_pid.setProcessValue(mainQuadState.estimated_rotation_r);
- roll = targetRoll_pid.compute();
- }else{
- targetRoll_pid.setSetPoint(mainQuadState.estimated_rotation_r);
- }*/
if(mainQuadState.reference_throttle_1 <= -1000)
mainQuadState.actual_throttle_1 = 0;
@@ -307,7 +244,6 @@
}
-
//OUTPUT
//-------------------------------------------------------------------------------
@@ -322,19 +258,35 @@
esc2();
esc3();
esc4();
-
-
-
led_esc = 0;
-//end of 12.5% frequency; step%8==3
-}
+
+} //end of 1/8 steps
+if(step % 24 == 21){
+ //REMOTE INPUT/OUTPUT EXCHANGE
+ //-------------------------------------------------------------------------------
+ led_txrx = 1;
+ txQuadState = mainQuadState; //send info back
+ txQuadState.timestamp = time(NULL);
+ if(TXRX_stateExchange(txQuadState,rxQuadState)){
+ mainQuadState.average_rx_dt = rxQuadState.average_rx_dt;
+ mainQuadState.actual_rx_dt = rxQuadState.actual_rx_dt;
+ mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER = rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER;
+ 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!!!
+ }
+ led_txrx = 0;
+
+
+}//end of 1/24 steps
+
++step;
}//End of main control loop
+
//It must not get here.
return 0;
}