Added sensor query support

Añadidos sensores de temperatura del propio dispositivo

hover-controller/Makefile

@@ -26,6 +26,8 @@ SOURCES = ${PROGRAMS:=.cpp}
 PARTS = motors servos sensors dispatcher
 PARTS_O = ${PARTS:=.o}
 
+SEN_PARTS = sensors/temperature.o
+
 
 all: ${PROGRAMS} tests
 
@@ -35,7 +37,7 @@ all: ${PROGRAMS} tests
 
 hover-controller: $@ ${PARTS_O}
 	$(CXX) $(CFLAGS) -I$(RF_HEADER_DIR) -L$(RF_LIB_DIR) -l$(RF_LIB) -l$(GPIO_LIB) -lrt $@.cpp \
-	${PARTS_O} -o $@
+	${SEN_PARTS} ${PARTS_O} -o $@
 
 dispatcher.o: dispatcher.cpp
 	$(CXX) $(CFLAGS) dispatcher.cpp -o $@ -c
@@ -46,13 +48,16 @@ servos.o: servos.cpp
 motors.o: motors.cpp
 	$(CXX) $(CFLAGS) motors.cpp -o $@ -c
 
-sensors.o: sensors.cpp
+sensors.o: sensors.cpp ${SEN_PARTS}
 	$(CXX) $(CFLAGS) sensors.cpp -o $@ -c
 
+sensors/%.o: sensors/%.cpp
+	$(CXX) $(CFLAGS) $< -o $@ -c
+
 tests: 
 
 
 .PHONY: clean remake
 clean:
-	rm -rf $(PROGRAMS) $(PARTS_O) *.o
+	rm -rf $(PROGRAMS) $(PARTS_O) *.o sensors/*.o
 remake: clean all

hover-controller/dispatcher.cpp

@@ -18,7 +18,7 @@ void process_msg(char*msg) {
         servo_process(msg);
     } else
     if (strcmp(msg_cmd, "SEN") == 0) {
-        
+        sensor_process(msg);
     } else {
         eprintf("CMD not valid");
         strcpy(msg, "CMD_NOK");

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;
+    }
+}

hover-controller/sensors.h

@@ -1,5 +1,9 @@
 #ifndef __SENSORS_H__
 #define __SENSORS_H__
 
+#ifdef __arm__
+#endif
+
+void sensor_process(char*msg);
 
 #endif // __SENSORS_H__

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;
+
+}

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__