dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Major refactor of HoTT drivers and finished sensor read implementation.

This commit is contained in:
Simon Wilks 2013-07-13 01:08:06 +02:00
parent a18c6cea18
commit b500cce31e
8 changed files with 153 additions and 109 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
src/drivers/hott/comms.c → src/drivers/hott/comms.cpp
View File

38
src/drivers/hott/hott_sensors/hott_sensors.c → src/drivers/hott/hott_sensors/hott_sensors.cpp
View File

@ -55,6 +55,14 @@
#include "../comms.h"
#include "../messages.h"
#define DEFAULT_UART "/dev/ttyS2"; /**< USART5 */
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
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 */
@ -64,7 +72,7 @@ static const char commandline_usage[] = "usage: hott_sensors start|status|stop [
/**
* Deamon management function.
*/
__EXPORT int hott_sensors_main(int argc, char *argv[]);
extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
@ -96,11 +104,13 @@ int
recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
{
static const int timeout_ms = 1000;
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
struct pollfd fds;
fds.fd = uart;
fds.events = POLLIN;
// XXX should this poll be inside the while loop???
if (poll(fds, 1, timeout_ms) > 0) {
if (poll(&fds, 1, timeout_ms) > 0) {
int i = 0;
bool stop_byte_read = false;
while (true) {
@ -129,7 +139,7 @@ hott_sensors_thread_main(int argc, char *argv[])
thread_running = true;
const char *device = "/dev/ttyS2"; /**< Default telemetry port: USART5 */
const char *device = DEFAULT_UART;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
@ -151,20 +161,20 @@ hott_sensors_thread_main(int argc, char *argv[])
thread_running = false;
}
pub_messages_init();
init_pub_messages();
uint8_t buffer[MESSAGE_BUFFER_SIZE];
uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
size_t size = 0;
uint8_t id = 0;
while (!thread_should_exit) {
// Currently we only support a General Air Module sensor.
build_gam_request(&buffer, &size);
build_gam_request(&buffer[0], &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);
recv_data(uart, &buffer[0], &size, &id);
// Determine which moduel sent it and process accordingly.
if (id == GAM_SENSOR_ID) {
@ -199,12 +209,12 @@ hott_sensors_main(int argc, char *argv[])
}
thread_should_exit = false;
deamon_task = task_spawn(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
2048,
hott_sensors_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
deamon_task = task_spawn_cmd(daemon_name,
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 40,
2048,
hott_sensors_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
}

6
src/drivers/hott/hott_sensors/module.mk
View File

@ -37,6 +37,6 @@
MODULE_COMMAND = hott_sensors
SRCS = hott_sensors.c \
../messages.c \
../comms.c
SRCS = hott_sensors.cpp \
../messages.cpp \
../comms.cpp

33
src/drivers/hott/hott_telemetry/hott_telemetry.c → src/drivers/hott/hott_telemetry/hott_telemetry.cpp
View File

@ -57,6 +57,14 @@
#include "../comms.h"
#include "../messages.h"
#define DEFAULT_UART "/dev/ttyS1"; /**< USART2 */
/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
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 */
@ -66,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop
/**
* Deamon management function.
*/
__EXPORT int hott_telemetry_main(int argc, char *argv[]);
extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
/**
* Mainloop of daemon.
@ -80,11 +88,14 @@ int
recv_req_id(int uart, uint8_t *id)
{
static const int timeout_ms = 1000; // TODO make it a define
struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
uint8_t mode;
if (poll(fds, 1, timeout_ms) > 0) {
struct pollfd fds;
fds.fd = uart;
fds.events = POLLIN;
if (poll(&fds, 1, timeout_ms) > 0) {
/* Get the mode: binary or text */
read(uart, &mode, sizeof(mode));
@ -109,11 +120,13 @@ 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 } };
struct pollfd fds;
fds.fd = uart;
fds.events = POLLIN;
// XXX should this poll be inside the while loop???
if (poll(fds, 1, timeout_ms) > 0) {
if (poll(&fds, 1, timeout_ms) > 0) {
int i = 0;
bool stop_byte_read = false;
while (true) {
@ -172,7 +185,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
thread_running = true;
const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */
const char *device = DEFAULT_UART;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
@ -194,9 +207,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
thread_running = false;
}
sub_messages_init();
init_sub_messages();
uint8_t buffer[MESSAGE_BUFFER_SIZE];
uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
size_t size = 0;
uint8_t id = 0;
bool connected = true;
@ -212,7 +225,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
case EAM_SENSOR_ID:
build_eam_response(buffer, &size);
break;
case GAM_SENSOR_ID:
build_gam_response(buffer, &size);
break;
case GPS_SENSOR_ID:
build_gps_response(buffer, &size);
break;

6
src/drivers/hott/hott_telemetry/module.mk
View File

@ -37,6 +37,6 @@
MODULE_COMMAND = hott_telemetry
SRCS = hott_telemetry.c \
../messages.c \
../comms.c
SRCS = hott_telemetry.cpp \
../messages.cpp \
../comms.cpp

140
src/drivers/hott/messages.c → src/drivers/hott/messages.cpp
View File

@ -46,6 +46,7 @@
#include <unistd.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
@ -53,36 +54,37 @@
/* The board is very roughly 5 deg warmer than the surrounding air */
#define BOARD_TEMP_OFFSET_DEG 5
static int battery_sub = -1;
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;
static int _battery_sub = -1;
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;
orb_advert_t _esc_pub;
struct esc_status_s _esc;
static bool home_position_set = false;
static double home_lat = 0.0d;
static double home_lon = 0.0d;
static bool _home_position_set = false;
static double _home_lat = 0.0d;
static double _home_lon = 0.0d;
void
sub_messages_init(void)
init_sub_messages(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));
_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_status));
}
void
pub_messages_init(void)
init_pub_messages(void)
{
//esc_pub = orb_subscribe(ORB_ID(esc));
memset(&_esc, 0, sizeof(_esc));
_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
}
void
@ -107,21 +109,22 @@ publish_gam_message(const uint8_t *buffer)
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);
// }
if (_esc_pub > 0) {
orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
} else {
_esc_pub = orb_advertise(ORB_ID(esc_status), &_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);
_esc.esc_count = 1;
_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
_esc.esc[0].esc_temperature = msg.temperature1 - 20;
_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
//printf("T: %d\n", _esc.esc[0].esc_temperature);
}
void
@ -130,12 +133,12 @@ build_eam_response(uint8_t *buffer, size_t *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);
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
/* get a local copy of the battery data */
struct battery_status_s battery;
memset(&battery, 0, sizeof(battery));
orb_copy(ORB_ID(battery_status), battery_sub, &battery);
orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
struct eam_module_msg msg;
*size = sizeof(msg);
@ -145,7 +148,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
msg.eam_sensor_id = EAM_SENSOR_ID;
msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius - 20);
msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10);
@ -157,7 +160,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
/* get a local copy of the airspeed data */
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
msg.speed_L = (uint8_t)speed & 0xff;
@ -167,20 +170,55 @@ build_eam_response(uint8_t *buffer, size_t *size)
memcpy(buffer, &msg, *size);
}
void
build_gam_response(uint8_t *buffer, size_t *size)
{
/* get a local copy of the ESC Status values */
struct esc_status_s esc;
memset(&esc, 0, sizeof(esc));
orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
struct gam_module_msg msg;
*size = sizeof(msg);
memset(&msg, 0, *size);
msg.start = START_BYTE;
msg.gam_sensor_id = GAM_SENSOR_ID;
msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
msg.temperature2 = 20; // 0 deg. C.
uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
msg.main_voltage_L = (uint8_t)voltage & 0xff;
msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
uint16_t current = (uint16_t)(esc.esc[0].esc_current);
msg.current_L = (uint8_t)current & 0xff;
msg.current_H = (uint8_t)(current >> 8) & 0xff;
uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
msg.rpm_L = (uint8_t)rpm & 0xff;
msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
msg.stop = STOP_BYTE;
memcpy(buffer, &msg, *size);
}
void
build_gps_response(uint8_t *buffer, size_t *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);
orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
/* get a local copy of the battery data */
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
struct gps_module_msg msg = { 0 };
struct gps_module_msg msg;
*size = sizeof(msg);
memset(&msg, 0, *size);
@ -200,7 +238,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
/* GPS speed */
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
msg.gps_speed_L = (uint8_t)speed & 0xff;
msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
@ -246,33 +284,33 @@ build_gps_response(uint8_t *buffer, size_t *size)
msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
/* Altitude */
uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f);
msg.altitude_L = (uint8_t)alt & 0xff;
msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
/* Get any (and probably only ever one) home_sub postion report */
/* Get any (and probably only ever one) _home_sub postion report */
bool updated;
orb_check(home_sub, &updated);
orb_check(_home_sub, &updated);
if (updated) {
/* get a local copy of the home position data */
struct home_position_s home;
memset(&home, 0, sizeof(home));
orb_copy(ORB_ID(home_position), home_sub, &home);
orb_copy(ORB_ID(home_position), _home_sub, &home);
home_lat = ((double)(home.lat))*1e-7d;
home_lon = ((double)(home.lon))*1e-7d;
home_position_set = true;
_home_lat = ((double)(home.lat))*1e-7d;
_home_lon = ((double)(home.lon))*1e-7d;
_home_position_set = true;
}
/* Distance from home */
if (home_position_set) {
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
if (_home_position_set) {
uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
msg.distance_L = (uint8_t)dist & 0xff;
msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
/* Direction back to home */
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
msg.home_direction = (uint8_t)bearing >> 1;
}
}

34
src/drivers/hott/messages.h
View File

@ -119,23 +119,16 @@ struct eam_module_msg {
uint8_t checksum; /**< Lower 8-bits of all bytes summed. */
};
/**
* The maximum buffer size required to store an Electric Air Module message.
*/
#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { \
struct eam_module_msg eam; \
})
/* General Air Module (GAM) constants. */
#define GAM_SENSOR_ID 0x8d
#define GAM_SENSOR_TEXT_ID 0xd0
struct gam_module_msg {
uint8_t start_byte; // start byte constant value 0x7c
uint8_t start; // start byte constant value 0x7c
uint8_t gam_sensor_id; // EAM sensort id. constat value 0x8d
uint8_t warning_beeps; // 1=A 2=B ... 0x1a=Z 0 = no alarm
uint8_t sensor_id; // constant value 0xd0
uint8_t sensor_text_id; // constant value 0xd0
uint8_t alarm_invers1; // alarm bitmask. Value is displayed inverted
uint8_t alarm_invers2; // alarm bitmask. Value is displayed inverted
uint8_t cell1; // cell 1 voltage lower value. 0.02V steps, 124=2.48V
@ -180,14 +173,6 @@ struct gam_module_msg {
uint8_t checksum; // checksum
};
/**
* The maximum buffer size required to store a General Air Module message.
*/
#define GAM_MESSAGE_BUFFER_SIZE sizeof(union { \
struct gam_module_msg gam; \
})
/* GPS sensor constants. */
#define GPS_SENSOR_ID 0x8a
#define GPS_SENSOR_TEXT_ID 0xa0
@ -247,20 +232,15 @@ struct gps_module_msg {
uint8_t checksum; /**< Byte 45: Parity Byte */
};
/**
* The maximum buffer size required to store a HoTT message.
*/
#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \
struct gps_module_msg gps; \
})
// The maximum size of a message.
#define MAX_MESSAGE_BUFFER_SIZE 45
#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE
void sub_messages_init(void);
void pub_messages_init(void);
void init_sub_messages(void);
void init_pub_messages(void);
void build_gam_request(uint8_t *buffer, size_t *size);
void publish_gam_message(const uint8_t *buffer);
void build_eam_response(uint8_t *buffer, size_t *size);
void build_gam_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);
void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);

3
src/modules/uORB/topics/esc_status.h
View File

@ -63,7 +63,8 @@
enum ESC_VENDOR {
ESC_VENDOR_GENERIC = 0, /**< generic ESC */
ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */
ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */
ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */
};
enum ESC_CONNECTION_TYPE {