Diff: main.cpp

Revision:

2:7439607ccd51

Parent:

1:acb2e0f9d1bc

Child:

3:5e7476bca25f

--- a/main.cpp	Fri Jul 19 06:45:20 2013 +0000
+++ b/main.cpp	Tue Oct 15 18:30:46 2013 +0000
@@ -1,39 +1,160 @@
-#include <stdint.h>
+#include "math.h"
 #include "mbed.h"
-#include "SharedObject.h"
-#include "RefRX.h"
+#include "rtos.h"
+//#include "EthernetInterface.h"
+//#include "RefRX.h"
 #include "esc.h"
-
-namespace SharedResources
-{
-    SharedObject <uint32_t> ref1 (0);
-    SharedObject <uint32_t> ref2 (0);
-    SharedObject <uint32_t> ref3 (0);
-    SharedObject <uint32_t> ref4 (0);
-    SharedObject <float>    integration_time (0);
-};
-
+#include "state.h"
+#include "TXRX.h"
+#include "IMU.h"
+#include "PID.h"
 
 int main()
 {
     //Setup ESCs
-    ESC esc1(p26);
-    ESC esc2(p27);
-    ESC esc3(p28);
-    ESC esc4(p29);
+    ESC esc1(p23);
+    ESC esc2(p24);
+    ESC esc3(p25);
+    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
+    
+    //Setup Ethernet
+    //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
-    RefRX::init(2222, 2223, &SharedResources::ref1, &SharedResources::ref2, &SharedResources::ref3, &SharedResources::ref4);
+    //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");
+    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");
     
+    //Setup state
+    QuadState mainQuadState;  mainQuadState.reset();
+    QuadState rxQuadState;    rxQuadState.reset();
+    QuadState txQuadState;    txQuadState.reset();
+    //pc.printf("Main and rx states setted up\r\n");
+    
+    //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;
+    
+    //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
-    while(1) {
+    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;
+        }
+        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!!!
+        
+        //retrive inertial values
+        IMU_filter_getYawPitchRoll(
+            mainQuadState.estimated_rotation_y,
+            mainQuadState.estimated_rotation_p,
+            mainQuadState.estimated_rotation_r );
+    
+        //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;
         
+        //elaborate signals
+        if(mainQuadState.ZERO_ALL_MOTORS_NOW__DANGER){
+            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.target_rotation_r = rxQuadState.target_rotation_r;
+            mainQuadState.target_rotation_r_isRequired = rxQuadState.target_rotation_r_isRequired;
+            
+        
+            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.setProcessValue(mainQuadState.estimated_rotation_r);
+                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;
+            else{
+                float out = 0.5; //NOTE: here we will have full throttle calcs..
+                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{
+                mainQuadState.actual_throttle_2 = 0;
+            }
+            if(rxQuadState.requested_throttle_3 >= 0 && rxQuadState.requested_throttle_3 <=1)
+                mainQuadState.actual_throttle_3 = rxQuadState.requested_throttle_3;
+            else{
+                float out = 0.5;
+                out += roll;
+                mainQuadState.actual_throttle_3 = out;
+            }
+            if(rxQuadState.requested_throttle_4 >= 0 && rxQuadState.requested_throttle_4 <=1)
+                mainQuadState.actual_throttle_4 = rxQuadState.requested_throttle_4;
+            else{
+                mainQuadState.actual_throttle_4 = 0;
+            }
+        }
         
         //compute new ESC values..
-        
-        //set it
-        esc1 = 0.5;
+        esc1 = mainQuadState.actual_throttle_1;
+        esc2 = mainQuadState.actual_throttle_2;
+        esc3 = mainQuadState.actual_throttle_3;
+        esc4 = mainQuadState.actual_throttle_4;
         
         //finally output to ESCs
         esc1();
@@ -41,6 +162,40 @@
         esc3();
         esc4();
         
-        Thread::wait(15);
+        led1 = !led1;
+        
+        
+        uint32_t w = 20-dt;
+        if(w<5) w=5;
+        else if(w>19) w=19;
+        Thread::wait(w /*ms*/);
+        
+        
+        //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;*/
     }
+    
+    return 0;
 }