Matteo Terruzzi
Work in progress...
Dependencies: ESC FreeIMU mbed-rtos mbed
Experiment - work in progress...
Diff: main.cpp
- Revision:
- 4:46f3c3dd5977
- Parent:
- 3:5e7476bca25f
- Child:
- 5:33abcc31b0aa
--- a/main.cpp Sun Oct 27 01:05:35 2013 +0000
+++ b/main.cpp Sat May 03 13:22:30 2014 +0000
@@ -41,6 +41,7 @@
#include "state.h"
+#include "IMU.h"
#include "TXRX.h"
int main()
@@ -65,7 +66,7 @@
//Setup Ethernet
EthernetInterface interface;
- interface.init("192.168.1.16","255.255.255.0","192.168.1.1");
+ interface.init("192.168.2.16","255.255.255.0","192.168.2.1");
interface.connect();
//Setup remote control
@@ -104,10 +105,9 @@
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);
+ /*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);
-
+ targetRoll_pid.setOutputLimits(0,0.5);*/
//MAIN CONTROL CYCLE
@@ -119,7 +119,7 @@
//IMU INPUT
//-------------------------------------------------------------------------------
- //Measure time
+ //Measure the time and sleep to correct the interval
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)
@@ -132,14 +132,20 @@
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]);
}
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;
@@ -156,9 +162,11 @@
//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!!!
}
@@ -189,16 +197,57 @@
//ELABORATION
//-------------------------------------------------------------------------------
- //Measure time
+ //Measure the time
QUAD_STATE_UPDATE_DT(mainQuadState,main,dt_timer)
- //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();
+ //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
+ {//Roll PID
+ float previous = mainQuadState.pid_rotation_r_error;
+ mainQuadState.pid_rotation_r_error = mainQuadState.target_rotation_r - mainQuadState.estimated_rotation_r;
+ mainQuadState.pid_rotation_r_error_integrative += mainQuadState.pid_rotation_r_error;
+ mainQuadState.pid_rotation_r_error_derivative = mainQuadState.pid_rotation_r_error - previous;
+ mainQuadState.pid_rotation_r_out = mainQuadState.pid_rotation_r_kP * mainQuadState.pid_rotation_r_error
+ + 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;
+ }{//Pitch PID
+ float previous = mainQuadState.pid_rotation_p_error;
+ mainQuadState.pid_rotation_p_error = mainQuadState.target_rotation_p - mainQuadState.estimated_rotation_p;
+ mainQuadState.pid_rotation_p_error_integrative += mainQuadState.pid_rotation_p_error;
+ mainQuadState.pid_rotation_p_error_derivative = mainQuadState.pid_rotation_p_error - previous;
+ mainQuadState.pid_rotation_p_out = mainQuadState.pid_rotation_p_kP * mainQuadState.pid_rotation_p_error
+ + 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;
+ }{//Yaw PID
+ float previous = mainQuadState.pid_rotation_y_error;
+ mainQuadState.pid_rotation_y_error = mainQuadState.target_rotation_y - mainQuadState.estimated_rotation_y;
+ mainQuadState.pid_rotation_y_error_integrative += mainQuadState.pid_rotation_y_error;
+ mainQuadState.pid_rotation_y_error_derivative = mainQuadState.pid_rotation_y_error - previous;
+ mainQuadState.pid_rotation_y_out = mainQuadState.pid_rotation_y_kP * mainQuadState.pid_rotation_y_error
+ + 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;
+ }
+ //Compute actual throttle values
if(mainQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0
|| rxQuadState.ZERO_MEANS_ZERO_ALL_MOTORS_NOW__DANGER == 0){
mainQuadState.actual_throttle_1 = 0;
@@ -211,8 +260,12 @@
mainQuadState.target_rotation_r = rxQuadState.target_rotation_r;
mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
+ mainQuadState.reference_throttle_1 = rxQuadState.reference_throttle_1;
+ 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;
+ /*float roll = 0;
if(mainQuadState.target_rotation_r_isRequired){
targetRoll_pid.setSetPoint(mainQuadState.target_rotation_r);
targetRoll_pid.setInterval(mainQuadState.average_main_dt);
@@ -220,32 +273,36 @@
roll = targetRoll_pid.compute();
}else{
targetRoll_pid.setSetPoint(mainQuadState.estimated_rotation_r);
- }
+ }*/
- if(rxQuadState.requested_throttle_1 >= 0 && rxQuadState.requested_throttle_1 <=1)
- mainQuadState.actual_throttle_1 = rxQuadState.requested_throttle_1;
+ if(mainQuadState.reference_throttle_1 <= -1000)
+ mainQuadState.actual_throttle_1 = 0;
else{
- float out = 0.5; //NOTE: here we will have full throttle calcs..
- out -= roll;
+ float out = mainQuadState.reference_throttle_1; //NOTE: here we will have full throttle calcs..
+ out -= mainQuadState.pid_rotation_r_out;
mainQuadState.actual_throttle_1 = out;
}
- if(rxQuadState.requested_throttle_2 >= 0 && rxQuadState.requested_throttle_2 <=1)
- mainQuadState.actual_throttle_2 = rxQuadState.requested_throttle_2;
+ if(mainQuadState.reference_throttle_2 <= -1000)
+ mainQuadState.actual_throttle_2 = 0;
else{
- mainQuadState.actual_throttle_2 = 0;
+ float out = mainQuadState.reference_throttle_2;
+ mainQuadState.actual_throttle_2 = out;
}
- if(rxQuadState.requested_throttle_3 >= 0 && rxQuadState.requested_throttle_3 <=1)
- mainQuadState.actual_throttle_3 = rxQuadState.requested_throttle_3;
+
+ if(mainQuadState.reference_throttle_3 <= -1000)
+ mainQuadState.actual_throttle_3 = 0;
else{
- float out = 0.5;
- out += roll;
+ float out = mainQuadState.reference_throttle_3;
+ out += mainQuadState.pid_rotation_r_out;
mainQuadState.actual_throttle_3 = out;
}
- if(rxQuadState.requested_throttle_4 >= 0 && rxQuadState.requested_throttle_4 <=1)
- mainQuadState.actual_throttle_4 = rxQuadState.requested_throttle_4;
+
+ if(mainQuadState.reference_throttle_4 <= -1000)
+ mainQuadState.actual_throttle_4 = 0;
else{
- mainQuadState.actual_throttle_4 = 0;
+ float out = mainQuadState.reference_throttle_4;
+ mainQuadState.actual_throttle_4 = out;
}
}