From 5f18ce506d1a8395e1565db941b7af64a5936f31 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 24 Nov 2013 13:39:02 +0100 Subject: [PATCH 01/40] 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 3068270865..7e53de0ae0 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 761fb8d9d6..c5190375f5 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 0000000000..8a57070b17 --- /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 0000000000..e0c39a42bd --- /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 0000000000..5f3837b6ed --- /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 0000000000..808966691a --- /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 From 50cbd1949971241a0b6108708b2b3fefe0c498e3 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 30 Dec 2013 20:27:04 +0100 Subject: [PATCH 02/40] 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 8a57070b17..3a0c4b4b3b 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 e0c39a42bd..cea93ef2b9 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); From 5f44be31ad77618d0a7514d129f41666a956a52d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:07:49 +0100 Subject: [PATCH 03/40] 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 3a0c4b4b3b..9a7b6beeff 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 cea93ef2b9..a7d9eee533 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 5f3837b6ed..c4e29fe518 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 808966691a..1632c74f75 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 From 8fd909f51911b9ede93b19188ed360231f75de1c Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:08:44 +0100 Subject: [PATCH 04/40] 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 9a7b6beeff..edbdbf366e 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); From 1e7e65717a4522de59957d20be2b06ccc7b5a71d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:11:52 +0100 Subject: [PATCH 05/40] 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 edbdbf366e..9e6d57fac1 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); } From 4508972121196d8892520e56e6b374a59281e50a Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 22:34:08 +0100 Subject: [PATCH 06/40] Further data format and code style fixes. --- src/drivers/frsky_telemetry/frsky_data.c | 90 +++++++++---------- src/drivers/frsky_telemetry/frsky_telemetry.c | 19 ++-- 2 files changed, 53 insertions(+), 56 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 9e6d57fac1..63b2d2d29f 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_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, frac(raw.baro_alt_meter) * 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, raw.baro_temp_celcius * 10.0f); + frsky_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius)); - 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_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 c4e29fe518..7b08ca69e9 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"); From e7c1e8e94b082a0ba5b22b2f449844ee9a76a50a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jan 2014 13:53:59 +0100 Subject: [PATCH 07/40] Added tests for mount / fsync / reboot --- src/systemcmds/tests/module.mk | 3 +- src/systemcmds/tests/test_mount.c | 232 ++++++++++++++++++++++++++++++ src/systemcmds/tests/tests.h | 1 + src/systemcmds/tests/tests_main.c | 1 + 4 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 src/systemcmds/tests/test_mount.c diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 68a080c77c..28e214a6c4 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -28,4 +28,5 @@ SRCS = test_adc.c \ tests_main.c \ test_param.c \ test_ppm_loopback.c \ - test_rc.c + test_rc.c \ + test_mount.c diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c new file mode 100644 index 0000000000..bef953d112 --- /dev/null +++ b/src/systemcmds/tests/test_mount.c @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file test_mount.c + * + * Device mount / unmount stress test + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "tests.h" + +int +test_mount(int argc, char *argv[]) +{ + const unsigned iterations = 100; + const unsigned alignments = 65; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + /* read current test status from file, write test instructions for next round */ + const char* cmd_filename = "/fs/microsd/mount_test_cmds"; + + /* initial values */ + int it_left_fsync = 100; + int it_left_abort = 100; + + int cmd_fd; + if (stat(cmd_filename, &buffer)) { + + /* command file exists, read off state */ + cmd_fd = open(cmd_filename, O_RDWR); + char buf[64]; + int ret = read(cmd_fd, buf, sizeof(buf)); + if (ret > 0) + ret = sscanf("%d %d", &it_left_fsync, &it_left_abort); + + warnx("Iterations left: #%d / #%d\n(%s)", it_left_fsync, it_left_abort, buf); + + /* now write again what to do next */ + if (it_left_fsync > 0) + it_left_fsync--; + if (it_left_fsync == 0 && it_left_abort > 0) + it_left_abort--; + + if (it_left_abort == 0) + (void)unlink(cmd_filename); + return 0; + + } else { + + /* this must be the first iteration, do something */ + cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT); + } + + char buf[64]; + sprintf(buf, "%d %d", it_left_fsync, it_left_abort); + write(cmd_fd, buf, strlen(buf) + 1); + + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32}; + + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { + + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + + for (unsigned a = 0; a < alignments; a++) { + + printf("\n"); + warnx("----- alignment test: %u bytes -----", a); + + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } + + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing unaligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); + + if ((0x3 & (uintptr_t)(write_buf + a))) + errx(1, "memory is unaligned, align shift: %d", a); + + } + + if (it_left_fsync > 0) { + fsync(fd); + } else { + if (it_left_abort % chunk_sizes[c] == 0) { + systemreset(); + } + } + //perf_end(wperf); + + } + end = hrt_absolute_time(); + + //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + + //perf_print_counter(wperf); + //perf_free(wperf); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + warnx("READ ERROR!"); + return 1; + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; + } + + } + + int ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } + + } + } + + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + return 1; + } + + /* we always reboot for the next test if we get here */ + systemreset(); + + /* never going to get here */ + return 0; +} diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h index a57d04be37..bec13bd304 100644 --- a/src/systemcmds/tests/tests.h +++ b/src/systemcmds/tests/tests.h @@ -109,6 +109,7 @@ extern int test_bson(int argc, char *argv[]); extern int test_file(int argc, char *argv[]); extern int test_mixer(int argc, char *argv[]); extern int test_rc(int argc, char *argv[]); +extern int test_mount(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 1088a44076..84535126fb 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -106,6 +106,7 @@ const struct { {"file", test_file, 0}, {"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST}, {"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST}, {"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST}, {NULL, NULL, 0} }; From 97e8386290b89b0877fe57de31000801b7d0b8c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jan 2014 13:54:14 +0100 Subject: [PATCH 08/40] Auto-restarting mount test if config file present --- ROMFS/px4fmu_test/init.d/rcS | 25 ++++++++++++++++++++++++- 1 file changed, 24 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 6aa1d3d46b..d8ed71f121 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -9,4 +9,27 @@ then # Try to get an USB console nshterm /dev/ttyACM0 & -fi \ No newline at end of file +fi + +# +# Try to mount the microSD card. +# +echo "[init] looking for microSD..." +if mount -t vfat /dev/mmcsd0 /fs/microsd +then + echo "[init] card mounted at /fs/microsd" + # Start playing the startup tune + tone_alarm start +else + echo "[init] no microSD card found" + # Play SOS + tone_alarm error +fi + +# +# The presence of this file suggests we're running a mount stress test +# +if [ -f /fs/microsd/mount_test_cmds ] +then + tests mount +fi From 7fa36a22d730ee9fbcd5ce8ecd61ae6e415b595e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jan 2014 14:05:23 +0100 Subject: [PATCH 09/40] Fixed tests config --- makefiles/config_px4fmu-v2_test.mk | 3 +++ 1 file changed, 3 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 0f60e88b5e..3139a0db49 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -16,11 +16,14 @@ MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 MODULES += systemcmds/perf MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm # # Library modules # MODULES += modules/systemlib +MODULES += modules/systemlib/mixer MODULES += modules/uORB # From 05649eb09c0ad26e64d42471bf091dc7ee6ce5fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jan 2014 17:05:19 +0100 Subject: [PATCH 10/40] Create test config for FMUv1 --- makefiles/config_px4fmu-v1_test.mk | 48 ++++++++++++++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 makefiles/config_px4fmu-v1_test.mk diff --git a/makefiles/config_px4fmu-v1_test.mk b/makefiles/config_px4fmu-v1_test.mk new file mode 100644 index 0000000000..41e8b95ff5 --- /dev/null +++ b/makefiles/config_px4fmu-v1_test.mk @@ -0,0 +1,48 @@ +# +# Makefile for the px4fmu_default configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/boards/px4fmu-v1 +MODULES += drivers/px4io +MODULES += systemcmds/perf +MODULES += systemcmds/reboot +MODULES += systemcmds/tests +MODULES += systemcmds/nshterm + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/uORB + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +# command priority stack entrypoint +BUILTIN_COMMANDS := \ + $(call _B, sercon, , 2048, sercon_main ) \ + $(call _B, serdis, , 2048, serdis_main ) From 7590d91cf24d7fdd9bc0167958eba16cf584c67c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jan 2014 17:05:52 +0100 Subject: [PATCH 11/40] Improved mount test --- ROMFS/px4fmu_test/init.d/rcS | 44 ++++++++- makefiles/config_px4fmu-v2_test.mk | 4 + src/systemcmds/tests/test_file.c | 71 +------------- src/systemcmds/tests/test_mount.c | 148 +++++++++++++++++++---------- 4 files changed, 149 insertions(+), 118 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index d8ed71f121..56482d1407 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -2,6 +2,7 @@ # # PX4FMU startup script for test hackery. # +uorb start if sercon then @@ -26,10 +27,51 @@ else tone_alarm error fi +# +# Start a minimal system +# + +if [ -f /etc/extras/px4io-v2_default.bin ] +then + set io_file /etc/extras/px4io-v2_default.bin +else + set io_file /etc/extras/px4io-v1_default.bin +fi + +if px4io start +then + echo "PX4IO OK" +fi + +if px4io checkcrc $io_file +then + echo "PX4IO CRC OK" +else + echo "PX4IO CRC failure" + tone_alarm MBABGP + if px4io forceupdate 14662 $io_file + then + usleep 500000 + if px4io start + then + echo "PX4IO restart OK" + tone_alarm MSPAA + else + echo "PX4IO restart failed" + tone_alarm MNGGG + sleep 5 + reboot + fi + else + echo "PX4IO update failed" + tone_alarm MNGGG + fi +fi + # # The presence of this file suggests we're running a mount stress test # -if [ -f /fs/microsd/mount_test_cmds ] +if [ -f /fs/microsd/mount_test_cmds.txt ] then tests mount fi diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 3139a0db49..6faef7e0a4 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -6,14 +6,18 @@ # Use the configuration's ROMFS. # ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_test +ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v2_default.bin # # Board support modules # MODULES += drivers/device MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm MODULES += drivers/led MODULES += drivers/boards/px4fmu-v2 +MODULES += drivers/px4io MODULES += systemcmds/perf MODULES += systemcmds/reboot MODULES += systemcmds/tests diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 798724cf1a..4ca84f276e 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -103,6 +103,7 @@ test_file(int argc, char *argv[]) if ((0x3 & (uintptr_t)(write_buf + a))) errx(1, "memory is unaligned, align shift: %d", a); + return 1; } fsync(fd); @@ -139,7 +140,8 @@ test_file(int argc, char *argv[]) } if (!compare_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -266,70 +268,3 @@ test_file(int argc, char *argv[]) return 0; } -#if 0 -int -test_file(int argc, char *argv[]) -{ - const iterations = 1024; - - /* check if microSD card is mounted */ - struct stat buffer; - if (stat("/fs/microsd/", &buffer)) { - warnx("no microSD card mounted, aborting file test"); - return 1; - } - - uint8_t buf[512]; - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); - - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, buf, sizeof(buf)); - perf_end(wperf); - } - end = hrt_absolute_time(); - - close(fd); - - unlink("/fs/microsd/testfile"); - - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - perf_print_counter(wperf); - perf_free(wperf); - - warnx("running unlink test"); - - /* ensure that common commands do not run against file count limits */ - for (unsigned i = 0; i < 64; i++) { - - warnx("unlink iteration #%u", i); - - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file before unlink()"); - int ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file before unlink()"); - close(fd); - - ret = unlink("/fs/microsd/testfile"); - if (ret != OK) - errx(1, "failed unlinking test file"); - - fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - if (fd < 0) - errx(1, "failed opening test file after unlink()"); - ret = write(fd, buf, sizeof(buf)); - if (ret < 0) - errx(1, "failed writing test file after unlink()"); - close(fd); - } - - return 0; -} -#endif diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index bef953d112..2f3a0d99e0 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -53,11 +54,17 @@ #include "tests.h" +const int fsync_tries = 50; +const int abort_tries = 200; + int test_mount(int argc, char *argv[]) { - const unsigned iterations = 100; - const unsigned alignments = 65; + const unsigned iterations = 10; + const unsigned alignments = 4; + + const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt"; + /* check if microSD card is mounted */ struct stat buffer; @@ -66,56 +73,114 @@ test_mount(int argc, char *argv[]) return 1; } + /* list directory */ + DIR *d; + struct dirent *dir; + d = opendir("/fs/microsd"); + if (d) { + + while ((dir = readdir(d)) != NULL) { + //printf("%s\n", dir->d_name); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + + if (stat(cmd_filename, &buffer) == OK) { + (void)unlink(cmd_filename); + } + + return 1; + } + /* read current test status from file, write test instructions for next round */ - const char* cmd_filename = "/fs/microsd/mount_test_cmds"; /* initial values */ - int it_left_fsync = 100; - int it_left_abort = 100; + int it_left_fsync = fsync_tries; + int it_left_abort = abort_tries; int cmd_fd; - if (stat(cmd_filename, &buffer)) { + if (stat(cmd_filename, &buffer) == OK) { /* command file exists, read off state */ - cmd_fd = open(cmd_filename, O_RDWR); + cmd_fd = open(cmd_filename, O_RDWR | O_NONBLOCK); char buf[64]; int ret = read(cmd_fd, buf, sizeof(buf)); - if (ret > 0) - ret = sscanf("%d %d", &it_left_fsync, &it_left_abort); - warnx("Iterations left: #%d / #%d\n(%s)", it_left_fsync, it_left_abort, buf); + if (ret > 0) { + int count = 0; + ret = sscanf(buf, "TEST: %u %u %n", &it_left_fsync, &it_left_abort, &count); + } else { + buf[0] = '\0'; + } + + if (it_left_fsync > fsync_tries) + it_left_fsync = fsync_tries; + + if (it_left_abort > abort_tries) + it_left_abort = abort_tries; + + warnx("Iterations left: #%d / #%d of %d / %d\n(%s)", it_left_fsync, it_left_abort, + fsync_tries, abort_tries, buf); + + int it_left_fsync_prev = it_left_fsync; /* now write again what to do next */ if (it_left_fsync > 0) it_left_fsync--; - if (it_left_fsync == 0 && it_left_abort > 0) + + if (it_left_fsync == 0 && it_left_abort > 0) { + it_left_abort--; - if (it_left_abort == 0) + /* announce mode switch */ + if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) { + warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC"); + fsync(stdout); + fsync(stderr); + usleep(20000); + } + + } + + if (it_left_abort == 0) { (void)unlink(cmd_filename); return 0; + } } else { /* this must be the first iteration, do something */ cmd_fd = open(cmd_filename, O_TRUNC | O_WRONLY | O_CREAT); + + warnx("First iteration of file test\n"); } char buf[64]; - sprintf(buf, "%d %d", it_left_fsync, it_left_abort); - write(cmd_fd, buf, strlen(buf) + 1); + int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort); + lseek(cmd_fd, 0, SEEK_SET); + write(cmd_fd, buf, strlen(buf) + 1); + fsync(cmd_fd); /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32}; + unsigned chunk_sizes[] = {32, 64, 128, 256, 512, 600, 1200}; for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); + printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); + fsync(stdout); + fsync(stderr); + usleep(50000); for (unsigned a = 0; a < alignments; a++) { - printf("\n"); - warnx("----- alignment test: %u bytes -----", a); + // warnx("----- alignment test: %u bytes -----", a); + printf("."); uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); @@ -127,15 +192,12 @@ test_mount(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); hrt_abstime start, end; - //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing unaligned writes - please wait.."); - start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { - //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != chunk_sizes[c]) { @@ -144,25 +206,23 @@ test_mount(int argc, char *argv[]) if ((0x3 & (uintptr_t)(write_buf + a))) errx(1, "memory is unaligned, align shift: %d", a); + return 1; + } if (it_left_fsync > 0) { fsync(fd); } else { - if (it_left_abort % chunk_sizes[c] == 0) { - systemreset(); + if (it_left_abort % 5 == 0) { + systemreset(false); + } else { + fsync(stdout); + fsync(stderr); } } - //perf_end(wperf); - } end = hrt_absolute_time(); - //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - //perf_print_counter(wperf); - //perf_free(wperf); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -204,28 +264,18 @@ test_mount(int argc, char *argv[]) } } - /* list directory */ - DIR *d; - struct dirent *dir; - d = opendir("/fs/microsd"); - if (d) { + fsync(stdout); + fsync(stderr); + usleep(20000); - while ((dir = readdir(d)) != NULL) { - //printf("%s\n", dir->d_name); - } - closedir(d); - - warnx("directory listing ok (FS mounted and readable)"); - - } else { - /* failed opening dir */ - warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); - return 1; - } /* we always reboot for the next test if we get here */ - systemreset(); + warnx("Iteration done, rebooting.."); + fsync(stdout); + fsync(stderr); + usleep(50000); + systemreset(false); /* never going to get here */ return 0; From 366af8da80a31c896b9b6b7254d80fa1b0fa86c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:49:21 +0100 Subject: [PATCH 12/40] Do not require suffixed constants --- makefiles/toolchain_gnu-arm-eabi.mk | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 9fd2dd516d..bb729e1032 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -138,8 +138,7 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wold-style-declaration \ -Wmissing-parameter-type \ -Wmissing-prototypes \ - -Wnested-externs \ - -Wunsuffixed-float-constants + -Wnested-externs # C++-specific warnings # From 76477b205775ddcbce6147d93985ddcdb10a3d52 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jan 2014 09:25:39 +0100 Subject: [PATCH 13/40] Added support for Octo Cox --- ROMFS/px4fmu_common/init.d/rcS | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f122921c56..66cb3f2375 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -213,7 +213,9 @@ then # 7000 .. 7999 Hexa + # 8000 .. 8999 Octo X # 9000 .. 9999 Octo + - # 10000 .. 19999 Wide arm / H frame + # 10000 .. 10999 Wide arm / H frame + # 11000 .. 11999 Hexa Cox + # 12000 .. 12999 Octo Cox if param compare SYS_AUTOSTART 4008 8 then @@ -277,6 +279,13 @@ then sh /etc/init.d/rc.octo set MODE custom fi + + if param compare SYS_AUTOSTART 12001 + then + set MIXER /etc/mixers/FMU_octo_cox.mix + sh /etc/init.d/rc.octo + set MODE custom + fi if param compare SYS_AUTOSTART 10015 15 then From 9886a384ff284722b65f61a6622669f86f1aa68f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 08:02:47 +0100 Subject: [PATCH 14/40] Fixed error handling logic, we want to return, not exit --- src/systemcmds/tests/test_file.c | 42 +++++++++++++++++--------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 4ca84f276e..419e8d5e9a 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -35,6 +35,8 @@ * @file test_file.c * * File write test. + * + * @author Lorenz Meier */ #include @@ -86,7 +88,6 @@ test_file(int argc, char *argv[]) uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); hrt_abstime start, end; - //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); @@ -94,29 +95,22 @@ test_file(int argc, char *argv[]) start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { - //perf_begin(wperf); int wret = write(fd, write_buf + a, chunk_sizes[c]); if (wret != chunk_sizes[c]) { warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) - errx(1, "memory is unaligned, align shift: %d", a); + warnx("memory is unaligned, align shift: %d", a); return 1; } fsync(fd); - //perf_end(wperf); } end = hrt_absolute_time(); - //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); - - //perf_print_counter(wperf); - //perf_free(wperf); - close(fd); fd = open("/fs/microsd/testfile", O_RDONLY); @@ -125,7 +119,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - errx(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -161,7 +156,8 @@ test_file(int argc, char *argv[]) int wret = write(fd, write_buf, chunk_sizes[c]); if (wret != chunk_sizes[c]) { - err(1, "WRITE ERROR!"); + warnx("WRITE ERROR!"); + return 1; } } @@ -180,7 +176,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } /* compare value */ @@ -195,7 +192,8 @@ test_file(int argc, char *argv[]) } if (!align_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -217,7 +215,8 @@ test_file(int argc, char *argv[]) int rret = read(fd, read_buf + a, chunk_sizes[c]); if (rret != chunk_sizes[c]) { - err(1, "READ ERROR!"); + warnx("READ ERROR!"); + return 1; } for (int j = 0; j < chunk_sizes[c]; j++) { @@ -233,7 +232,8 @@ test_file(int argc, char *argv[]) } if (!unalign_read_ok) { - errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + warnx("ABORTING FURTHER COMPARISON DUE TO ERROR"); + return 1; } } @@ -241,9 +241,10 @@ test_file(int argc, char *argv[]) ret = unlink("/fs/microsd/testfile"); close(fd); - if (ret) - err(1, "UNLINKING FILE FAILED"); - + if (ret) { + warnx("UNLINKING FILE FAILED"); + return 1; + } } } @@ -263,7 +264,8 @@ test_file(int argc, char *argv[]) } else { /* failed opening dir */ - err(1, "FAILED LISTING MICROSD ROOT DIRECTORY"); + warnx("FAILED LISTING MICROSD ROOT DIRECTORY"); + return 1; } return 0; From f35e6efbcaa5f9770ee4f6ef7686752b40c6a9ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 08:37:06 +0100 Subject: [PATCH 15/40] Check 30 seconds for USB port --- src/systemcmds/nshterm/nshterm.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 458bb2259e..7d9484d3eb 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -62,7 +62,9 @@ nshterm_main(int argc, char *argv[]) } uint8_t retries = 0; int fd = -1; - while (retries < 50) { + + /* try the first 30 seconds */ + while (retries < 300) { /* the retries are to cope with the behaviour of /dev/ttyACM0 */ /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); From 138b2890c4a874a82aff33df8e5ea37bd3a74e35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 08:37:34 +0100 Subject: [PATCH 16/40] Better mount test, still not reproducing failure very well --- src/systemcmds/tests/test_mount.c | 31 +++++++++++++++++++------------ 1 file changed, 19 insertions(+), 12 deletions(-) diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index 2f3a0d99e0..db4ddeed9e 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -54,14 +54,14 @@ #include "tests.h" -const int fsync_tries = 50; -const int abort_tries = 200; +const int fsync_tries = 1; +const int abort_tries = 10; int test_mount(int argc, char *argv[]) { - const unsigned iterations = 10; - const unsigned alignments = 4; + const unsigned iterations = 2000; + const unsigned alignments = 10; const char* cmd_filename = "/fs/microsd/mount_test_cmds.txt"; @@ -173,13 +173,13 @@ test_mount(int argc, char *argv[]) for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC"); + printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n"); fsync(stdout); fsync(stderr); usleep(50000); for (unsigned a = 0; a < alignments; a++) { - // warnx("----- alignment test: %u bytes -----", a); printf("."); uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); @@ -204,7 +204,7 @@ test_mount(int argc, char *argv[]) warn("WRITE ERROR!"); if ((0x3 & (uintptr_t)(write_buf + a))) - errx(1, "memory is unaligned, align shift: %d", a); + warnx("memory is unaligned, align shift: %d", a); return 1; @@ -213,14 +213,21 @@ test_mount(int argc, char *argv[]) if (it_left_fsync > 0) { fsync(fd); } else { - if (it_left_abort % 5 == 0) { - systemreset(false); - } else { - fsync(stdout); - fsync(stderr); - } + printf("#"); + fsync(stdout); + fsync(stderr); } } + + if (it_left_fsync > 0) { + printf("#"); + } + + printf("\n"); + fsync(stdout); + fsync(stderr); + usleep(1000000); + end = hrt_absolute_time(); close(fd); From 1a13e66aab3cd88b5447448b577fc44165ab01bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 15:59:18 +0800 Subject: [PATCH 17/40] px4iofirmware: make forceupdate more reliable this schedules a reboot rather than rebooting immediately, which means the FMU gets an ACK for its reboot operation, preventing it from timing out waiting for the ACK. That makes the timing of the reboot more consistent, which makes it more reliable for forceupdate --- src/modules/px4iofirmware/px4io.c | 21 +++++++++++++++++++++ src/modules/px4iofirmware/px4io.h | 4 ++++ src/modules/px4iofirmware/registers.c | 13 ++++--------- 3 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705f..0b8c4a6a8b 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 dea04a663b..a0daa97ea0 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 0358725db3..ad44730736 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -518,16 +518,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: From c4fc730acad12b74f51d9ba7d3ff267e3e1a1ab3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:13:51 +0800 Subject: [PATCH 18/40] FMUv2: make all UARTs use 512 byte buffer, 2048 for CDCACM output this is important when using UARTs for things like secondary GPS modules, which may produce large enough transfers that 128 bytes is not enough. The 2048 buffer for CDCACM transmit makes mavlink log and parameter transfer faster --- nuttx-configs/px4fmu-v2/nsh/defconfig | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 110bcb3637..e20411bca3 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -582,8 +582,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=128 -CONFIG_UART4_TXBUFSIZE=128 +CONFIG_UART4_RXBUFSIZE=512 +CONFIG_UART4_TXBUFSIZE=512 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -594,8 +594,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=256 -CONFIG_USART6_TXBUFSIZE=256 +CONFIG_USART6_RXBUFSIZE=512 +CONFIG_USART6_TXBUFSIZE=512 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -662,7 +662,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From d4d2571161530848f2a4ac153f2529ab50ec4fcc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:25:09 +0800 Subject: [PATCH 19/40] FMUv2: enable RXDMA on 2nd GPS port and debug console This should reduce the chance of lost data on these ports due to high interrupt latency --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index e20411bca3..8a282693f4 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -295,7 +295,7 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -# CONFIG_USART1_RXDMA is not set +CONFIG_USART1_RXDMA=y # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set @@ -304,7 +304,7 @@ CONFIG_USART3_RXDMA=y CONFIG_UART4_RXDMA=y # CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set -# CONFIG_USART6_RXDMA is not set +CONFIG_USART6_RXDMA=y # CONFIG_UART7_RS485 is not set # CONFIG_UART7_RXDMA is not set # CONFIG_UART8_RS485 is not set From 1f564a95ee89278b3e4eea6c2d4ded378b71542c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:37:40 +0800 Subject: [PATCH 20/40] meas_airspeed: avoid trivial dependency on math lib including the math lib adds a huge amount to flash usage --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 3cd6d67203..a95c4576b0 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; From 3be1a5182db7bd3802b77e7c03fc14f00ca218c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 22:39:51 +1100 Subject: [PATCH 21/40] FMUv1: use larger CDCACM buffer size for faster log transfer on FMUv1 --- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index e43b9c18e8..e60120b492 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -566,7 +566,7 @@ CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 CONFIG_CDCACM_RXBUFSIZE=512 -CONFIG_CDCACM_TXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=2048 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" From d6088efd34e5482d302e3a253fdd3a170d88490a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 10:32:15 +1100 Subject: [PATCH 22/40] ms5611: report P and T in ms5611 info --- src/drivers/ms5611/ms5611.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 87788824a9..6326cf7fcb 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); From 94b539dfddc5a2e293f51058ee5bf0d6ffc78406 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 18 Sep 2013 10:30:09 +1000 Subject: [PATCH 23/40] px4io: enable power on Spektrum connector on init --- src/modules/px4iofirmware/dsm.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 4d306d6d00..60eda23192 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); From 4ef7817d965ec77c04acd4e4173bb6051e7d6836 Mon Sep 17 00:00:00 2001 From: Buzz Date: Fri, 27 Sep 2013 15:10:37 +1000 Subject: [PATCH 24/40] added otp library --- src/modules/systemlib/module.mk | 4 +- src/modules/systemlib/otp.c | 191 ++++++++++++++++++++++++++++++++ src/modules/systemlib/otp.h | 156 ++++++++++++++++++++++++++ 3 files changed, 350 insertions(+), 1 deletion(-) create mode 100644 src/modules/systemlib/otp.c create mode 100644 src/modules/systemlib/otp.h diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 843cda7225..1749ec9c78 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -49,4 +49,6 @@ SRCS = err.c \ airspeed.c \ system_params.c \ mavlink_log.c \ - rc_check.c + rc_check.c \ + otp.c + diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c new file mode 100644 index 0000000000..7968637de1 --- /dev/null +++ b/src/modules/systemlib/otp.c @@ -0,0 +1,191 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Authors: + * Lorenz Meier + * David "Buzz" Bussenschutt + * + * 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 otp.c + * otp estimation + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#include +#include +#include +#include // memset +#include "conversions.h" +#include "otp.h" +#include "err.h" // warnx +#include + + +int val_read(void* dest, volatile const void* src, int bytes) +{ + + int i; + for (i = 0; i < bytes / 4; i++) { + *(((volatile uint32_t *)dest) + i) = *(((volatile uint32_t *)src) + i); + } + return i*4; +} + + +int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char* signature) +{ + + warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n",id_type, vid, pid); + + // descriptor + F_write_byte( ADDR_OTP_START, 'P'); // write the 'P' from PX4. to first byte in OTP + F_write_byte( ADDR_OTP_START+1, 'X'); // write the 'P' from PX4. to first byte in OTP + F_write_byte( ADDR_OTP_START+2, '4'); + F_write_byte( ADDR_OTP_START+3, '\0'); + //id_type + F_write_byte( ADDR_OTP_START+4, id_type); + // vid and pid are 4 bytes each + F_write_word( ADDR_OTP_START+5, vid); + F_write_word( ADDR_OTP_START+9, pid); + // leave some 19 bytes of space, and go to the next block... + // then the auth sig starts + for ( int i = 0 ; i < 128 ; i++ ) { + F_write_byte( ADDR_OTP_START+32+i, signature[i]); + } + + +} + +int lock_otp(void) +{ + //determine the required locking size - can only write full lock bytes */ +// int size = sizeof(struct otp) / 32; +// +// struct otp_lock otp_lock_mem; +// +// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem)); +// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++) +// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED; + //XXX add the actual call here to write the OTP_LOCK bytes only at final stage + // val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem)); + + int locksize = 5; + // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. + for ( int i = 0 ; i < locksize ; i++ ) { + F_write_byte( ADDR_OTP_LOCK_START+i, OTP_LOCK_LOCKED); + } + +} + + + +// COMPLETE, BUSY, or other flash error? +uint8_t F_GetStatus(void) { + uint8_t fs = F_COMPLETE; + if((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else { + if((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else { + if((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else { + if((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else { + fs = F_COMPLETE; } } } } + return fs; +} + + +// enable FLASH Registers +void F_unlock(void) +{ + if((FLASH->control & F_CR_LOCK) != 0) + { + FLASH->key = F_KEY1; + FLASH->key = F_KEY2; + } +} + +// lock the FLASH Registers +void F_lock(void) +{ + FLASH->control |= F_CR_LOCK; +} + +// flash write word. +uint8_t F_write_word(uint32_t Address, uint32_t Data) +{ + unsigned char octet[4] = {0,0,0,0}; + for (int i=0; i<4; i++) + { + octet[i] = ( Data >> (i*8) ) & 0xFF; + F_write_byte(Address+i,octet[i]); + } + + } + +// flash write byte +uint8_t F_write_byte(uint32_t Address, uint8_t Data) +{ + volatile uint8_t status = F_COMPLETE; + + //warnx("F_write_byte: %08X %02d", Address , Data ) ; + + //Check the parameters + assert(IS_F_ADDRESS(Address)); + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + while(status == F_BUSY){ status = F_GetStatus();} + + if(status == F_COMPLETE) + { + //if the previous operation is completed, proceed to program the new data + FLASH->control &= CR_PSIZE_MASK; + FLASH->control |= F_PSIZE_BYTE; + FLASH->control |= F_CR_PG; + + *(volatile uint8_t*)Address = Data; + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + while(status == F_BUSY){ status = F_GetStatus();} + + //if the program operation is completed, disable the PG Bit + FLASH->control &= (~F_CR_PG); + } + + //Return the Program Status + return status; +} + + + + \ No newline at end of file diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h new file mode 100644 index 0000000000..e80ca9afb4 --- /dev/null +++ b/src/modules/systemlib/otp.h @@ -0,0 +1,156 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Authors: + * Lorenz Meier + * David "Buzz" Bussenschutt + * + * 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 otp.h + * One TIme Programmable ( OTP ) Flash routine/s. + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#ifndef OTP_H_ +#define OTP_H_ + + __BEGIN_DECLS + +#define ADDR_OTP_START 0x1FFF7800 +#define ADDR_OTP_LOCK_START 0x1FFF7A00 + +#define OTP_LOCK_LOCKED 0x00 +#define OTP_LOCK_UNLOCKED 0xFF + + + +#include +#include + +// possible flash statuses +#define F_BUSY 1 +#define F_ERROR_WRP 2 +#define F_ERROR_PROGRAM 3 +#define F_ERROR_OPERATION 4 +#define F_COMPLETE 5 + +typedef struct +{ + volatile uint32_t accesscontrol; // 0x00 + volatile uint32_t key; // 0x04 + volatile uint32_t optionkey; // 0x08 + volatile uint32_t status; // 0x0C + volatile uint32_t control; // 0x10 + volatile uint32_t optioncontrol; //0x14 +} flash_registers; + +#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address +#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000) +#define F_R_BASE (AHB1PERIPH_BASE + 0x3C00) +#define FLASH ((flash_registers *) F_R_BASE) + +#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit +#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit +#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit +#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF) +#define F_PSIZE_WORD ((uint32_t)0x00000200) +#define F_PSIZE_BYTE ((uint32_t)0x00000000) +#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register +#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit. + +#define F_KEY1 ((uint32_t)0x45670123) +#define F_KEY2 ((uint32_t)0xCDEF89AB) +#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) + + + + #pragma pack(push, 1) + +/* + * The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes. + * The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15) + * to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed + * until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only + * contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly. + */ + + struct otp { + // first 32 bytes = the '0' Block + char id[4]; ///4 bytes < 'P' 'X' '4' '\n' + uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID + uint32_t vid; ///4 bytes + uint32_t pid; ///4 bytes + char unused[19]; ///19 bytes + // Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block ) + char signature[128]; + // insert extras here + uint32_t lock_bytes[4]; + }; + + struct otp_lock { + uint8_t lock_bytes[16]; + }; +#pragma pack(pop) + +#define UDID_START 0x1FFF7A10 +#define ADDR_F_SIZE 0x1FFF7A22 + +#pragma pack(push, 1) + union udid { + uint32_t serial[3]; + char data[12]; + }; +#pragma pack(pop) + + + /** + * s + */ + //__EXPORT float calc_indicated_airspeed(float differential_pressure); + + __EXPORT void F_unlock(void); + __EXPORT void F_lock(void); + __EXPORT int val_read(void* dest, volatile const void* src, int bytes); + __EXPORT int val_write(volatile void* dest, const void* src, int bytes); + __EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char* signature); + __EXPORT int lock_otp(void); + + + __EXPORT uint8_t F_write_byte(uint32_t Address, uint8_t Data); + __EXPORT uint8_t F_write_word(uint32_t Address, uint32_t Data); + +__END_DECLS + +#endif From 0ef85c133b387f5d5aab26e00985922c9f05c7e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:41:07 +0100 Subject: [PATCH 25/40] OTP return value cleanup --- src/modules/systemlib/otp.c | 209 +++++++++++++++++++++--------------- src/modules/systemlib/otp.h | 105 +++++++++--------- 2 files changed, 171 insertions(+), 143 deletions(-) diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 7968637de1..695574fdcb 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -1,9 +1,9 @@ /**************************************************************************** * * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Authors: + * Authors: * Lorenz Meier - * David "Buzz" Bussenschutt + * David "Buzz" Bussenschutt * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,6 +43,8 @@ * */ +#include +#include #include #include #include @@ -53,139 +55,170 @@ #include -int val_read(void* dest, volatile const void* src, int bytes) +int val_read(void *dest, volatile const void *src, int bytes) { - + int i; + for (i = 0; i < bytes / 4; i++) { - *(((volatile uint32_t *)dest) + i) = *(((volatile uint32_t *)src) + i); + *(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i); } - return i*4; + + return i * 4; } -int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char* signature) +int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) { - - warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n",id_type, vid, pid); - // descriptor - F_write_byte( ADDR_OTP_START, 'P'); // write the 'P' from PX4. to first byte in OTP - F_write_byte( ADDR_OTP_START+1, 'X'); // write the 'P' from PX4. to first byte in OTP - F_write_byte( ADDR_OTP_START+2, '4'); - F_write_byte( ADDR_OTP_START+3, '\0'); - //id_type - F_write_byte( ADDR_OTP_START+4, id_type); - // vid and pid are 4 bytes each - F_write_word( ADDR_OTP_START+5, vid); - F_write_word( ADDR_OTP_START+9, pid); - // leave some 19 bytes of space, and go to the next block... - // then the auth sig starts - for ( int i = 0 ; i < 128 ; i++ ) { - F_write_byte( ADDR_OTP_START+32+i, signature[i]); - } + warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid); - + int errors = 0; + + // descriptor + if (F_write_byte(ADDR_OTP_START, 'P')) + errors++; + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) + errors++; // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 2, '4')) + errors++; + if (F_write_byte(ADDR_OTP_START + 3, '\0')) + errors++; + //id_type + if (F_write_byte(ADDR_OTP_START + 4, id_type)) + errors++; + // vid and pid are 4 bytes each + if (F_write_word(ADDR_OTP_START + 5, vid)) + errors++; + if (F_write_word(ADDR_OTP_START + 9, pid)) + errors++; + + // leave some 19 bytes of space, and go to the next block... + // then the auth sig starts + for (int i = 0 ; i < 128 ; i++) { + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + errors++; + } + + return errors; } int lock_otp(void) { //determine the required locking size - can only write full lock bytes */ // int size = sizeof(struct otp) / 32; -// +// // struct otp_lock otp_lock_mem; // // memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem)); // for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++) // otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED; - //XXX add the actual call here to write the OTP_LOCK bytes only at final stage + //XXX add the actual call here to write the OTP_LOCK bytes only at final stage // val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem)); - - int locksize = 5; - // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. - for ( int i = 0 ; i < locksize ; i++ ) { - F_write_byte( ADDR_OTP_LOCK_START+i, OTP_LOCK_LOCKED); - } + int locksize = 5; + + int errors = 0; + + // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. + for (int i = 0 ; i < locksize ; i++) { + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + errors++; + } + + return errors; } -// COMPLETE, BUSY, or other flash error? -uint8_t F_GetStatus(void) { - uint8_t fs = F_COMPLETE; - if((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else { - if((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else { - if((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else { - if((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else { - fs = F_COMPLETE; } } } } - return fs; -} +// COMPLETE, BUSY, or other flash error? +int F_GetStatus(void) +{ + int fs = F_COMPLETE; + + if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else { + + if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else { + + if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else { + + if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else { + fs = F_COMPLETE; + } + } + } + } + + return fs; +} -// enable FLASH Registers +// enable FLASH Registers void F_unlock(void) { - if((FLASH->control & F_CR_LOCK) != 0) - { - FLASH->key = F_KEY1; - FLASH->key = F_KEY2; - } + if ((FLASH->control & F_CR_LOCK) != 0) { + FLASH->key = F_KEY1; + FLASH->key = F_KEY2; + } } -// lock the FLASH Registers +// lock the FLASH Registers void F_lock(void) { - FLASH->control |= F_CR_LOCK; + FLASH->control |= F_CR_LOCK; } -// flash write word. -uint8_t F_write_word(uint32_t Address, uint32_t Data) +// flash write word. +int F_write_word(uint32_t Address, uint32_t Data) { - unsigned char octet[4] = {0,0,0,0}; - for (int i=0; i<4; i++) - { - octet[i] = ( Data >> (i*8) ) & 0xFF; - F_write_byte(Address+i,octet[i]); - } - - } + unsigned char octet[4] = {0, 0, 0, 0}; + + int ret = 0; + + for (int i = 0; i < 4; i++) { + octet[i] = (Data >> (i * 8)) & 0xFF; + ret = F_write_byte(Address + i, octet[i]); + } + + return ret; +} // flash write byte -uint8_t F_write_byte(uint32_t Address, uint8_t Data) +int F_write_byte(uint32_t Address, uint8_t Data) { - volatile uint8_t status = F_COMPLETE; - - //warnx("F_write_byte: %08X %02d", Address , Data ) ; + volatile int status = F_COMPLETE; - //Check the parameters - assert(IS_F_ADDRESS(Address)); + //warnx("F_write_byte: %08X %02d", Address , Data ) ; - //Wait for FLASH operation to complete by polling on BUSY flag. - status = F_GetStatus(); - while(status == F_BUSY){ status = F_GetStatus();} - - if(status == F_COMPLETE) - { - //if the previous operation is completed, proceed to program the new data - FLASH->control &= CR_PSIZE_MASK; - FLASH->control |= F_PSIZE_BYTE; - FLASH->control |= F_CR_PG; - - *(volatile uint8_t*)Address = Data; + //Check the parameters + assert(IS_F_ADDRESS(Address)); - //Wait for FLASH operation to complete by polling on BUSY flag. - status = F_GetStatus(); - while(status == F_BUSY){ status = F_GetStatus();} + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); - //if the program operation is completed, disable the PG Bit - FLASH->control &= (~F_CR_PG); - } + while (status == F_BUSY) { status = F_GetStatus();} - //Return the Program Status - return status; + if (status == F_COMPLETE) { + //if the previous operation is completed, proceed to program the new data + FLASH->control &= CR_PSIZE_MASK; + FLASH->control |= F_PSIZE_BYTE; + FLASH->control |= F_CR_PG; + + *(volatile uint8_t *)Address = Data; + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + //if the program operation is completed, disable the PG Bit + FLASH->control &= (~F_CR_PG); + } + + //Return the Program Status + return !(status == F_COMPLETE); } - \ No newline at end of file diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index e80ca9afb4..f10e129d8c 100644 --- a/src/modules/systemlib/otp.h +++ b/src/modules/systemlib/otp.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Authors: - * Lorenz Meier - * David "Buzz" Bussenschutt + * Copyright (c) 2012-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 @@ -36,7 +33,7 @@ /** * @file otp.h - * One TIme Programmable ( OTP ) Flash routine/s. + * One TIme Programmable ( OTP ) Flash routine/s. * * @author Lorenz Meier * @author David "Buzz" Bussenschutt @@ -46,8 +43,8 @@ #ifndef OTP_H_ #define OTP_H_ - __BEGIN_DECLS - +__BEGIN_DECLS + #define ADDR_OTP_START 0x1FFF7800 #define ADDR_OTP_LOCK_START 0x1FFF7A00 @@ -58,22 +55,21 @@ #include #include - -// possible flash statuses + +// possible flash statuses #define F_BUSY 1 #define F_ERROR_WRP 2 #define F_ERROR_PROGRAM 3 #define F_ERROR_OPERATION 4 #define F_COMPLETE 5 -typedef struct -{ - volatile uint32_t accesscontrol; // 0x00 - volatile uint32_t key; // 0x04 - volatile uint32_t optionkey; // 0x08 - volatile uint32_t status; // 0x0C - volatile uint32_t control; // 0x10 - volatile uint32_t optioncontrol; //0x14 +typedef struct { + volatile uint32_t accesscontrol; // 0x00 + volatile uint32_t key; // 0x04 + volatile uint32_t optionkey; // 0x08 + volatile uint32_t status; // 0x0C + volatile uint32_t control; // 0x10 + volatile uint32_t optioncontrol; //0x14 } flash_registers; #define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address @@ -96,7 +92,7 @@ typedef struct - #pragma pack(push, 1) +#pragma pack(push, 1) /* * The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes. @@ -106,51 +102,50 @@ typedef struct * contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly. */ - struct otp { - // first 32 bytes = the '0' Block - char id[4]; ///4 bytes < 'P' 'X' '4' '\n' - uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID - uint32_t vid; ///4 bytes - uint32_t pid; ///4 bytes - char unused[19]; ///19 bytes - // Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block ) - char signature[128]; - // insert extras here - uint32_t lock_bytes[4]; - }; +struct otp { + // first 32 bytes = the '0' Block + char id[4]; ///4 bytes < 'P' 'X' '4' '\n' + uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID + uint32_t vid; ///4 bytes + uint32_t pid; ///4 bytes + char unused[19]; ///19 bytes + // Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block ) + char signature[128]; + // insert extras here + uint32_t lock_bytes[4]; +}; - struct otp_lock { - uint8_t lock_bytes[16]; - }; +struct otp_lock { + uint8_t lock_bytes[16]; +}; #pragma pack(pop) -#define UDID_START 0x1FFF7A10 #define ADDR_F_SIZE 0x1FFF7A22 #pragma pack(push, 1) - union udid { - uint32_t serial[3]; - char data[12]; - }; +union udid { + uint32_t serial[3]; + char data[12]; +}; #pragma pack(pop) - - /** - * s - */ - //__EXPORT float calc_indicated_airspeed(float differential_pressure); - - __EXPORT void F_unlock(void); - __EXPORT void F_lock(void); - __EXPORT int val_read(void* dest, volatile const void* src, int bytes); - __EXPORT int val_write(volatile void* dest, const void* src, int bytes); - __EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char* signature); - __EXPORT int lock_otp(void); - - - __EXPORT uint8_t F_write_byte(uint32_t Address, uint8_t Data); - __EXPORT uint8_t F_write_word(uint32_t Address, uint32_t Data); - + +/** + * s + */ +//__EXPORT float calc_indicated_airspeed(float differential_pressure); + +__EXPORT void F_unlock(void); +__EXPORT void F_lock(void); +__EXPORT int val_read(void *dest, volatile const void *src, int bytes); +__EXPORT int val_write(volatile void *dest, const void *src, int bytes); +__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature); +__EXPORT int lock_otp(void); + + +__EXPORT int F_write_byte(uint32_t Address, uint8_t Data); +__EXPORT int F_write_word(uint32_t Address, uint32_t Data); + __END_DECLS #endif From ea4552a53d5bced405b83e455ded691d62bc7fb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:41:54 +0100 Subject: [PATCH 26/40] Added functionality to read serial --- src/modules/systemlib/board_serial.c | 60 ++++++++++++++++++++++++++++ src/modules/systemlib/board_serial.h | 49 +++++++++++++++++++++++ 2 files changed, 109 insertions(+) create mode 100644 src/modules/systemlib/board_serial.c create mode 100644 src/modules/systemlib/board_serial.h diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c new file mode 100644 index 0000000000..ad8c2bf83e --- /dev/null +++ b/src/modules/systemlib/board_serial.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#include "otp.h" +#include "board_config.h" +#include "board_serial.h" + +int get_board_serial(char *serialid) +{ + const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + union udid id; + val_read((unsigned *)&id, udid_ptr, sizeof(id)); + + + /* Copy the serial from the chips non-write memory and swap endianess */ + serialid[0] = id.data[3]; serialid[1] = id.data[2]; serialid[2] = id.data[1]; serialid[3] = id.data[0]; + serialid[4] = id.data[7]; serialid[5] = id.data[6]; serialid[6] = id.data[5]; serialid[7] = id.data[4]; + serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; + + return 0; +} \ No newline at end of file diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h new file mode 100644 index 0000000000..b14bb4376b --- /dev/null +++ b/src/modules/systemlib/board_serial.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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. + * + ****************************************************************************/ + +/** + * @file board_serial.h + * Read off the board serial + * + * @author Lorenz Meier + * @author David "Buzz" Bussenschutt + * + */ + +#pragma once + +__BEGIN_DECLS + +__EXPORT int get_board_serial(char *serialid); + +__END_DECLS From 72b9d3a0b1084e8ae9216edf95055ae5e0cb5fb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:42:17 +0100 Subject: [PATCH 27/40] Added unique ID location --- src/drivers/boards/px4fmu-v1/board_config.h | 1 + src/drivers/boards/px4fmu-v2/board_config.h | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 6f71662846..02c26b5c0b 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -60,6 +60,7 @@ __BEGIN_DECLS /* PX4IO connection configuration */ #define PX4IO_SERIAL_DEVICE "/dev/ttyS2" +#define UDID_START 0x1FFF7A10 //#ifdef CONFIG_STM32_SPI2 //# error "SPI2 is not supported on this board" diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index a19ed9d247..4972e69147 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -52,6 +52,8 @@ __BEGIN_DECLS /* these headers are not C++ safe */ #include #include + +#define UDID_START 0x1FFF7A10 /**************************************************************************************************** * Definitions From 1834a884b2abab0b38612c8d69f15156d0ef9d14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:42:39 +0100 Subject: [PATCH 28/40] Added FMU command to read serial --- src/drivers/px4fmu/fmu.cpp | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index b878d29bcc..b28d318d7e 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -65,6 +65,7 @@ #include #include #include +#include #include #include @@ -224,10 +225,10 @@ PX4FMU::PX4FMU() : _armed(false), _pwm_on(false), _mixers(nullptr), - _failsafe_pwm( {0}), - _disarmed_pwm( {0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _failsafe_pwm({0}), + _disarmed_pwm({0}), + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; @@ -575,7 +576,7 @@ PX4FMU::task_main() if (i >= outputs.noutputs || !isfinite(outputs.output[i]) || outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { + outputs.output[i] > 1.0f) { /* * Value is NaN, INF or out of band - set to the minimum value. * This will be clearly visible on the servo status and will limit the risk of accidentally @@ -933,7 +934,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(3): case PWM_SERVO_SET(2): if (_mode < MODE_4PWM) { @@ -941,7 +942,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(1): case PWM_SERVO_SET(0): if (arg <= 2100) { @@ -960,7 +961,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(3): case PWM_SERVO_GET(2): if (_mode < MODE_4PWM) { @@ -968,7 +969,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): *(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0)); @@ -1591,6 +1592,15 @@ fmu_main(int argc, char *argv[]) errx(0, "FMU driver stopped"); } + if (!strcmp(verb, "id")) { + char id[12]; + (void)get_board_serial(id); + + errx(0, "Board serial:\n %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X", + (unsigned)id[0], (unsigned)id[1], (unsigned)id[2], (unsigned)id[3], (unsigned)id[4], (unsigned)id[5], + (unsigned)id[6], (unsigned)id[7], (unsigned)id[8], (unsigned)id[9], (unsigned)id[10], (unsigned)id[11]); + } + if (fmu_start() != OK) errx(1, "failed to start the FMU driver"); @@ -1647,6 +1657,7 @@ fmu_main(int argc, char *argv[]) sensor_reset(0); warnx("resettet default time"); } + exit(0); } From c463fde0b95eb07a5f2792032d24fbda8626b808 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:42:51 +0100 Subject: [PATCH 29/40] Compiling in new functions --- src/modules/systemlib/module.mk | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 1749ec9c78..8c6c300d6b 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -50,5 +50,6 @@ SRCS = err.c \ system_params.c \ mavlink_log.c \ rc_check.c \ - otp.c + otp.c \ + board_serial.c From fed5a2daae3298ba097b4e7b406168928fc7816b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Jan 2014 08:41:50 +0100 Subject: [PATCH 30/40] Better settings for the mount test --- src/systemcmds/tests/test_mount.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/test_mount.c b/src/systemcmds/tests/test_mount.c index db4ddeed9e..44e34d9ef3 100644 --- a/src/systemcmds/tests/test_mount.c +++ b/src/systemcmds/tests/test_mount.c @@ -223,10 +223,10 @@ test_mount(int argc, char *argv[]) printf("#"); } - printf("\n"); + printf("."); fsync(stdout); fsync(stderr); - usleep(1000000); + usleep(200000); end = hrt_absolute_time(); From 184f4a29eb010413a27cabedbcbc348ef1af7100 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 8 Jan 2014 18:06:30 +0100 Subject: [PATCH 31/40] Improved file test, allowed abortion --- src/systemcmds/tests/test_file.c | 50 +++++++++++++++++++++++++++++++- 1 file changed, 49 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 798724cf1a..cdb0b03374 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -38,6 +38,7 @@ */ #include +#include #include #include #include @@ -51,6 +52,38 @@ #include "tests.h" +int check_user_abort(); + +int check_user_abort() { + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'c': + case 'q': + { + warnx("Test aborted."); + return OK; + /* not reached */ + } + } + } + + return 1; +} + int test_file(int argc, char *argv[]) { @@ -108,6 +141,9 @@ test_file(int argc, char *argv[]) fsync(fd); //perf_end(wperf); + if (!check_user_abort()) + return OK; + } end = hrt_absolute_time(); @@ -142,6 +178,9 @@ test_file(int argc, char *argv[]) errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); } + if (!check_user_abort()) + return OK; + } /* @@ -152,7 +191,7 @@ test_file(int argc, char *argv[]) int ret = unlink("/fs/microsd/testfile"); fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("testing aligned writes - please wait.."); + warnx("testing aligned writes - please wait.. (CTRL^C to abort)"); start = hrt_absolute_time(); for (unsigned i = 0; i < iterations; i++) { @@ -162,6 +201,9 @@ test_file(int argc, char *argv[]) err(1, "WRITE ERROR!"); } + if (!check_user_abort()) + return OK; + } fsync(fd); @@ -190,6 +232,9 @@ test_file(int argc, char *argv[]) align_read_ok = false; break; } + + if (!check_user_abort()) + return OK; } if (!align_read_ok) { @@ -228,6 +273,9 @@ test_file(int argc, char *argv[]) if (unalign_read_err_count > 10) break; } + + if (!check_user_abort()) + return OK; } if (!unalign_read_ok) { From 2d8d43c4a8802b859b482a3fb4d26dc268fdacfe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:11:35 +0100 Subject: [PATCH 32/40] Added UART5 DMA config for reliable IO updates --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 110bcb3637..bc2395af5b 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -302,7 +302,7 @@ CONFIG_USART2_RXDMA=y CONFIG_USART3_RXDMA=y # CONFIG_UART4_RS485 is not set CONFIG_UART4_RXDMA=y -# CONFIG_UART5_RXDMA is not set +CONFIG_UART5_RXDMA=y # CONFIG_USART6_RS485 is not set # CONFIG_USART6_RXDMA is not set # CONFIG_UART7_RS485 is not set From f5dfc241977e5095f4821d9f325a4d702905cb04 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 08:44:57 +0100 Subject: [PATCH 33/40] Allow to check IO CRC independent of the IO start status (retains the interface status, startet or unstarted --- src/drivers/px4io/px4io.cpp | 116 ++++++++++++++++++++++-------------- 1 file changed, 71 insertions(+), 45 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cbdd5acc44..0ca35d2f29 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -35,7 +35,7 @@ * @file px4io.cpp * Driver for the PX4IO board. * - * PX4IO is connected via I2C. + * PX4IO is connected via I2C or DMA enabled high-speed UART. */ #include @@ -1690,6 +1690,9 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + if (ret) + return ret; + retries--; log("mixer sent"); @@ -2419,6 +2422,69 @@ detect(int argc, char *argv[]) } } +void +checkcrc(int argc, char *argv[]) +{ + bool keep_running = false; + + if (g_dev == nullptr) { + /* allocate the interface */ + device::Device *interface = get_interface(); + + /* create the driver - it will set g_dev */ + (void)new PX4IO(interface); + + if (g_dev == nullptr) + errx(1, "driver alloc failed"); + } else { + /* its already running, don't kill the driver */ + keep_running = true; + } + + /* + check IO CRC against CRC of a file + */ + if (argc < 2) { + printf("usage: px4io checkcrc filename\n"); + exit(1); + } + int fd = open(argv[1], O_RDONLY); + if (fd == -1) { + printf("open of %s failed - %d\n", argv[1], errno); + exit(1); + } + const uint32_t app_size_max = 0xf000; + uint32_t fw_crc = 0; + uint32_t nbytes = 0; + while (true) { + uint8_t buf[16]; + int n = read(fd, buf, sizeof(buf)); + if (n <= 0) break; + fw_crc = crc32part(buf, n, fw_crc); + nbytes += n; + } + close(fd); + while (nbytes < app_size_max) { + uint8_t b = 0xff; + fw_crc = crc32part(&b, 1, fw_crc); + nbytes++; + } + + int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); + + if (!keep_running) { + delete g_dev; + g_dev = nullptr; + } + + if (ret != OK) { + printf("check CRC failed - %d\n", ret); + exit(1); + } + printf("CRCs match\n"); + exit(0); +} + void bind(int argc, char *argv[]) { @@ -2613,6 +2679,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "detect")) detect(argc - 1, argv + 1); + if (!strcmp(argv[1], "checkcrc")) + checkcrc(argc - 1, argv + 1); + if (!strcmp(argv[1], "update")) { if (g_dev != nullptr) { @@ -2798,49 +2867,6 @@ px4io_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "checkcrc")) { - /* - check IO CRC against CRC of a file - */ - if (argc <= 2) { - printf("usage: px4io checkcrc filename\n"); - exit(1); - } - if (g_dev == nullptr) { - printf("px4io is not started\n"); - exit(1); - } - int fd = open(argv[2], O_RDONLY); - if (fd == -1) { - printf("open of %s failed - %d\n", argv[2], errno); - exit(1); - } - const uint32_t app_size_max = 0xf000; - uint32_t fw_crc = 0; - uint32_t nbytes = 0; - while (true) { - uint8_t buf[16]; - int n = read(fd, buf, sizeof(buf)); - if (n <= 0) break; - fw_crc = crc32part(buf, n, fw_crc); - nbytes += n; - } - close(fd); - while (nbytes < app_size_max) { - uint8_t b = 0xff; - fw_crc = crc32part(&b, 1, fw_crc); - nbytes++; - } - - int ret = g_dev->ioctl(nullptr, PX4IO_CHECK_CRC, fw_crc); - if (ret != OK) { - printf("check CRC failed - %d\n", ret); - exit(1); - } - printf("CRCs match\n"); - exit(0); - } - if (!strcmp(argv[1], "rx_dsm") || !strcmp(argv[1], "rx_dsm_10bit") || !strcmp(argv[1], "rx_dsm_11bit") || From 4fcbe806cef61aa3b8a749602b65da0e5c8d48a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 9 Jan 2014 18:05:17 +0100 Subject: [PATCH 34/40] Cleaned up init config and picked a safe bet on FRAM clock speed --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 269ec238eb..71414d62c7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -282,7 +282,7 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi1, PX4_SPIDEV_MPU, false); up_udelay(20); - message("[boot] Successfully initialized SPI port 1\n"); + message("[boot] Initialized SPI port 1 (SENSORS)\n"); /* Get the SPI port for the FRAM */ @@ -294,20 +294,23 @@ __EXPORT int nsh_archinitialize(void) return -ENODEV; } - /* Default SPI2 to 37.5 MHz (F4 max) and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 375000000); + /* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max) + * and de-assert the known chip selects. */ + + // XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated + SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); - message("[boot] Successfully initialized SPI port 2\n"); + message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); if (!sdio) { - message("nsh_archinitialize: Failed to initialize SDIO slot %d\n", + message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); return -ENODEV; } @@ -315,7 +318,7 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); if (ret != OK) { - message("nsh_archinitialize: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; } From a303175c4c24f33b15b787b99be47be5ddafae3b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 10 Jan 2014 07:51:47 +0100 Subject: [PATCH 35/40] Reduce the scheduler priority to a more acceptable value --- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 042d9f8162..d293f9954e 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -237,7 +237,7 @@ hott_telemetry_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); From 7265006f3f6d3da7d5fd7010dc9da92a22cae6d8 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 10 Jan 2014 08:03:54 +0100 Subject: [PATCH 36/40] Adjust the HoTT sensor scheduler priority as well --- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index e322c6349e..a3d3a39333 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -211,7 +211,7 @@ hott_sensors_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, + SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); From f205c07c084fd1008f201518d84c64718e7df9cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 10 Jan 2014 22:38:12 +0100 Subject: [PATCH 37/40] very simple gps fix fake in gps driver only for development --- src/drivers/gps/gps.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ecd..1afb279fe6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -294,6 +294,29 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ +//#define FAKEGPS +#ifdef FAKEGPS + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 20.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated +#endif if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); From a522c64fee57c8e5e0cd188e589c8bcf87b5396d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:08:02 +0100 Subject: [PATCH 38/40] fake gps: gps device is not needed for fake position generation --- src/drivers/gps/gps.cpp | 57 ++++++++++++++++++++++++----------------- 1 file changed, 34 insertions(+), 23 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1afb279fe6..3649a41d6c 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -264,6 +264,39 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { +//#define FAKEGPS +#ifdef FAKEGPS + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 20.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + + usleep(2e5); +#else + if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ @@ -294,29 +327,6 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ -//#define FAKEGPS -#ifdef FAKEGPS - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 20.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; - - //no time and satellite information simulated -#endif if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); @@ -380,6 +390,7 @@ GPS::task_main() default: break; } +#endif } From 2ed315480e4582c9f223b88e1fee39303fbc9248 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:19:43 +0100 Subject: [PATCH 39/40] fakegps: default to 0 m/s speed --- src/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 3649a41d6c..7df9cdb6d6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -264,7 +264,7 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { -//#define FAKEGPS +#define FAKEGPS #ifdef FAKEGPS _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; @@ -278,7 +278,7 @@ GPS::task_main() _report.eph_m = 10.0f; _report.epv_m = 10.0f; _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 20.0f; + _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; _report.vel_d_m_s = 0.0f; _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); From bccd0f8947f8d95f048b421b06fde25d162aac50 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:46:45 +0100 Subject: [PATCH 40/40] fakegps: add command-line option --- src/drivers/gps/gps.cpp | 260 +++++++++++++++++++++------------------- 1 file changed, 135 insertions(+), 125 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 7df9cdb6d6..6b72d560fa 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: struct vehicle_gps_position_s _report; ///< uORB topic for gps position orb_advert_t _report_pub; ///< uORB pub for gps position float _rate; ///< position update rate + bool _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,134 +266,135 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { -#define FAKEGPS -#ifdef FAKEGPS - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 0.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; + if (_fake_gps) { - //no time and satellite information simulated + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + //no time and satellite information simulated - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - usleep(2e5); -#else - - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; - - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; - - default: - break; - } - - unlock(); - - if (_Helper->configure(_baudrate) == 0) { - unlock(); - - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); - - while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { -// lock(); - /* opportunistic publishing - else invalid data would end up on the bus */ - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } - - last_rate_count++; - - /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { - _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); - last_rate_measurement = hrt_absolute_time(); - last_rate_count = 0; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } - - if (!_healthy) { - char *mode_str = "unknown"; - - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; - - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; - - default: - break; - } - - warnx("module found: %s", mode_str); - _healthy = true; - } + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + usleep(2e5); + + } else { + + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + default: + break; + } + + unlock(); + + if (_Helper->configure(_baudrate) == 0) { + unlock(); + + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); + + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { + // lock(); + /* opportunistic publishing - else invalid data would end up on the bus */ + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + + last_rate_count++; + + /* measure update rate every 5 seconds */ + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); + last_rate_measurement = hrt_absolute_time(); + last_rate_count = 0; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } + + if (!_healthy) { + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); + _healthy = true; + } + } + + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; + } + + lock(); } lock(); + + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + + default: + break; + } } - lock(); - - /* select next mode */ - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _mode = GPS_DRIVER_MODE_MTK; - break; - - case GPS_DRIVER_MODE_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - - default: - break; - } -#endif - } warnx("exiting"); @@ -451,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -461,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -469,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -561,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -576,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -601,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); }