Diff: main.cpp

Revision:

1:e11cc38ef502

Parent:

0:a8a417d50cc7

Child:

2:ce397d92f68e

--- a/main.cpp	Tue Aug 28 16:57:47 2018 +0000
+++ b/main.cpp	Mon Sep 10 20:11:24 2018 +0000
@@ -4,29 +4,65 @@
 #define CHANNELS_NUMBER 12 
 #define CMD_VALUE 65535
 #define CMD_GET_STATUS 1
-//DigitalOut myled(LED1);
+#define CMD_INITIALIZE 2
 
+#define RCV_TIMEOUT_MS 1000
 
 #define SERVO_MAX 2500
 #define SERVO_MIN 800
 #define SERVO_CENTER SERVO_MAX-SERVO_MIN
 
+#define STATE_INITILIZATION 0x00
+#define STATE_RUN 0x01
+#define STATE_FAILSAFE 0x02
+
 Serial pc(USBTX, USBRX); // tx, rx
  
 nRF24L01P radio(D11, D12, D13, D10, D9, D8);    // mosi, miso, sck, csn, ce, irq
 
 uint16_t channels[CHANNELS_NUMBER];
 uint16_t buffer  [CHANNELS_NUMBER]; 
+int bytesWritten;
 
-//PwmOut v_servo(PTC2);
+uint16_t state = ((uint16_t)0x00 << 8)|(uint8_t)STATE_INITILIZATION;
+
+/*
+PwmOut depthEsc1(PTC2);
+PwmOut depthEsc2(PTC2);
+PwmOut leftEsc(PTC2);
+PwmOut rightEsc(PTC2);
+*/
+
+Timer t;
+
+void setState(uint8_t newState) {
+    state=state | newState;
+}
+
+void initialize() {
+    state=((uint16_t)0x01 << 8) | state;
+}
+
+bool isInitializated() {
+    return (state>>8) & 0x01;
+}
 
 void zeroBuffer(){
     memset(buffer, 0, sizeof(buffer));     
 }
 
 int main() {
-    //v_servo.period_us(20000);          // servo requires a 20ms period
-    //v_servo.pulsewidth_us(SERVO_CENTER);
+    /*
+    depthEsc1.period_us(20000);          // servo requires a 20ms period
+    depthEsc1.pulsewidth_us(SERVO_CENTER);
+    depthEsc2.period_us(20000);          // servo requires a 20ms period
+    depthEsc2.pulsewidth_us(SERVO_CENTER);
+    leftEsc.period_us(20000);          // servo requires a 20ms period
+    leftEsc.pulsewidth_us(SERVO_CENTER);
+    rightEsc.period_us(20000);          // servo requires a 20ms period
+    rightEsc.pulsewidth_us(SERVO_CENTER);
+    
+    */
     //v_servo.pulsewidth_us(pulse); // servo position determined by a pulsewidth between 1-2ms
     
     radio.powerUp();
@@ -51,11 +87,24 @@
     radio.enable();
     
     pc.printf("Greetings, capitain!\r\n");
+    pc.printf("is initializated: %s\r\n", isInitializated()?"true":"false");
+    pc.printf( "0x%X\r\n", state );
+    t.start();
     
     while (1)
     {
-        while( !radio.readable() );
-        {
+        int startTime=t.read_ms();
+        while( !radio.readable() && t.read_ms()-startTime < RCV_TIMEOUT_MS);
+        
+        if(!radio.readable()) {
+            //failsafe
+            setState(STATE_FAILSAFE);
+            pc.printf("Failsafe\n\r", channels[0],channels[1],channels[2],channels[3]);
+            channels[0]=512;
+            channels[1]=512;
+            channels[2]=512;
+            channels[3]=512;
+        } else {
             int rxDataCnt = radio.read( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
             
             if (buffer[0] == CMD_VALUE) {
@@ -63,18 +112,24 @@
                 switch(buffer[1]){
                     case CMD_GET_STATUS:
                         zeroBuffer();
-                        strcpy((char*)buffer,"I'm alive");
+                        //strcpy((char*)buffer,"I'm alive");
+                        buffer[0]=state;
+                        buffer[1]=t.read_us();
                         radio.setTransmitMode();
                         //wait(0.1);
-                        int bytesWritten = radio.write( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
+                        bytesWritten = radio.write( NRF24L01P_PIPE_P0, (char*)buffer, sizeof( buffer ) );
                         if (bytesWritten<2*CHANNELS_NUMBER){
                             pc.printf("Transmit error\r\n");
                         } else {
-                            pc.printf("Sent telemetry: %s\r\n",(char*)buffer);
+                            //pc.printf("Sent telemetry: %s\r\n",(char*)buffer);
                         }
                         radio.setReceiveMode();
                         //wait(0.1);
                     break;
+                    case CMD_INITIALIZE:
+                        initialize();
+                        setState(STATE_RUN);
+                    break;
                 }    
             } else {
                 for(int i=0; i<CHANNELS_NUMBER; i++){
@@ -83,6 +138,8 @@
                 pc.printf("received channels: %d %d %d %d\n\r", channels[0],channels[1],channels[2],channels[3]);
             }
         }
+        
+        
     }
     
 }
\ No newline at end of file