fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 22:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

allocate structs on the heap; exit daemon if no FrSky receiver detected

Browse Source
This commit is contained in:
Mark Whitehorn
2016-03-01 09:02:14 +01:00
committed by Lorenz Meier
parent 95d328f57c
commit 6d2927cd7a
3 changed files with 58 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/frsky_telemetry/frsky_telemetry.c
+22 -10
View File
@@ -67,8 +67,6 @@
static volatile bool thread_should_exit = false;
static volatile bool thread_running = false;
static int frsky_task;
static struct sensor_baro_s sensor_baro;
static float filtered_alt = NAN;
/* functions */
static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original);
@@ -176,7 +174,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
}
/* Open UART assuming SmartPort telemetry */
warnx("opening uart");
struct termios uart_config_original;
struct termios uart_config;
const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original);
@@ -201,21 +198,28 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
*/
int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000);
// warnx("poll status: %u", status);
if (status > 0) {
/* received some data; check size of packet */
usleep(5000);
status = read(uart, &sbuf[0], sizeof(sbuf));
// warnx("received %u bytes", status);
}
if (status > 0 && status < 3) {
/* traffic on the port, D type is 11 bytes per frame, SmartPort is only 2 */
/* Subscribe to topics */
sPort_init();
if (!sPort_init()) {
err(1, "could not allocate memory");
}
warnx("sending FrSky SmartPort telemetry");
struct sensor_baro_s *sensor_baro = malloc(sizeof(struct sensor_baro_s));
if (sensor_baro == NULL) {
err(1, "could not allocate memory");
}
static float filtered_alt = NAN;
int sensor_sub = orb_subscribe(ORB_ID(sensor_baro));
/* send S.port telemetry */
@@ -252,13 +256,13 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
orb_check(sensor_sub, &sensor_updated);
if (sensor_updated) {
orb_copy(ORB_ID(sensor_baro), sensor_sub, &sensor_baro);
orb_copy(ORB_ID(sensor_baro), sensor_sub, sensor_baro);
if (isnan(filtered_alt)) {
filtered_alt = sensor_baro.altitude;
filtered_alt = sensor_baro->altitude;
} else {
filtered_alt = .05f * sensor_baro.altitude + .95f * filtered_alt;
filtered_alt = .05f * sensor_baro->altitude + .95f * filtered_alt;
}
}
@@ -390,9 +394,14 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
}
}
} else if (status >= 0) {
warnx("freeing sPort memory");
sPort_deinit();
free(sensor_baro);
/* either no traffic on the port (0=>timeout), or D type packet */
} else if (status > 0) {
warnx("sending FrSky D-type telemetry");
/* detected D type telemetry: reconfigure UART */
status = set_uart_speed(uart, &uart_config, B9600);
@@ -453,6 +462,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
/* TODO: flush the input buffer if in full duplex mode */
read(uart, &sbuf[0], sizeof(sbuf));
} else {
warnx("FrSky receiver not detected, exiting");
}
/* Reset the UART flags to original state */
src/drivers/frsky_telemetry/sPort_data.c
+33 -21
View File
@@ -57,23 +57,35 @@
#define frac(f) (f - (int)f)
//static int battery_sub = -1;
static int sensor_sub = -1;
static int global_position_sub = -1;
static int vehicle_status_sub = -1;
static struct vehicle_status_s vehicle_status;
static struct sensor_combined_s sensor_data;
static struct vehicle_global_position_s global_pos;
static struct vehicle_status_s *vehicle_status;
static struct sensor_combined_s *sensor_data;
static struct vehicle_global_position_s *global_pos;
/**
* Initializes the uORB subscriptions.
*/
void sPort_init()
bool sPort_init()
{
vehicle_status = malloc(sizeof(struct vehicle_status_s));
sensor_data = malloc(sizeof(struct sensor_combined_s));
global_pos = malloc(sizeof(struct vehicle_global_position_s));
if (vehicle_status == NULL || vehicle_status == NULL || vehicle_status == NULL) {
return false;
}
global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
return true;
}
void sPort_deinit()
{
free(vehicle_status);
free(sensor_data);
free(global_pos);
}
static void update_crc(uint16_t *crc, unsigned char b)
@@ -145,10 +157,10 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data)
void sPort_send_BATV(int uart)
{
/* get a local copy of the vehicle status data */
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, vehicle_status);
/* send battery voltage as VFAS */
uint32_t voltage = (int)(100 * vehicle_status.battery_voltage);
uint32_t voltage = (int)(100 * vehicle_status->battery_voltage);
sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage);
}
@@ -156,7 +168,7 @@ void sPort_send_BATV(int uart)
void sPort_send_CUR(int uart)
{
/* send data */
uint32_t current = (int)(10 * vehicle_status.battery_current);
uint32_t current = (int)(10 * vehicle_status->battery_current);
sPort_send_data(uart, SMARTPORT_ID_CURR, current);
}
@@ -166,10 +178,10 @@ void sPort_send_CUR(int uart)
void sPort_send_ALT(int uart)
{
/* get a local copy of the current sensor values */
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor_data);
orb_copy(ORB_ID(sensor_combined), sensor_sub, sensor_data);
/* send data */
uint32_t alt = (int)(100 * sensor_data.baro_alt_meter[0]);
uint32_t alt = (int)(100 * sensor_data->baro_alt_meter[0]);
sPort_send_data(uart, SMARTPORT_ID_ALT, alt);
}
@@ -177,7 +189,7 @@ void sPort_send_ALT(int uart)
void sPort_send_SPD(int uart)
{
/* send data */
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
float speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e);
uint32_t ispeed = (int)(10 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}
@@ -194,7 +206,7 @@ void sPort_send_VSPD(int uart, float speed)
void sPort_send_FUEL(int uart)
{
/* send data */
uint32_t fuel = (int)(100 * vehicle_status.battery_remaining);
uint32_t fuel = (int)(100 * vehicle_status->battery_remaining);
sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel);
}
@@ -202,14 +214,14 @@ void sPort_send_GPS_LON(int uart)
{
/* send longitude */
/* get a local copy of the global position data */
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos);
orb_copy(ORB_ID(vehicle_global_position), global_position_sub, global_pos);
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
/* precision is approximately 0.1m */
uint32_t iLon = 6E5 * fabs(global_pos.lon);
uint32_t iLon = 6E5 * fabs(global_pos->lon);
iLon |= (1 << 31);
if (global_pos.lon < 0) { iLon |= (1 << 30); }
if (global_pos->lon < 0) { iLon |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon);
}
@@ -219,9 +231,9 @@ void sPort_send_GPS_LAT(int uart)
/* send latitude */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */
uint32_t iLat = 6E5 * fabs(global_pos.lat);
uint32_t iLat = 6E5 * fabs(global_pos->lat);
if (global_pos.lat < 0) { iLat |= (1 << 30); }
if (global_pos->lat < 0) { iLat |= (1 << 30); }
sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat);
}
@@ -231,7 +243,7 @@ void sPort_send_GPS_ALT(int uart)
/* send altitude */
/* convert to 100 * m/sec */
uint32_t iAlt = 100 * global_pos.alt;
uint32_t iAlt = 100 * global_pos->alt;
sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt);
}
@@ -240,14 +252,14 @@ void sPort_send_GPS_COG(int uart)
/* send course */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
uint32_t iYaw = 100 * global_pos.yaw;
uint32_t iYaw = 100 * global_pos->yaw;
sPort_send_data(uart, SMARTPORT_ID_GPS_CRS, iYaw);
}
void sPort_send_GPS_SPD(int uart)
{
/* send 100 * knots */
float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e);
float speed = sqrtf(global_pos->vel_n * global_pos->vel_n + global_pos->vel_e * global_pos->vel_e);
uint32_t ispeed = (int)(1944 * speed);
sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed);
}
src/drivers/frsky_telemetry/sPort_data.h
+3 -1
View File
@@ -43,6 +43,7 @@
#define _SPORT_DATA_H
#include <sys/types.h>
#include <stdbool.h>
/* FrSky SmartPort polling IDs captured from X4R */
#define SMARTPORT_POLL_1 0x1B
@@ -78,7 +79,8 @@
#define SMARTPORT_ID_GPS_TIME 0x0850
// Public functions
void sPort_init(void);
bool sPort_init(void);
void sPort_deinit(void);
void sPort_send_data(int uart, uint16_t id, uint32_t data);
void sPort_send_BATV(int uart);
void sPort_send_CUR(int uart);