From 86adaeb3e8f28c92005a38b7c71e12111efe8694 Mon Sep 17 00:00:00 2001
From: Simon Wilks <sjwilks@gmail.com>
Date: Sat, 6 Jul 2013 15:02:34 +0200
Subject: [PATCH] More cleanups
---
src/drivers/hott/comms.c | 9 ++++---
src/drivers/hott/comms.h | 4 +--
src/drivers/hott/hott_sensors/hott_sensors.c | 26 ++++++++----------
.../hott/hott_telemetry/hott_telemetry.c | 7 ++---
src/drivers/hott/messages.c | 27 +++++++++++++++++--
src/drivers/hott/messages.h | 5 ++--
6 files changed, 47 insertions(+), 31 deletions(-)
diff --git a/src/drivers/hott/comms.c b/src/drivers/hott/comms.c
index 4a7d3c8451..a2de87407d 100644
--- a/src/drivers/hott/comms.c
+++ b/src/drivers/hott/comms.c
@@ -41,17 +41,17 @@
#include <fcntl.h>
#include <nuttx/config.h>
-#include <poll.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/ioctl.h>
-#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
+#include <termios.h>
+#include <unistd.h>
int
-open_uart(const char *device, struct termios *uart_config_original)
+open_uart(const char *device)
{
/* baud rate */
static const speed_t speed = B19200;
@@ -65,7 +65,8 @@ open_uart(const char *device, struct termios *uart_config_original)
/* Back up the original uart configuration to restore it after exit */
int termios_state;
- if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
+ struct termios uart_config_original;
+ if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
close(uart);
err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
}
diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h
index a1173631dd..4954a309e2 100644
--- a/src/drivers/hott/comms.h
+++ b/src/drivers/hott/comms.h
@@ -41,9 +41,7 @@
#ifndef COMMS_H_
#define COMMS_H
-#include <termios.h>
-
-int open_uart(const char *device, struct termios *uart_config_original);
+int open_uart(const char *device);
#endif /* COMMS_H_ */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.c b/src/drivers/hott/hott_sensors/hott_sensors.c
index dc10685b47..41ca0c92f8 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.c
+++ b/src/drivers/hott/hott_sensors/hott_sensors.c
@@ -47,7 +47,6 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
-#include <termios.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <systemlib/err.h>
@@ -60,7 +59,7 @@ static int thread_should_exit = false; /**< Deamon exit flag */
static int thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */
static const char daemon_name[] = "hott_sensors";
-static const char commandline_usage[] = "usage: hott_sensor start|status|stop [-d <device>]";
+static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
/**
* Deamon management function.
@@ -96,8 +95,6 @@ send_poll(int uart, uint8_t *buffer, size_t size)
int
recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
{
- usleep(5000);
-
static const int timeout_ms = 1000;
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
@@ -108,7 +105,6 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
bool stop_byte_read = false;
while (true) {
read(uart, &buffer[i], sizeof(buffer[i]));
- //printf("[%d]: %d\n", i, buffer[i]);
if (stop_byte_read) {
// XXX process checksum
@@ -149,37 +145,37 @@ hott_sensors_thread_main(int argc, char *argv[])
}
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
- struct termios uart_config_original;
- const int uart = open_uart(device, &uart_config_original);
-
+ const int uart = open_uart(device);
if (uart < 0) {
errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false;
}
+ pub_messages_init();
+
uint8_t buffer[MESSAGE_BUFFER_SIZE];
size_t size = 0;
uint8_t id = 0;
- bool connected = true;
while (!thread_should_exit) {
// Currently we only support a General Air Module sensor.
build_gam_request(&buffer, &size);
send_poll(uart, buffer, size);
+
+ // The sensor will need a little time before it starts sending.
+ usleep(5000);
+
recv_data(uart, &buffer, &size, &id);
// Determine which moduel sent it and process accordingly.
if (id == GAM_SENSOR_ID) {
- //warnx("extract");
- extract_gam_message(buffer);
+ publish_gam_message(buffer);
} else {
- warnx("Unknown sensor ID received: %d", id);
- }
+ warnx("Unknown sensor ID: %d", id);
+ }
}
warnx("exiting");
-
close(uart);
-
thread_running = false;
return 0;
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.c b/src/drivers/hott/hott_telemetry/hott_telemetry.c
index fc80ac4d23..c87810b42f 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.c
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.c
@@ -49,7 +49,6 @@
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
-#include <termios.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <systemlib/err.h>
@@ -189,15 +188,13 @@ hott_telemetry_thread_main(int argc, char *argv[])
}
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
- struct termios uart_config_original;
- const int uart = open_uart(device, &uart_config_original);
-
+ const int uart = open_uart(device);
if (uart < 0) {
errx(1, "Failed opening HoTT UART, exiting.");
thread_running = false;
}
- messages_init();
+ sub_messages_init();
uint8_t buffer[MESSAGE_BUFFER_SIZE];
size_t size = 0;
diff --git a/src/drivers/hott/messages.c b/src/drivers/hott/messages.c
index 1a29b7e73d..ba2f6212da 100644
--- a/src/drivers/hott/messages.c
+++ b/src/drivers/hott/messages.c
@@ -58,19 +58,31 @@ static int gps_sub = -1;
static int home_sub = -1;
static int sensor_sub = -1;
static int airspeed_sub = -1;
+static int esc_sub = -1;
+
+//orb_advert_t _esc_pub;
+//struct esc_s _esc;
+
static bool home_position_set = false;
static double home_lat = 0.0d;
static double home_lon = 0.0d;
void
-messages_init(void)
+sub_messages_init(void)
{
battery_sub = orb_subscribe(ORB_ID(battery_status));
gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
home_sub = orb_subscribe(ORB_ID(home_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+ //esc_sub = orb_subscribe(ORB_ID(esc));
+}
+
+void
+pub_messages_init(void)
+{
+ //esc_pub = orb_subscribe(ORB_ID(esc));
}
void
@@ -87,17 +99,28 @@ build_gam_request(uint8_t *buffer, size_t *size)
}
void
-extract_gam_message(const uint8_t *buffer)
+publish_gam_message(const uint8_t *buffer)
{
struct gam_module_msg msg;
size_t size = sizeof(msg);
memset(&msg, 0, size);
memcpy(&msg, buffer, size);
+ /* announce the esc if needed, just publish else */
+// if (esc_pub > 0) {
+// orb_publish(ORB_ID(airspeed), _esc_pub, &_esc);
+//
+// } else {
+// _esc_pub = orb_advertise(ORB_ID(esc), &_esc);
+// }
+
// Publish it.
uint16_t rpm = ((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
+ //_esc.rpm = rpm;
uint8_t temp = msg.temperature2 + 20;
+ //_esc.temperature = temp;
float current = ((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1f;
+ //_esc.current = current;
printf("RPM: %d TEMP: %d A: %2.1f\n", rpm, temp, current);
}
diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h
index ecfad8569e..dce90f273c 100644
--- a/src/drivers/hott/messages.h
+++ b/src/drivers/hott/messages.h
@@ -256,9 +256,10 @@ struct gps_module_msg {
#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE
-void messages_init(void);
+void sub_messages_init(void);
+void pub_messages_init(void);
void build_gam_request(uint8_t *buffer, size_t *size);
-void extract_gam_message(const uint8_t *buffer);
+void publish_gam_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);
--
GitLab