Added motor query support

Basic actions: start stop main propeller
some minor fixes

hover-controller/PinConfig.h

@@ -1,25 +1,34 @@
 #ifndef __PIN_CONFIG_H__
 #define __PIN_CONFIG_H__
 
-#define RF_CE_PIN RPI_V2_GPIO_P1_22
 
-#define SERVO_MAX 2400 //2300
-#define SERVO_MIN 500
+// RF pins
+#define RF_CE_PIN   RPI_V2_GPIO_P1_22
 
-#define LEFT_RUDDER_GPIO 23
-#define LEFT_RUDDER_MAX SERVO_MAX
-#define LEFT_RUDDER_MIN SERVO_MIN
+// Servo pins & config
+#define SERVO_MAX   2400 //2300
+#define SERVO_MIN   500
 
-#define RIGHT_RUDDER_GPIO 24
-#define RIGHT_RUDDER_MAX SERVO_MAX
-#define RIGHT_RUDDER_MIN SERVO_MIN
+#define LEFT_RUDDER_GPIO    23
+#define LEFT_RUDDER_MAX     SERVO_MAX
+#define LEFT_RUDDER_MIN     SERVO_MIN
 
+#define RIGHT_RUDDER_GPIO   24
+#define RIGHT_RUDDER_MAX    SERVO_MAX
+#define RIGHT_RUDDER_MIN    SERVO_MIN
 
-#define ESC_MAX
-#define ESC_MIN
-#define ESC_REVERSE
+// ESC pins & config
+#define ESC_MAX     2020
+#define ESC_MIN     1000
+#define ESC_REVERSE 
+
+#define RIGHT_PROP_GPIO     RPI_V2_GPIO_P1_11
+#define LEFT_PROP_GPIO      RPI_V2_GPIO_P1_15
+
+#define MAIN_PROP_GPIO      RPI_V2_GPIO_P1_13
+
+#define MAIN_PROP_START_PPM 1060
+#define MAIN_PROP_STOP_PPM  2400
 
-#define RIGHT_PROP_GPIO
-#define LEFT_PROP_GPIO
 
 #endif // __PIN_CONFIG_H__

hover-controller/dispatcher.cpp

@@ -12,7 +12,7 @@ void process_msg(char*msg) {
 
 
     if (strcmp(msg_cmd, "ESC") == 0) {
-        
+        motor_process(msg);
     } else
     if (strcmp(msg_cmd, "SER") == 0) {
         servo_process(msg);

hover-controller/hover-controller.cpp

@@ -78,6 +78,10 @@ int main(int argc, char** argv) {
     if(gpioInitialise()<0) {
         perror("[pigpio] gpioInitialise error");
         exit(EXIT_FAILURE);
+    }else{
+        servo_mv_r(MAIN_PROP_GPIO, 0);
+        servo_mv_r(LEFT_PROP_GPIO, 0);
+        servo_mv_r(RIGHT_PROP_GPIO, 0);
     }
 
     // This opens two pipes for these two nodes to communicate

hover-controller/motors.cpp

@@ -0,0 +1,54 @@
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "motors.h"
+
+void motor_esc_initialize(unsigned int pin) {
+    //servo_mv_r(pin, 0);
+    //sleep(0.5);
+    servo_mv_r(pin, MAIN_PROP_STOP_PPM);
+    usleep(10000);
+    //sleep(1);
+    //servo_mv_r(pin, MAIN_PROP_START_PPM);
+}
+
+void motor_process(char*msg){
+    char servo_index = msg[4];
+    char servo_mode  = msg[5];
+    int lv;
+    char action[5];
+
+    static bool started=false;
+
+    sscanf(&msg[3]," %s ", action);
+    eprintf("Act: %s\n", action);
+
+    if (strcmp(action, "START") == 0) {
+        if (!started){
+            eprintf("Estarting ESC\n");
+            motor_esc_initialize(MAIN_PROP_GPIO);
+            servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_START_PPM);
+            started=true;
+        }else
+            eprintf("ESC already started\n");
+    } else
+    if (strcmp(action, "STOP") == 0) {
+        servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_STOP_PPM);
+        started=false;
+    } else {
+        sscanf(&msg[6], " %i ", &lv );
+    }
+
+    if (servo_index == 'M') {
+        servo_mv_r(MAIN_PROP_GPIO, lv);
+    } else
+    if (servo_index == 'L') {
+        servo_mv_r(LEFT_PROP_GPIO, lv);
+
+    } else
+    if (servo_index == 'R') {
+        servo_mv_r(RIGHT_PROP_GPIO, lv);
+
+    }
+}

hover-controller/motors.h

@@ -2,5 +2,6 @@
 #define __MOTORS_H__
 
 #include "servos.h"
+void motor_process(char*msg);
 
 #endif // __MOTORS_H__

hover-controller/servos.h

@@ -7,5 +7,7 @@
 #include "PinConfig.h"
 
 void servo_process(char*msg);
+void servo_mv_r(unsigned int pin, int lv);
+int map(int input, int input_start, int input_end, int output_start, int output_end);
 
 #endif // __SERVOS_H__

rfudp/TxTunnelSender.cpp

@@ -183,7 +183,7 @@ int main(int argc, char** argv){
 
         loop_start = millis();
         radio.startListening();
-        while ( !radio.available() && (millis() - loop_start) < 100) {
+        while ( !radio.available() && (millis() - loop_start) < 200) {
             // wait till receive or timeout
         }
         if (millis() - loop_start >= 100) {