From dadd8703b422523d88b02effe48e76152bcb2fce Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 28 Jun 2013 08:42:05 +0200 Subject: Initial non-tested code for reading from the ESC. --- src/drivers/hott_telemetry/hott_telemetry_main.c | 44 +++++++++++++++++ src/drivers/hott_telemetry/messages.c | 28 ++++++++++- src/drivers/hott_telemetry/messages.h | 62 ++++++++++++++++++------ 3 files changed, 117 insertions(+), 17 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index 1d2bdd92e..2c954e41e 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -149,6 +149,29 @@ recv_req_id(int uart, uint8_t *id) return OK; } +int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t id) +{ + usleep(100000); + + static const int timeout_ms = 200; + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + int i = 0; + if (poll(fds, 1, timeout_ms) > 0) { + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + + if (&buffer[i] == STOP_BYTE) { + *size = ++i; + id = &buffer[1]; + return OK; + } + } + } + return ERROR; +} + int send_data(int uart, uint8_t *buffer, size_t size) { @@ -218,6 +241,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) bool connected = true; while (!thread_should_exit) { + // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { if (!connected) { connected = true; @@ -242,6 +266,26 @@ hott_telemetry_thread_main(int argc, char *argv[]) connected = false; warnx("syncing"); } + + // Poll the next HoTT devices. + // TODO(sjwilks): Currently there is only one but if there would be more we would round-robin + // calling one for every loop iteration. + build_esc_request(&buffer, &size); + send_data(uart, buffer, size); + + // Listen for a response. + recv_data(uart, &buffer, &size, &id); + + for (size_t i = 0; i < size; i++) { + warnx("%d", buffer[i]); + } + + // Determine which moduel sent it and process accordingly. + if (id == ESC_SENSOR_ID) { + extract_esc_message(buffer); + } else { + warnx("Unknown sensor ID received: %d", id); + } } warnx("exiting"); diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index d2634ef41..149c4d367 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -73,6 +73,31 @@ messages_init(void) airspeed_sub = orb_subscribe(ORB_ID(airspeed)); } +void +build_esc_request(uint8_t *buffer, size_t *size) +{ + struct esc_module_poll_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.mode = BINARY_MODE_REQUEST_ID; + msg.id = ESC_SENSOR_ID; + + memcpy(&msg, buffer, size); +} + +void +extract_esc_message(const uint8_t *buffer) +{ + struct esc_module_msg msg; + size_t size = sizeof(msg); + memset(&msg, 0, size); + memcpy(buffer, &msg, size); + + // Publish it. + +} + void build_eam_response(uint8_t *buffer, size_t *size) { @@ -92,7 +117,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.eam_sensor_id = EAM_SENSOR_ID; - msg.sensor_id = EAM_SENSOR_TEXT_ID; + msg.sensor_text_id = EAM_SENSOR_TEXT_ID; msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; @@ -111,7 +136,6 @@ build_eam_response(uint8_t *buffer, size_t *size) uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); msg.speed_L = (uint8_t)speed & 0xff; msg.speed_H = (uint8_t)(speed >> 8) & 0xff; - msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h index e6d5cc723..e1a4262a1 100644 --- a/src/drivers/hott_telemetry/messages.h +++ b/src/drivers/hott_telemetry/messages.h @@ -60,19 +60,47 @@ #define STOP_BYTE 0x7d #define TEMP_ZERO_CELSIUS 0x14 +#define ESC_SENSOR_ID 0x8e + +/* The ESC Module poll message. */ +struct esc_module_poll_msg { + uint8_t mode; + uint8_t id; +}; + +/* The Electric Air Module message. */ +struct esc_module_msg { + uint8_t start; /**< Start byte */ + uint8_t sensor_id; + uint8_t warning; + uint8_t sensor_text_id; + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; + uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ + uint8_t temperature2; + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_H; + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_H; + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_H; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ +}; + /* Electric Air Module (EAM) constants. */ #define EAM_SENSOR_ID 0x8e #define EAM_SENSOR_TEXT_ID 0xe0 /* The Electric Air Module message. */ struct eam_module_msg { - uint8_t start; /**< Start byte */ + uint8_t start; /**< Start byte */ uint8_t eam_sensor_id; /**< EAM sensor */ uint8_t warning; - uint8_t sensor_id; /**< Sensor ID, why different? */ + uint8_t sensor_text_id; uint8_t alarm_inverse1; uint8_t alarm_inverse2; - uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ + uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ uint8_t cell2_L; uint8_t cell3_L; uint8_t cell4_L; @@ -92,9 +120,9 @@ struct eam_module_msg { uint8_t batt2_voltage_H; uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ uint8_t temperature2; - uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ + uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ uint8_t altitude_H; - uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ uint8_t current_H; uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ uint8_t main_voltage_H; @@ -103,21 +131,21 @@ struct eam_module_msg { uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ uint8_t climbrate_H; uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ - uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ uint8_t rpm_H; uint8_t electric_min; /**< Flight time in minutes. */ uint8_t electric_sec; /**< Flight time in seconds. */ - uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ uint8_t speed_H; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ }; /** * The maximum buffer size required to store a HoTT message. */ -#define MESSAGE_BUFFER_SIZE sizeof(union { \ - struct eam_module_msg eam; \ +#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { \ + struct eam_module_msg eam; \ }) /* GPS sensor constants. */ @@ -129,9 +157,9 @@ struct eam_module_msg { * Struct based on: https://code.google.com/p/diy-hott-gps/downloads */ struct gps_module_msg { - uint8_t start; /**< Start byte */ - uint8_t sensor_id; /**< GPS sensor ID*/ - uint8_t warning; /**< Byte 3: 0…= warning beeps */ + uint8_t start; /**< Start byte */ + uint8_t sensor_id; /**< GPS sensor ID*/ + uint8_t warning; /**< Byte 3: 0…= warning beeps */ uint8_t sensor_text_id; /**< GPS Sensor text mode ID */ uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */ uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */ @@ -183,10 +211,14 @@ struct gps_module_msg { * The maximum buffer size required to store a HoTT message. */ #define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct gps_module_msg gps; \ + struct gps_module_msg gps; \ }) +#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE + void messages_init(void); +void build_esc_request(uint8_t *buffer, size_t *size); +void extract_esc_message(const uint8_t *buffer); void build_eam_response(uint8_t *buffer, size_t *size); void build_gps_response(uint8_t *buffer, size_t *size); float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -- cgit v1.2.3