From 5f18ce506d1a8395e1565db941b7af64a5936f31 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 24 Nov 2013 13:39:02 +0100 Subject: Add FrSky telemetry application. This daemon emulates an FrSky sensor hub by periodically sending data packets to an attached FrSky receiver. --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/frsky_telemetry/frsky_data.c | 223 +++++++++++++++++++++ src/drivers/frsky_telemetry/frsky_data.h | 84 ++++++++ src/drivers/frsky_telemetry/frsky_telemetry.c | 269 ++++++++++++++++++++++++++ src/drivers/frsky_telemetry/module.mk | 41 ++++ 6 files changed, 619 insertions(+) create mode 100644 src/drivers/frsky_telemetry/frsky_data.c create mode 100644 src/drivers/frsky_telemetry/frsky_data.h create mode 100644 src/drivers/frsky_telemetry/frsky_telemetry.c create mode 100644 src/drivers/frsky_telemetry/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 306827086..7e53de0ae 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -37,6 +37,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d..c5190375f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -35,6 +35,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # Needs to be burned to the ground and re-written; for now, diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c new file mode 100644 index 000000000..8a57070b1 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_data.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ + +#include "frsky_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; + + +/** + * Initializes the uORB subscriptions. + */ +void frsky_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); +} + +/** + * Sends a 0x5E start/stop byte. + */ +static void +frsky_send_startstop(int uart) +{ + static const uint8_t c = 0x5E; + write(uart, &c, sizeof(c)); +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void +frsky_send_byte(int uart, uint8_t value) +{ + const uint8_t x5E[] = {0x5D, 0x3E}; + const uint8_t x5D[] = {0x5D, 0x3D}; + + switch (value) + { + case 0x5E: + write(uart, x5E, sizeof(x5E)); + break; + + case 0x5D: + write(uart, x5D, sizeof(x5D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +static void +frsky_send_data(int uart, uint8_t id, uint16_t data) +{ + frsky_send_startstop(uart); + + frsky_send_byte(uart, id); + frsky_send_byte(uart, data); /* Low */ + frsky_send_byte(uart, data >> 8); /* High */ +} + +/** + * Sends frame 1 (every 200ms): + * acceleration values, altitude (vario), temperatures, current & voltages, RPM + */ +void frsky_send_frame1(int uart) +{ + /* 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); + + /* 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); + + /* send formatted frame */ + // TODO + frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius); + frsky_send_data(uart, FRSKY_ID_TEMP2, 0); + + frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ + frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); + + frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v); + frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_RPM, 0); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 2 (every 1000ms): + * course, latitude, longitude, speed, altitude (GPS), fuel level + */ +void frsky_send_frame2(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + // TODO + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + if (global_pos.valid) + { + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + // TODO: latitude, longitude + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + alt = global_pos.alt; + } + + frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, (speed - (int)speed) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, (alt - (int)alt) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_FUEL, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * date, time + */ +void frsky_send_frame3(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + // TODO + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + + frsky_send_startstop(uart); +} diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h new file mode 100644 index 000000000..e0c39a42b --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_data.h + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H + +// FrSky sensor hub data IDs +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +// Public functions +void frsky_init(void); +void frsky_send_frame1(int uart); +void frsky_send_frame2(int uart); +void frsky_send_frame3(int uart); + +#endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c new file mode 100644 index 000000000..5f3837b6e --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_telemetry.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + * This daemon emulates an FrSky sensor hub by periodically sending data + * packets to an attached FrSky receiver. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "frsky_data.h" + + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int frsky_task; + +/* functions */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static void usage(void); +static int frsky_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int frsky_telemetry_main(int argc, char *argv[]); + + +/** + * Opens the UART device and sets all required serial parameters. + */ +static int +frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +{ + /* Open UART */ + const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", uart_name); + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Disable output post-processing */ + uart_config.c_oflag &= ~OPOST; + + /* Set baud rate */ + static const speed_t speed = B9600; + + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + return uart; +} + +/** + * Print command usage information + */ +static void +usage() +{ + fprintf(stderr, "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); + exit(1); +} + +/** + * The daemon thread. + */ +static int +frsky_telemetry_thread_main(int argc, char *argv[]) +{ + /* Default values for arguments */ + char *device_name = "/dev/ttyS1"; /* USART2 */ + + /* Work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + case 'd': + device_name = optarg; + break; + + default: + usage(); + break; + } + } + + /* Print welcome text */ + warnx("FrSky telemetry interface starting..."); + + /* Open UART */ + struct termios uart_config_original; + const int uart = frsky_open_uart(device_name, &uart_config_original); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Subscribe to topics */ + frsky_init(); + + thread_running = true; + + /* Main thread loop */ + unsigned int iteration = 0; + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) + { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) + { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int +frsky_telemetry_main(int argc, char *argv[]) +{ + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "frsky_telemetry already running"); + + thread_should_exit = false; + frsky_task = task_spawn_cmd("frsky_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + frsky_telemetry_thread_main, + (const char **)argv); + + while (!thread_running) { + usleep(200); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "frsky_telemetry already stopped"); + + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk new file mode 100644 index 000000000..808966691 --- /dev/null +++ b/src/drivers/frsky_telemetry/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# FrSky telemetry application. +# + +MODULE_COMMAND = frsky_telemetry + +SRCS = frsky_data.c \ + frsky_telemetry.c -- cgit v1.2.3 From 50cbd1949971241a0b6108708b2b3fefe0c498e3 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 30 Dec 2013 20:27:04 +0100 Subject: Fixes to FrSky telemetry data formats. --- src/drivers/frsky_telemetry/frsky_data.c | 137 +++++++++++++++++++++++-------- src/drivers/frsky_telemetry/frsky_data.h | 33 -------- 2 files changed, 105 insertions(+), 65 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 8a57070b1..3a0c4b4b3 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -51,11 +51,52 @@ #include #include #include +#include + + +// FrSky sensor hub data IDs +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + + +#define frac(f) (f - (int)f) + +float frsky_format_gps(float dec); static int battery_sub = -1; static int sensor_sub = -1; static int global_position_sub = -1; +static int vehicle_status_sub = -1; /** @@ -66,6 +107,7 @@ void frsky_init() battery_sub = orb_subscribe(ORB_ID(battery_status)); 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)); } /** @@ -107,13 +149,16 @@ frsky_send_byte(int uart, uint8_t value) * Sends one data id/value pair. */ static void -frsky_send_data(int uart, uint8_t id, uint16_t data) +frsky_send_data(int uart, uint8_t id, int16_t data) { + // Cast data to unsigned, because signed shift might behave incorrectly + uint16_t udata = data; + frsky_send_startstop(uart); frsky_send_byte(uart, id); - frsky_send_byte(uart, data); /* Low */ - frsky_send_byte(uart, data >> 8); /* High */ + frsky_send_byte(uart, udata); /* LSB */ + frsky_send_byte(uart, udata >> 8); /* MSB */ } /** @@ -133,70 +178,96 @@ void frsky_send_frame1(int uart) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* send formatted frame */ - // TODO - frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); - frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius); + frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); frsky_send_data(uart, FRSKY_ID_TEMP2, 0); frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); - frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v); - frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f); + float voltage = battery.voltage_v * 11.0f / 21.0f; + frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage); + frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f); frsky_send_data(uart, FRSKY_ID_RPM, 0); frsky_send_startstop(uart); } +/** + * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + */ +float frsky_format_gps(float dec) +{ + float dms_deg = (int)dec; + float dec_deg = dec - dms_deg; + float dms_min = (int)(dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; + + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); +} + /** * Sends frame 2 (every 1000ms): * course, latitude, longitude, speed, altitude (GPS), fuel level */ void frsky_send_frame2(int uart) { - /* get a local copy of the battery data */ + /* get a local copy of the global position data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + /* send formatted frame */ - // TODO float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + char lat_ns = 0, lon_ew = 0; if (global_pos.valid) { + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - // TODO: latitude, longitude - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - alt = global_pos.alt; + lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; } frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); - frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); - frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, (speed - (int)speed) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); - frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, (alt - (int)alt) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); - frsky_send_data(uart, FRSKY_ID_FUEL, 0); + frsky_send_data(uart, FRSKY_ID_FUEL, vehicle_status.battery_remaining); - frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); frsky_send_startstop(uart); } @@ -213,11 +284,13 @@ void frsky_send_frame3(int uart) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* send formatted frame */ - // TODO - frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0); - frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0); - frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0); - frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); frsky_send_startstop(uart); } diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index e0c39a42b..cea93ef2b 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -42,39 +42,6 @@ #ifndef _FRSKY_DATA_H #define _FRSKY_DATA_H -// FrSky sensor hub data IDs -#define FRSKY_ID_GPS_ALT_BP 0x01 -#define FRSKY_ID_TEMP1 0x02 -#define FRSKY_ID_RPM 0x03 -#define FRSKY_ID_FUEL 0x04 -#define FRSKY_ID_TEMP2 0x05 -#define FRSKY_ID_VOLTS 0x06 -#define FRSKY_ID_GPS_ALT_AP 0x09 -#define FRSKY_ID_BARO_ALT_BP 0x10 -#define FRSKY_ID_GPS_SPEED_BP 0x11 -#define FRSKY_ID_GPS_LONG_BP 0x12 -#define FRSKY_ID_GPS_LAT_BP 0x13 -#define FRSKY_ID_GPS_COURS_BP 0x14 -#define FRSKY_ID_GPS_DAY_MONTH 0x15 -#define FRSKY_ID_GPS_YEAR 0x16 -#define FRSKY_ID_GPS_HOUR_MIN 0x17 -#define FRSKY_ID_GPS_SEC 0x18 -#define FRSKY_ID_GPS_SPEED_AP 0x19 -#define FRSKY_ID_GPS_LONG_AP 0x1A -#define FRSKY_ID_GPS_LAT_AP 0x1B -#define FRSKY_ID_GPS_COURS_AP 0x1C -#define FRSKY_ID_BARO_ALT_AP 0x21 -#define FRSKY_ID_GPS_LONG_EW 0x22 -#define FRSKY_ID_GPS_LAT_NS 0x23 -#define FRSKY_ID_ACCEL_X 0x24 -#define FRSKY_ID_ACCEL_Y 0x25 -#define FRSKY_ID_ACCEL_Z 0x26 -#define FRSKY_ID_CURRENT 0x28 -#define FRSKY_ID_VARIO 0x30 -#define FRSKY_ID_VFAS 0x39 -#define FRSKY_ID_VOLTS_BP 0x3A -#define FRSKY_ID_VOLTS_AP 0x3B - // Public functions void frsky_init(void); void frsky_send_frame1(int uart); -- cgit v1.2.3 From 5f44be31ad77618d0a7514d129f41666a956a52d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:07:49 +0100 Subject: Update copyright info for 2014. Happy New Year everyone! --- src/drivers/frsky_telemetry/frsky_data.c | 4 ++-- src/drivers/frsky_telemetry/frsky_data.h | 4 ++-- src/drivers/frsky_telemetry/frsky_telemetry.c | 4 ++-- src/drivers/frsky_telemetry/module.mk | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 3a0c4b4b3..9a7b6beef 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index cea93ef2b..a7d9eee53 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 5f3837b6e..c4e29fe51 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 808966691..1632c74f7 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions -- cgit v1.2.3 From 8fd909f51911b9ede93b19188ed360231f75de1c Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:08:44 +0100 Subject: Directly write to the voltage field for better precision. --- src/drivers/frsky_telemetry/frsky_data.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 9a7b6beef..edbdbf366 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -191,9 +191,7 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); - float voltage = battery.voltage_v * 11.0f / 21.0f; - frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage); - frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f); + frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); frsky_send_data(uart, FRSKY_ID_RPM, 0); -- cgit v1.2.3 From 1e7e65717a4522de59957d20be2b06ccc7b5a71d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:11:52 +0100 Subject: Only send data packets we really support. --- src/drivers/frsky_telemetry/frsky_data.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index edbdbf366..9e6d57fac 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -186,14 +186,9 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); - frsky_send_data(uart, FRSKY_ID_TEMP2, 0); - - frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ - frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); - - frsky_send_data(uart, FRSKY_ID_RPM, 0); + frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); frsky_send_startstop(uart); } -- cgit v1.2.3 From 4508972121196d8892520e56e6b374a59281e50a Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 22:34:08 +0100 Subject: Further data format and code style fixes. --- src/drivers/frsky_telemetry/frsky_data.c | 96 +++++++++++++-------------- src/drivers/frsky_telemetry/frsky_telemetry.c | 19 +++--- 2 files changed, 56 insertions(+), 59 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 9e6d57fac..63b2d2d29 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,8 +53,7 @@ #include #include - -// FrSky sensor hub data IDs +/* FrSky sensor hub data IDs */ #define FRSKY_ID_GPS_ALT_BP 0x01 #define FRSKY_ID_TEMP1 0x02 #define FRSKY_ID_RPM 0x03 @@ -87,18 +86,13 @@ #define FRSKY_ID_VOLTS_BP 0x3A #define FRSKY_ID_VOLTS_AP 0x3B - #define frac(f) (f - (int)f) -float frsky_format_gps(float dec); - - static int battery_sub = -1; static int sensor_sub = -1; static int global_position_sub = -1; static int vehicle_status_sub = -1; - /** * Initializes the uORB subscriptions. */ @@ -113,8 +107,7 @@ void frsky_init() /** * Sends a 0x5E start/stop byte. */ -static void -frsky_send_startstop(int uart) +static void frsky_send_startstop(int uart) { static const uint8_t c = 0x5E; write(uart, &c, sizeof(c)); @@ -123,14 +116,12 @@ frsky_send_startstop(int uart) /** * Sends one byte, performing byte-stuffing if necessary. */ -static void -frsky_send_byte(int uart, uint8_t value) +static void frsky_send_byte(int uart, uint8_t value) { - const uint8_t x5E[] = {0x5D, 0x3E}; - const uint8_t x5D[] = {0x5D, 0x3D}; + const uint8_t x5E[] = { 0x5D, 0x3E }; + const uint8_t x5D[] = { 0x5D, 0x3D }; - switch (value) - { + switch (value) { case 0x5E: write(uart, x5E, sizeof(x5E)); break; @@ -148,10 +139,9 @@ frsky_send_byte(int uart, uint8_t value) /** * Sends one data id/value pair. */ -static void -frsky_send_data(int uart, uint8_t id, int16_t data) +static void frsky_send_data(int uart, uint8_t id, int16_t data) { - // Cast data to unsigned, because signed shift might behave incorrectly + /* Cast data to unsigned, because signed shift might behave incorrectly */ uint16_t udata = data; frsky_send_startstop(uart); @@ -163,7 +153,7 @@ frsky_send_data(int uart, uint8_t id, int16_t data) /** * Sends frame 1 (every 200ms): - * acceleration values, altitude (vario), temperatures, current & voltages, RPM + * acceleration values, barometer altitude, temperature, battery voltage & current */ void frsky_send_frame1(int uart) { @@ -178,17 +168,25 @@ void frsky_send_frame1(int uart) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* send formatted frame */ - frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f); - - frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); - - frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); - - frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); - frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); + frsky_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter) * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius)); + + frsky_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + frsky_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); frsky_send_startstop(uart); } @@ -196,47 +194,48 @@ void frsky_send_frame1(int uart) /** * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. */ -float frsky_format_gps(float dec) +static float frsky_format_gps(float dec) { - float dms_deg = (int)dec; - float dec_deg = dec - dms_deg; - float dms_min = (int)(dec_deg * 60); - float dec_min = (dec_deg * 60) - dms_min; - float dms_sec = dec_min * 60; + float dms_deg = (int) dec; + float dec_deg = dec - dms_deg; + float dms_min = (int) (dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; - return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); } /** * Sends frame 2 (every 1000ms): - * course, latitude, longitude, speed, altitude (GPS), fuel level + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level */ void frsky_send_frame2(int uart) { - /* get a local copy of the global position data */ + /* get a local copy of the global position data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - /* get a local copy of the vehicle status data */ + /* get a local copy of the vehicle status data */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); /* send formatted frame */ - float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; - if (global_pos.valid) - { + int sec = 0; + if (global_pos.valid) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); - lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f; + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) + * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; } @@ -258,7 +257,8 @@ void frsky_send_frame2(int uart) frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); - frsky_send_data(uart, FRSKY_ID_FUEL, vehicle_status.battery_remaining); + frsky_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); @@ -267,11 +267,11 @@ void frsky_send_frame2(int uart) /** * Sends frame 3 (every 5000ms): - * date, time + * GPS date & time */ void frsky_send_frame3(int uart) { - /* get a local copy of the battery data */ + /* get a local copy of the battery data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index c4e29fe51..7b08ca69e 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -72,8 +72,7 @@ __EXPORT int frsky_telemetry_main(int argc, char *argv[]); /** * Opens the UART device and sets all required serial parameters. */ -static int -frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) { /* Open UART */ const int uart = open(uart_name, O_WRONLY | O_NOCTTY); @@ -118,20 +117,19 @@ frsky_open_uart(const char *uart_name, struct termios *uart_config_original) /** * Print command usage information */ -static void -usage() +static void usage() { - fprintf(stderr, "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + fprintf(stderr, + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); exit(1); } /** * The daemon thread. */ -static int -frsky_telemetry_thread_main(int argc, char *argv[]) +static int frsky_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ char *device_name = "/dev/ttyS1"; /* USART2 */ @@ -207,8 +205,7 @@ frsky_telemetry_thread_main(int argc, char *argv[]) * The main command function. * Processes command line arguments and starts the daemon. */ -int -frsky_telemetry_main(int argc, char *argv[]) +int frsky_telemetry_main(int argc, char *argv[]) { if (argc < 2) { warnx("missing command"); -- cgit v1.2.3