From e0987aa83ab065a7e0eb47ad18e396e763db474a Mon Sep 17 00:00:00 2001 From: Angel Garcia Date: Thu, 23 May 2019 03:36:18 +0200 Subject: [PATCH] Added sensor query support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Añadidos sensores de temperatura del propio dispositivo --- hover-controller/Makefile | 11 +++-- hover-controller/dispatcher.cpp | 2 +- hover-controller/sensors.cpp | 51 ++++++++++++++++++++++++ hover-controller/sensors.h | 4 ++ hover-controller/sensors/temperature.cpp | 38 ++++++++++++++++++ hover-controller/sensors/temperature.h | 13 ++++++ 6 files changed, 115 insertions(+), 4 deletions(-) create mode 100644 hover-controller/sensors/temperature.cpp create mode 100644 hover-controller/sensors/temperature.h diff --git a/hover-controller/Makefile b/hover-controller/Makefile index 30aad71..c41cf3d 100644 --- a/hover-controller/Makefile +++ b/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 diff --git a/hover-controller/dispatcher.cpp b/hover-controller/dispatcher.cpp index 744db85..30f8038 100644 --- a/hover-controller/dispatcher.cpp +++ b/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"); diff --git a/hover-controller/sensors.cpp b/hover-controller/sensors.cpp index e69de29..90697cb 100644 --- a/hover-controller/sensors.cpp +++ b/hover-controller/sensors.cpp @@ -0,0 +1,51 @@ +#include + +#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; + } +} diff --git a/hover-controller/sensors.h b/hover-controller/sensors.h index f327ff9..636a8a0 100644 --- a/hover-controller/sensors.h +++ b/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__ diff --git a/hover-controller/sensors/temperature.cpp b/hover-controller/sensors/temperature.cpp new file mode 100644 index 0000000..2ac976d --- /dev/null +++ b/hover-controller/sensors/temperature.cpp @@ -0,0 +1,38 @@ +#include "temperature.h" + +#include +//#include + +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; + +} diff --git a/hover-controller/sensors/temperature.h b/hover-controller/sensors/temperature.h new file mode 100644 index 0000000..7807d4a --- /dev/null +++ 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__