diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c index 00b8f8d665..ba08aa3c5e 100644 --- a/apps/hott_telemetry/hott_telemetry_main.c +++ b/apps/hott_telemetry/hott_telemetry_main.c @@ -82,8 +82,8 @@ __EXPORT int hott_telemetry_main(int argc, char *argv[]); */ int hott_telemetry_thread_main(int argc, char *argv[]); -static int read_data(int uart); -static int send_data(int uart, const struct eam_module_msg *msg); +static int read_data(int uart, int *id); +static int send_data(int uart, char *buffer, int size); static void uart_disable_rx(void); static void uart_disable_tx(void); static uint32_t get_uart_address(const char *device); @@ -135,7 +135,7 @@ static int open_uart(const char *uart_name, struct termios *uart_config_original return uart; } -int read_data(int uart) +int read_data(int uart, int *id) { uart_disable_tx(); @@ -148,12 +148,11 @@ int read_data(int uart) read(uart, &mode, 1); /* read the poll ID (device ID being targetted) */ - char id; - read(uart, &id, 1); + read(uart, id, 1); - /* if we have a binary mode request for our sensor ID let's run with it. */ - if (mode != BINARY_MODE_REQUEST_ID || id != ELECTRIC_AIR_MODULE) { - return ERROR; // not really an error, rather uninteresting. + /* if we have a binary mode request */ + if (mode != BINARY_MODE_REQUEST_ID) { + return ERROR; } } else { printf("Timeout\n"); @@ -162,18 +161,13 @@ int read_data(int uart) return OK; } -int send_data(int uart, const struct eam_module_msg *msg) +int send_data(int uart, char *buffer, int size) { usleep(POST_READ_DELAY_IN_USECS); uart_disable_rx(); uint16_t checksum = 0; - int size = sizeof(*msg); - char buffer[size]; - - memcpy(buffer, msg, size); - for(int i = 0; i < size; i++) { if (i == size - 1) { /* Set the checksum: the first uint8_t is taken as the checksum. */ @@ -260,11 +254,19 @@ int hott_telemetry_thread_main(int argc, char *argv[]) messages_init(); - struct eam_module_msg msg; + char *buffer; + int size = 0; + int id = 0; while (!thread_should_exit) { - build_eam_response(&msg); - if (read_data(uart) == OK) { - send_data(uart, &msg); + if (read_data(uart, &id) == OK) { + switch(id) { + case ELECTRIC_AIR_MODULE: + build_eam_response(&buffer, &size); + break; + default: + continue; // Not a module we support. + } + send_data(uart, buffer, size); } } diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c index eee78a01e8..1ce103b125 100644 --- a/apps/hott_telemetry/messages.c +++ b/apps/hott_telemetry/messages.c @@ -51,25 +51,32 @@ void messages_init(void) sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); } -void build_eam_response(struct eam_module_msg *msg) +void build_eam_response(char **buffer, int *size) { /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - memset(msg, 0, sizeof(*msg)); + struct eam_module_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); - msg->start = START_BYTE; - msg->eam_sensor_id = ELECTRIC_AIR_MODULE; - msg->sensor_id = EAM_SENSOR_ID; - msg->temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); - msg->temperature2 = TEMP_ZERO_CELSIUS; - msg->main_voltage_L = (uint8_t)(raw.battery_voltage_v * 10); + msg.start = START_BYTE; + msg.eam_sensor_id = ELECTRIC_AIR_MODULE; + msg.sensor_id = EAM_SENSOR_ID; + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); + msg.temperature2 = TEMP_ZERO_CELSIUS; + msg.main_voltage_L = (uint8_t)(raw.battery_voltage_v * 10); uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); - msg->altitude_L = (uint8_t)alt & 0xff; - msg->altitude_H = (uint8_t)(alt >> 8) & 0xff; + msg.altitude_L = (uint8_t)alt & 0xff; + msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - msg->stop = STOP_BYTE; + msg.stop = STOP_BYTE; + + //*chunk = malloc( sizeof(char) * length); + //char tmp_buffer[*size]; + *buffer = malloc(sizeof(char) * *size); + memcpy(*buffer, &msg, *size); } \ No newline at end of file diff --git a/apps/hott_telemetry/messages.h b/apps/hott_telemetry/messages.h index fb305cd8d2..51217a23b9 100644 --- a/apps/hott_telemetry/messages.h +++ b/apps/hott_telemetry/messages.h @@ -35,7 +35,7 @@ /** * @file messages.h * - * Graupner HoTT Telemetry message generator. + * Graupner HoTT Telemetry message generation. * */ #ifndef MESSAGES_H_ @@ -109,6 +109,6 @@ struct eam_module_msg { }; void messages_init(void); -void build_eam_response(struct eam_module_msg *msg); +void build_eam_response(char **buffer, int *size); #endif /* MESSAGES_H_ */