Fix Makefile error

this would prevent make from recompiling when the source has changed
Fixed crash when 0 length packet was send to tunnel
2019-05-23 04:13:57 +02:00 · 2019-05-23 04:13:07 +02:00 · 2019-05-23 03:36:18 +02:00 · 2019-05-23 03:34:56 +02:00 · 2019-05-23 02:01:03 +02:00
15 changed files with 362 additions and 48 deletions
--- a/hover-controller/Makefile
+++ b/hover-controller/Makefile
@ -17,21 +17,47 @@ RF_LIB=rf24
 RF_LIB_DIR=/usr/local/lib
 RF_HEADER_DIR=/usr/local/include/RF24

+GPIO_LIB=pigpio
+
 # define all programs
 PROGRAMS =  hover-controller
 SOURCES = ${PROGRAMS:=.cpp}

+PARTS = motors servos sensors dispatcher
+PARTS_O = ${PARTS:=.o}
+
+SEN_PARTS = sensors/temperature.o
+

 all: ${PROGRAMS} tests


-${PROGRAMS}: ${SOURCES}
-	$(CXX) $(CFLAGS) -I$(RF_HEADER_DIR) -L$(RF_LIB_DIR) -l$(RF_LIB) $@.cpp -o $@
+#${PROGRAMS}: ${SOURCES}
+#	$(CXX) $(CFLAGS) -I$(RF_HEADER_DIR) -L$(RF_LIB_DIR) -l$(RF_LIB) $@.cpp -o $@

+hover-controller: hover-controller.cpp ${PARTS_O}
+	$(CXX) $(CFLAGS) -I$(RF_HEADER_DIR) -L$(RF_LIB_DIR) -l$(RF_LIB) -l$(GPIO_LIB) -lrt $@.cpp \
+	${SEN_PARTS} ${PARTS_O} -o $@
+
+dispatcher.o: dispatcher.cpp
+	$(CXX) $(CFLAGS) dispatcher.cpp -o $@ -c
+
+servos.o: servos.cpp
+	$(CXX) $(CFLAGS) servos.cpp -o $@ -c
+
+motors.o: motors.cpp
+	$(CXX) $(CFLAGS) motors.cpp -o $@ -c
+
+sensors.o: sensors.cpp ${SEN_PARTS}
+	$(CXX) $(CFLAGS) sensors.cpp -o $@ -c
+
+sensors/%.o: sensors/%.cpp
+	$(CXX) $(CFLAGS) $< -o $@ -c

 tests: 


-.PHONY: clean
+.PHONY: clean remake
 clean:
-	rm -rf $(PROGRAMS) *.o
+	rm -rf $(PROGRAMS) $(PARTS_O) *.o sensors/*.o
+remake: clean all
--- a/hover-controller/PinConfig.h
+++ b/hover-controller/PinConfig.h
@ -0,0 +1,25 @@
+#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
+
+#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
+
+#define RIGHT_PROP_GPIO
+#define LEFT_PROP_GPIO
+
+#endif // __PIN_CONFIG_H__
--- a/hover-controller/dispatcher.cpp
+++ b/hover-controller/dispatcher.cpp
@ -0,0 +1,26 @@
+#include <string.h>
+
+#include "dispatcher.h"
+
+
+void process_msg(char*msg) {
+
+    char msg_cmd[3+1];
+
+    strncpy(msg_cmd, msg, 3);
+    msg_cmd[3]='\0';
+
+
+    if (strcmp(msg_cmd, "ESC") == 0) {
+        
+    } else
+    if (strcmp(msg_cmd, "SER") == 0) {
+        servo_process(msg);
+    } else
+    if (strcmp(msg_cmd, "SEN") == 0) {
+        sensor_process(msg);
+    } else {
+        eprintf("CMD not valid");
+        strcpy(msg, "CMD_NOK");
+    }
+}
--- a/hover-controller/dispatcher.h
+++ b/hover-controller/dispatcher.h
@ -0,0 +1,12 @@
+#ifndef __DISPACHER_H__
+#define __DISPACHER_H__
+
+#include "../rfudp/TxTunnel.h"
+#include "PinConfig.h"
+#include "sensors.h"
+#include "servos.h"
+#include "motors.h"
+
+void process_msg(char*msg);
+
+#endif // __DISPACHER_H__
--- a/hover-controller/hover-controller.cpp
+++ b/hover-controller/hover-controller.cpp
@ -14,6 +14,8 @@ TMRh20 2014

 #include "../rfudp/TxTunnel.h"

+#include "dispatcher.h"
+
 using namespace std;
 //
 // Hardware configuration
@ -25,7 +27,8 @@ using namespace std;
 // See http://www.airspayce.com/mikem/bcm2835/group__constants.html#ga63c029bd6500167152db4e57736d0939 and the related enumerations for pin information.

 // Setup for GPIO 15 CE and CE0 CSN with SPI Speed @ 8Mhz
-RF24 radio(RPI_V2_GPIO_P1_15, RPI_V2_GPIO_P1_24, BCM2835_SPI_SPEED_32MHZ);
+//RF24 radio(RPI_V2_GPIO_P1_15, RPI_V2_GPIO_P1_24, BCM2835_SPI_SPEED_32MHZ);
+RF24 radio(RF_CE_PIN, RPI_V2_GPIO_P1_24, BCM2835_SPI_SPEED_32MHZ);


 // Radio pipe addresses for the 2 nodes to communicate. From "TxTunnel.h"
@ -34,11 +37,11 @@ RF24 radio(RPI_V2_GPIO_P1_15, RPI_V2_GPIO_P1_24, BCM2835_SPI_SPEED_32MHZ);
 // loop condition
 sig_atomic_t loop_on = TRUE;

-void signal_handler(int sig){
+void signal_handler(int sig) {
  loop_on = FALSE;
 }

-int main(int argc, char** argv){
+int main(int argc, char** argv) {


    char buffer[MAX_MSG_SZ+1];
@ -53,7 +56,7 @@ int main(int argc, char** argv){
    sigaction(SIGTERM, &sa, NULL);


-/* RF setup */
+    /* RF setup */
    radio.begin();                      // Setup and configure rf radio
    radio.powerUp();
    #ifdef RF_CHANNEL
@ -69,7 +72,13 @@ int main(int argc, char** argv){
    radio.setRetries(2,10);                  // Optionally, increase the delay between retries & # of retries
    radio.setCRCLength(RF24_CRC_8);     // Use 8-bit CRC for performance

+    radio.printDetails();

+    /* GPIO setup */
+    if(gpioInitialise()<0) {
+        perror("[pigpio] gpioInitialise error");
+        exit(EXIT_FAILURE);
+    }

    // This opens two pipes for these two nodes to communicate
    // back and forth.
@ -77,7 +86,7 @@ int main(int argc, char** argv){
    {
      radio.openWritingPipe(addresses[1]);
      radio.openReadingPipe(1,addresses[0]);
-	  radio.stopListening();
+      radio.stopListening();
    } else {
      radio.openWritingPipe(addresses[0]);
      radio.openReadingPipe(1,addresses[1]);
@ -90,6 +99,8 @@ int main(int argc, char** argv){
    uint8_t pipeNo;
    memset(buffer, 0, sizeof(buffer));

+    printf("Starting listenning for messages\n");
+
    do{
        // GET DATA FROM CLIENT
        if(radio.available(&pipeNo)){
@ -109,29 +120,36 @@ int main(int argc, char** argv){


        /* START process Part */
-
+        process_msg(buffer);
+        n = strlen(buffer);

        /* END process Part */

+        printf("ANS: \'%s\'\n", buffer);
+
        // Answer the client
-		#if ACK_MODE		//ACK MODE
+        #if ACK_MODE        //ACK MODE
        radio.writeAckPayload(pipeNo, buffer, n );
-		#else
-		radio.stopListening();
+        #else
+        radio.stopListening();
        if ( radio.write(buffer,n) )
-			printf("sending correct\n");
-		else
-			eprintf("error\n");
-		radio.startListening();
-		#endif
-
+            printf("sending correct\n");
+        else
+            eprintf("error\n");
+        radio.startListening();
+        #endif

+        //clean buffer
+        //buffer[0]='\0';
+        //memset(buffer, 0, sizeof(buffer));

    }while(loop_on);
    
    // Power down the antenna
    eprintf("Powering down the antenna");
    radio.powerDown();
+    eprintf("Terminating PiGPIO");
+    gpioTerminate();
    eprintf("\n");
 }

--- a/hover-controller/motors.cpp
+++ b/hover-controller/motors.cpp
--- a/hover-controller/motors.h
+++ b/hover-controller/motors.h
@ -0,0 +1,6 @@
+#ifndef __MOTORS_H__
+#define __MOTORS_H__
+
+#include "servos.h"
+
+#endif // __MOTORS_H__
--- a/hover-controller/sensors.cpp
+++ b/hover-controller/sensors.cpp
@ -0,0 +1,51 @@
+#include <stdio.h>
+
+#include "servos.h"
+#include "sensors/temperature.h"
+
+void sensor_gyro();
+void sensor_acel();
+void sensor_ulson();
+void sensor_laser();
+void sensor_temp(char*msg, char index) {
+    float temp;
+    if (index == 'C') {
+        temp = temp_cpu();
+    } else
+    if (index == 'G') {
+        temp = temp_gpu();
+    } else {
+        eprintf("[sensor:temp] class not valid (%c)\n", index);
+        strcpy(msg, "SEN_TMP_CLS_NOK");
+        return;
+    }
+    sprintf(msg, "SEN T%c %.2f", index,  temp);
+}
+
+void sensor_process(char*msg) {
+    char sensor_type = msg[4];
+    char sensor_index = msg[5];
+    int lv;
+
+    sscanf(&msg[6], " %i ", &lv );
+
+    if (sensor_type == 'G') {
+
+    } else
+    if (sensor_type == 'A') {
+
+    } else
+    if (sensor_type == 'U') {
+
+    } else
+    if (sensor_type == 'L') {
+
+    } else
+    if (sensor_type == 'T') {
+        sensor_temp(msg, sensor_index);
+    } else {
+        eprintf("[sensor] type not valid (%c)\n", sensor_type);
+        strcpy(msg, "SEN_TYP_NOK");
+        return;
+    }
+}
--- a/hover-controller/sensors.h
+++ b/hover-controller/sensors.h
@ -0,0 +1,9 @@
+#ifndef __SENSORS_H__
+#define __SENSORS_H__
+
+#ifdef __arm__
+#endif
+
+void sensor_process(char*msg);
+
+#endif // __SENSORS_H__
--- a/hover-controller/sensors/temperature.cpp
+++ b/hover-controller/sensors/temperature.cpp
@ -0,0 +1,38 @@
+#include "temperature.h"
+
+#include <stdio.h>
+//#include <string.h>
+
+float temp_cpu(){
+    float systemp;
+    float millideg;
+    FILE *thermal;
+
+    thermal = fopen("/sys/class/thermal/thermal_zone0/temp","r");
+    fscanf(thermal,"%f",&millideg);
+    fclose(thermal);
+    systemp = millideg / 1000;
+
+    fprintf(stderr, "[sensor:temp:cpu] CPU temperature is %f degrees C\n",systemp);
+
+    return systemp;
+
+}
+
+float temp_gpu(){
+    float systemp = -127;
+  #ifdef __Pi__
+    FILE *fp;
+    fp = popen("/opt/vc/bin/vcgencmd measure_temp | cut -c6-9", "r");
+    if(fp == NULL){
+        perror("[sensor:temp:gpu] GPU command failed!\nAre you on a Pi?\n");
+        return -127;
+    }
+    fscanf(fp,"%f", &systemp);
+    pclose(fp);
+  #else
+  #endif
+
+    return systemp;
+
+}
--- a/hover-controller/sensors/temperature.h
+++ b/hover-controller/sensors/temperature.h
@ -0,0 +1,13 @@
+#ifndef __SENSORS_TEMP_H__
+#define __SENSORS_TEMP_H__
+
+#ifdef __arm__
+  #if __has_include("/opt/vc/include/bcm_host.h")
+    #define __Pi__
+  #endif
+#endif
+
+float temp_cpu();
+float temp_gpu();
+
+#endif // __SENSORS_TEMP_H__
--- a/hover-controller/servos.cpp
+++ b/hover-controller/servos.cpp
@ -0,0 +1,70 @@
+#include <stdio.h>
+#include <string.h>
+
+#include "PinConfig.h"
+#include "servos.h"
+
+
+void servo_mv_r(unsigned int pin, int lv) {
+        gpioSetMode(pin, PI_OUTPUT);
+        gpioServo(pin, lv);
+}
+
+int map(int input, int input_start, int input_end, int output_start, int output_end) {
+    return output_start + ((output_end - output_start) / (input_end - input_start)) * (input - input_start);
+}
+
+int conver_pc_raw(unsigned pin, int lv) {
+    int lv_new;
+    switch(pin) {
+        case LEFT_RUDDER_GPIO:
+            lv_new = map(lv, 0, 100, LEFT_RUDDER_MIN, LEFT_RUDDER_MAX);
+            break;
+        case RIGHT_RUDDER_GPIO:
+            lv_new=map(lv, 0, 100, RIGHT_RUDDER_MIN, RIGHT_RUDDER_MAX);
+            break;
+        default:
+            return -1;
+    }
+    return lv_new;
+}
+
+void servo_mv(char*msg, char* mode, unsigned int pin, int *lv) {
+    if (*mode == ' ')
+        *mode = ( *lv < 500 ) ? '%':'R';
+
+    if (*mode == '%') {
+        *lv=conver_pc_raw(pin, *lv);
+    } else
+    if (*mode == 'R') {
+    } else {
+        eprintf("[servos] mode not valid (%c)\n", *mode);
+        strcpy(msg, "SER_MODE_NOK");
+        return;
+    }
+
+    servo_mv_r(pin, *lv);
+
+}
+
+void servo_process(char*msg) {
+    char servo_index = msg[4];
+    char servo_mode  = msg[5];
+    int lv;
+
+    sscanf(&msg[6], " %i ", &lv );
+
+    if (servo_index == 'L') {
+        servo_mv(msg, &servo_mode, LEFT_RUDDER_GPIO, &lv);
+    } else
+    if (servo_index == 'R') {
+        servo_mv(msg, &servo_mode, RIGHT_RUDDER_GPIO, &lv);
+    } else {
+        eprintf("[servos] index not valid (%c)\n", servo_index);
+        strcpy(msg, "SER_INDX_NOK");
+        return;
+    }
+    sprintf(msg, "SER %c%c %i", servo_index, servo_mode, lv);
+}
+
+
--- a/hover-controller/servos.h
+++ b/hover-controller/servos.h
@ -0,0 +1,11 @@
+#ifndef __SERVOS_H__
+#define __SERVOS_H__
+
+#include <pigpio.h>
+
+#include "../rfudp/TxTunnel.h"
+#include "PinConfig.h"
+
+void servo_process(char*msg);
+
+#endif // __SERVOS_H__
--- a/rfudp/TxTunnelSender.cpp
+++ b/rfudp/TxTunnelSender.cpp
@ -56,7 +56,7 @@ int main(int argc, char** argv){
    sigaction(SIGTERM, &sa, NULL);


-/* UDP LISTENER setup */
+    /* UDP LISTENER setup */

    // Creating socket file descriptor
    if ( (sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0 ) {
@ -83,7 +83,8 @@ int main(int argc, char** argv){
    }


-/* RF setup */
+    /* RF setup */
+
    radio.begin();                      // Setup and configure rf radio
    radio.powerUp();
    #ifdef RF_CHANNEL
@ -100,12 +101,12 @@ int main(int argc, char** argv){
    radio.setCRCLength(RF24_CRC_8);     // Use 8-bit CRC for performance


-/* Print setup details*/
+    /* Print setup details*/
    printf("================ UDP Configuration ================\n");
-    printf("SERVER IP	 = %s\n", inet_ntoa(servaddr.sin_addr));
-    printf("SERVER PORT	 = %i\n", ntohs(servaddr.sin_port));
-    printf("MSG MAX SIZE	 = %i\n", MAX_MSG_SZ);
-    printf("ACK MODE	 = %s\n", ACK_MODE? "true":"false");
+    printf("SERVER IP     = %s\n", inet_ntoa(servaddr.sin_addr));
+    printf("SERVER PORT     = %i\n", ntohs(servaddr.sin_port));
+    printf("MSG MAX SIZE     = %i\n", MAX_MSG_SZ);
+    printf("ACK MODE     = %s\n", ACK_MODE? "true":"false");
    radio.printDetails();
    printf("\n");

@ -117,7 +118,7 @@ int main(int argc, char** argv){
    {
      radio.openWritingPipe(addresses[1]);
      radio.openReadingPipe(1,addresses[0]);
-	  radio.stopListening();
+      radio.stopListening();
    } else {
      radio.openWritingPipe(addresses[0]);
      radio.openReadingPipe(1,addresses[1]);
@ -127,9 +128,9 @@ int main(int argc, char** argv){

    // 
    int n;
-	#if !ACK_MODE
-	unsigned long loop_start;
-	#endif
+    #if !ACK_MODE
+    unsigned long loop_start;
+    #endif
    unsigned int cliaddr_len = sizeof(cliaddr);
    memset(buffer, 0, sizeof(buffer));

@ -153,11 +154,15 @@ int main(int argc, char** argv){
            inet_ntoa(cliaddr.sin_addr),    // addrress
            ntohs(cliaddr.sin_port)         // port
        );
+        if ( n ==0 ) {
+            printf("empty msg, discarded\n");
+            continue;
+        }
        printf("MSG: \'%s\'\n", buffer);


        /* START RF Part */
-		#if ACK_MODE
+      #if ACK_MODE
        if ( radio.write(buffer,n) ){
            //usleep(10000);
            if(!radio.available()){
@ -171,28 +176,27 @@ int main(int argc, char** argv){
            // If no ack response, sending failed
            eprintf("Sending failed.\n\r");
            strcpy(buffer,"TxERR\0");
-        	n=strlen(buffer);
+            n=strlen(buffer);
        }
-		#else
-		radio.write(buffer,n);
+      #else
+        radio.write(buffer,n);

-		loop_start = millis();
-		radio.startListening();
-		while ( !radio.available() && (millis() - loop_start) < 100) {
-    	// Serial.println(F("waiting."));
-		}
-		if (millis() - loop_start >= 100) {
+        loop_start = millis();
+        radio.startListening();
+        while ( !radio.available() && (millis() - loop_start) < 100) {
+            // wait till receive or timeout
+        }
+        if (millis() - loop_start >= 100) {
            eprintf("Not Response.\n\r");
            strcpy(buffer,"TxERR\0");
-        	n=strlen(buffer);
-		} else {
-		  n = radio.getDynamicPayloadSize();
-		  radio.read(buffer, MAX_MSG_SZ);
-		}
-		radio.stopListening();
-		#endif
+            n=strlen(buffer);
+        } else {
+          n = radio.getDynamicPayloadSize();
+          radio.read(buffer, MAX_MSG_SZ);
+        }
+        radio.stopListening();
+      #endif
        buffer[n] = '\0';
-
        /* END RF Part */

        printf("ANS: \'%s\'\n", buffer);
@ -203,6 +207,10 @@ int main(int argc, char** argv){
             (const struct sockaddr *) &cliaddr, cliaddr_len
        );

+        //clean buffer
+        //buffer[0]='\0';
+        //memset(buffer, 0, sizeof(buffer));
+
    }while(loop_on);
    
    // Power down the antenna
--- a/rfudp/client_test.cpp
+++ b/rfudp/client_test.cpp
@ -63,5 +63,6 @@ do{

  buffer[n] = '\0';
  printf("ANS: \'%s\'\n", buffer);
+  //buffer[0]='\0';
 }while(loop_on);
 }
Author	SHA1	Message	Date
Angel Garcia	4a82ee605f	Fix Makefile error this would prevent make from recompiling when the source has changed	2019-05-23 04:13:57 +02:00
Angel Garcia	0bbf7ad5e4	Fixed crash when 0 length packet was send to tunnel Now UDP packets with 0 length are discarded without answer to sender. Minor	2019-05-23 04:13:07 +02:00
Angel Garcia	e0987aa83a	Added sensor query support Añadidos sensores de temperatura del propio dispositivo	2019-05-23 03:36:18 +02:00
Angel Garcia	7f2e176f41	Changed antenna CE pin Hecho para dejar una mejor organización en los GPIO para el resto de conexiones, dejando los cables dela antena mas juntos y en una zona mas concreta	2019-05-23 03:34:56 +02:00
Angel Garcia	90f27fa3d0	Order dispatcher and servo move Definida estructura del progrma (mas o menos) Un gestor de ordenes que las pasa a sus respectivos lugares dependiendo del tipo de las mismas Movimiento de servos, tanto raw ppm como por porcentaje	2019-05-23 02:01:03 +02:00