diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/drivers/frsky_telemetry/frsky_data.c | 289 | ||||
-rw-r--r-- | src/drivers/frsky_telemetry/frsky_data.h | 51 | ||||
-rw-r--r-- | src/drivers/frsky_telemetry/frsky_telemetry.c | 266 | ||||
-rw-r--r-- | src/drivers/frsky_telemetry/module.mk | 41 | ||||
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 4 | ||||
-rw-r--r-- | src/drivers/ms5611/ms5611.cpp | 6 | ||||
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 6 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.c | 21 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 4 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 15 |
10 files changed, 692 insertions, 11 deletions
diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c new file mode 100644 index 000000000..63b2d2d29 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado <px4@sradonia.net> + * + * 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 <px4@sradonia.net> + * + * FrSky telemetry implementation. + * + */ + +#include "frsky_data.h" + +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <arch/math.h> +#include <geo/geo.h> + +#include <uORB/topics/battery_status.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_status.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 + +#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; + +/** + * 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)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); +} + +/** + * 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, 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, udata); /* LSB */ + frsky_send_byte(uart, udata >> 8); /* MSB */ +} + +/** + * Sends frame 1 (every 200ms): + * acceleration values, barometer altitude, temperature, battery voltage & current + */ +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 */ + 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); +} + +/** + * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + */ +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; + + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); +} + +/** + * Sends frame 2 (every 1000ms): + * 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 */ + 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 */ + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; + char lat_ns = 0, lon_ew = 0; + 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'; + 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, frac(course) * 1000.0f); + + 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, 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, frac(speed) * 100.0f); + + 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, + roundf(vehicle_status.battery_remaining * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * GPS 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 */ + 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 new file mode 100644 index 000000000..a7d9eee53 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado <px4@sradonia.net> + * + * 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 <px4@sradonia.net> + * + * FrSky telemetry implementation. + * + */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H + +// 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..7b08ca69e --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado <px4@sradonia.net> + * + * 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 <px4@sradonia.net> + * + * FrSky telemetry implementation. + * + * This daemon emulates an FrSky sensor hub by periodically sending data + * packets to an attached FrSky receiver. + * + */ + +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <string.h> +#include <sys/types.h> +#include <fcntl.h> +#include <unistd.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> +#include <termios.h> + +#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 <devicename>]\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..1632c74f7 --- /dev/null +++ b/src/drivers/frsky_telemetry/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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 +# 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 diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 3cd6d6720..a95c4576b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -198,7 +198,9 @@ MEASAirspeed::collect() // uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + if (diff_press_pa < 0.0f) + diff_press_pa = 0.0f; struct differential_pressure_s report; diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 87788824a..6326cf7fc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -124,6 +124,8 @@ protected: int32_t _TEMP; int64_t _OFF; int64_t _SENS; + float _P; + float _T; /* altitude conversion calibration */ unsigned _msl_pressure; /* in kPa */ @@ -623,6 +625,8 @@ MS5611::collect() /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; /* generate a new report */ report.temperature = _TEMP / 100.0f; @@ -695,6 +699,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", _P); + printf("T: %.3f\n", _T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4d306d6d0..60eda2319 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -203,6 +203,12 @@ dsm_guess_format(bool reset) int dsm_init(const char *device) { + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + if (dsm_fd < 0) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705..0b8c4a6a8 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -125,6 +125,25 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} static void calculate_fw_crc(void) @@ -249,6 +268,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + check_reboot(); + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea04a663..a0daa97ea 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -220,3 +220,7 @@ extern volatile uint8_t debug_level; /** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index d557a7bad..a59dbe89c 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -516,16 +516,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // check the magic value if (value != PX4IO_REBOOT_BL_MAGIC) break; - - // note that we don't set BL_WAIT_MAGIC in - // BKP_DR1 as that is not necessary given the - // timing of the forceupdate command. The - // bootloader on px4io waits for enough time - // anyway, and this method works with older - // bootloader versions (tested with both - // revision 3 and revision 4). - - up_systemreset(); + + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: |