forked from Archive/PX4-Autopilot
Initial non-tested code for reading from the ESC.
This commit is contained in:
parent
85b5da8078
commit
dadd8703b4
|
@ -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");
|
||||
|
|
|
@ -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;
|
||||
|
@ -112,7 +137,6 @@ build_eam_response(uint8_t *buffer, size_t *size)
|
|||
msg.speed_L = (uint8_t)speed & 0xff;
|
||||
msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
|
||||
|
||||
|
||||
msg.stop = STOP_BYTE;
|
||||
memcpy(buffer, &msg, *size);
|
||||
}
|
||||
|
|
|
@ -60,6 +60,34 @@
|
|||
#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
|
||||
|
@ -69,7 +97,7 @@ struct eam_module_msg {
|
|||
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. */
|
||||
|
@ -116,7 +144,7 @@ struct eam_module_msg {
|
|||
/**
|
||||
* The maximum buffer size required to store a HoTT message.
|
||||
*/
|
||||
#define MESSAGE_BUFFER_SIZE sizeof(union { \
|
||||
#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { \
|
||||
struct eam_module_msg eam; \
|
||||
})
|
||||
|
||||
|
@ -186,7 +214,11 @@ struct gps_module_msg {
|
|||
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);
|
||||
|
|
Loading…
Reference in New Issue