From 5f18ce506d1a8395e1565db941b7af64a5936f31 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Sun, 24 Nov 2013 13:39:02 +0100 Subject: Add FrSky telemetry application. This daemon emulates an FrSky sensor hub by periodically sending data packets to an attached FrSky receiver. --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/frsky_telemetry/frsky_data.c | 223 +++++++++++++++++++++ src/drivers/frsky_telemetry/frsky_data.h | 84 ++++++++ src/drivers/frsky_telemetry/frsky_telemetry.c | 269 ++++++++++++++++++++++++++ src/drivers/frsky_telemetry/module.mk | 41 ++++ 6 files changed, 619 insertions(+) create mode 100644 src/drivers/frsky_telemetry/frsky_data.c create mode 100644 src/drivers/frsky_telemetry/frsky_data.h create mode 100644 src/drivers/frsky_telemetry/frsky_telemetry.c create mode 100644 src/drivers/frsky_telemetry/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 306827086..7e53de0ae 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -37,6 +37,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d..c5190375f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -35,6 +35,7 @@ MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed +MODULES += drivers/frsky_telemetry MODULES += modules/sensors # Needs to be burned to the ground and re-written; for now, diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c new file mode 100644 index 000000000..8a57070b1 --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_data.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ + +#include "frsky_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include + + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; + + +/** + * Initializes the uORB subscriptions. + */ +void frsky_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); +} + +/** + * Sends a 0x5E start/stop byte. + */ +static void +frsky_send_startstop(int uart) +{ + static const uint8_t c = 0x5E; + write(uart, &c, sizeof(c)); +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void +frsky_send_byte(int uart, uint8_t value) +{ + const uint8_t x5E[] = {0x5D, 0x3E}; + const uint8_t x5D[] = {0x5D, 0x3D}; + + switch (value) + { + case 0x5E: + write(uart, x5E, sizeof(x5E)); + break; + + case 0x5D: + write(uart, x5D, sizeof(x5D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +static void +frsky_send_data(int uart, uint8_t id, uint16_t data) +{ + frsky_send_startstop(uart); + + frsky_send_byte(uart, id); + frsky_send_byte(uart, data); /* Low */ + frsky_send_byte(uart, data >> 8); /* High */ +} + +/** + * Sends frame 1 (every 200ms): + * acceleration values, altitude (vario), temperatures, current & voltages, RPM + */ +void frsky_send_frame1(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send formatted frame */ + // TODO + frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius); + frsky_send_data(uart, FRSKY_ID_TEMP2, 0); + + frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ + frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); + + frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v); + frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f); + + frsky_send_data(uart, FRSKY_ID_RPM, 0); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 2 (every 1000ms): + * course, latitude, longitude, speed, altitude (GPS), fuel level + */ +void frsky_send_frame2(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + // TODO + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + if (global_pos.valid) + { + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + // TODO: latitude, longitude + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + alt = global_pos.alt; + } + + frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, (speed - (int)speed) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, (alt - (int)alt) * 100.0f); + + frsky_send_data(uart, FRSKY_ID_FUEL, 0); + + frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + + frsky_send_startstop(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * date, time + */ +void frsky_send_frame3(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + // TODO + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + + frsky_send_startstop(uart); +} diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h new file mode 100644 index 000000000..e0c39a42b --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -0,0 +1,84 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_data.h + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H + +// FrSky sensor hub data IDs +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +// Public functions +void frsky_init(void); +void frsky_send_frame1(int uart); +void frsky_send_frame2(int uart); +void frsky_send_frame3(int uart); + +#endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c new file mode 100644 index 000000000..5f3837b6e --- /dev/null +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file frsky_telemetry.c + * @author Stefan Rado + * + * FrSky telemetry implementation. + * + * This daemon emulates an FrSky sensor hub by periodically sending data + * packets to an attached FrSky receiver. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "frsky_data.h" + + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int frsky_task; + +/* functions */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static void usage(void); +static int frsky_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int frsky_telemetry_main(int argc, char *argv[]); + + +/** + * Opens the UART device and sets all required serial parameters. + */ +static int +frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +{ + /* Open UART */ + const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", uart_name); + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERROR get termios config %s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Disable output post-processing */ + uart_config.c_oflag &= ~OPOST; + + /* Set baud rate */ + static const speed_t speed = B9600; + + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + return uart; +} + +/** + * Print command usage information + */ +static void +usage() +{ + fprintf(stderr, "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); + exit(1); +} + +/** + * The daemon thread. + */ +static int +frsky_telemetry_thread_main(int argc, char *argv[]) +{ + /* Default values for arguments */ + char *device_name = "/dev/ttyS1"; /* USART2 */ + + /* Work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + case 'd': + device_name = optarg; + break; + + default: + usage(); + break; + } + } + + /* Print welcome text */ + warnx("FrSky telemetry interface starting..."); + + /* Open UART */ + struct termios uart_config_original; + const int uart = frsky_open_uart(device_name, &uart_config_original); + + if (uart < 0) + err(1, "could not open %s", device_name); + + /* Subscribe to topics */ + frsky_init(); + + thread_running = true; + + /* Main thread loop */ + unsigned int iteration = 0; + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) + { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) + { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int +frsky_telemetry_main(int argc, char *argv[]) +{ + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) + errx(0, "frsky_telemetry already running"); + + thread_should_exit = false; + frsky_task = task_spawn_cmd("frsky_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + frsky_telemetry_thread_main, + (const char **)argv); + + while (!thread_running) { + usleep(200); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) + errx(0, "frsky_telemetry already stopped"); + + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk new file mode 100644 index 000000000..808966691 --- /dev/null +++ b/src/drivers/frsky_telemetry/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# FrSky telemetry application. +# + +MODULE_COMMAND = frsky_telemetry + +SRCS = frsky_data.c \ + frsky_telemetry.c -- cgit v1.2.3 From 50cbd1949971241a0b6108708b2b3fefe0c498e3 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 30 Dec 2013 20:27:04 +0100 Subject: Fixes to FrSky telemetry data formats. --- src/drivers/frsky_telemetry/frsky_data.c | 137 +++++++++++++++++++++++-------- src/drivers/frsky_telemetry/frsky_data.h | 33 -------- 2 files changed, 105 insertions(+), 65 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 8a57070b1..3a0c4b4b3 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -51,11 +51,52 @@ #include #include #include +#include + + +// FrSky sensor hub data IDs +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + + +#define frac(f) (f - (int)f) + +float frsky_format_gps(float dec); static int battery_sub = -1; static int sensor_sub = -1; static int global_position_sub = -1; +static int vehicle_status_sub = -1; /** @@ -66,6 +107,7 @@ void frsky_init() battery_sub = orb_subscribe(ORB_ID(battery_status)); global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); } /** @@ -107,13 +149,16 @@ frsky_send_byte(int uart, uint8_t value) * Sends one data id/value pair. */ static void -frsky_send_data(int uart, uint8_t id, uint16_t data) +frsky_send_data(int uart, uint8_t id, int16_t data) { + // Cast data to unsigned, because signed shift might behave incorrectly + uint16_t udata = data; + frsky_send_startstop(uart); frsky_send_byte(uart, id); - frsky_send_byte(uart, data); /* Low */ - frsky_send_byte(uart, data >> 8); /* High */ + frsky_send_byte(uart, udata); /* LSB */ + frsky_send_byte(uart, udata >> 8); /* MSB */ } /** @@ -133,70 +178,96 @@ void frsky_send_frame1(int uart) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* send formatted frame */ - // TODO - frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 100); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 100); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 100); + frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f); frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, (raw.baro_alt_meter - (int)raw.baro_alt_meter) * 1000.0f); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); - frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius); + frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); frsky_send_data(uart, FRSKY_ID_TEMP2, 0); frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); - frsky_send_data(uart, FRSKY_ID_VOLTS_BP, battery.voltage_v); - frsky_send_data(uart, FRSKY_ID_VOLTS_AP, (battery.voltage_v - (int)battery.voltage_v) * 1000.0f); + float voltage = battery.voltage_v * 11.0f / 21.0f; + frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage); + frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f); frsky_send_data(uart, FRSKY_ID_RPM, 0); frsky_send_startstop(uart); } +/** + * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. + */ +float frsky_format_gps(float dec) +{ + float dms_deg = (int)dec; + float dec_deg = dec - dms_deg; + float dms_min = (int)(dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; + + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); +} + /** * Sends frame 2 (every 1000ms): * course, latitude, longitude, speed, altitude (GPS), fuel level */ void frsky_send_frame2(int uart) { - /* get a local copy of the battery data */ + /* get a local copy of the global position data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + /* send formatted frame */ - // TODO float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + char lat_ns = 0, lon_ew = 0; if (global_pos.valid) { + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - // TODO: latitude, longitude - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - alt = global_pos.alt; + lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; } frsky_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); - frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, (course - (int)course) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, 0); - frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, 0); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + frsky_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); frsky_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); - frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, (speed - (int)speed) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); - frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, (alt - (int)alt) * 100.0f); + frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); - frsky_send_data(uart, FRSKY_ID_FUEL, 0); + frsky_send_data(uart, FRSKY_ID_FUEL, vehicle_status.battery_remaining); - frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); frsky_send_startstop(uart); } @@ -213,11 +284,13 @@ void frsky_send_frame3(int uart) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* send formatted frame */ - // TODO - frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, 0); - frsky_send_data(uart, FRSKY_ID_GPS_YEAR, 0); - frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, 0); - frsky_send_data(uart, FRSKY_ID_GPS_SEC, 0); + time_t time_gps = global_pos.time_gps_usec / 1000000; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + frsky_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + frsky_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + frsky_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); frsky_send_startstop(uart); } diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index e0c39a42b..cea93ef2b 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -42,39 +42,6 @@ #ifndef _FRSKY_DATA_H #define _FRSKY_DATA_H -// FrSky sensor hub data IDs -#define FRSKY_ID_GPS_ALT_BP 0x01 -#define FRSKY_ID_TEMP1 0x02 -#define FRSKY_ID_RPM 0x03 -#define FRSKY_ID_FUEL 0x04 -#define FRSKY_ID_TEMP2 0x05 -#define FRSKY_ID_VOLTS 0x06 -#define FRSKY_ID_GPS_ALT_AP 0x09 -#define FRSKY_ID_BARO_ALT_BP 0x10 -#define FRSKY_ID_GPS_SPEED_BP 0x11 -#define FRSKY_ID_GPS_LONG_BP 0x12 -#define FRSKY_ID_GPS_LAT_BP 0x13 -#define FRSKY_ID_GPS_COURS_BP 0x14 -#define FRSKY_ID_GPS_DAY_MONTH 0x15 -#define FRSKY_ID_GPS_YEAR 0x16 -#define FRSKY_ID_GPS_HOUR_MIN 0x17 -#define FRSKY_ID_GPS_SEC 0x18 -#define FRSKY_ID_GPS_SPEED_AP 0x19 -#define FRSKY_ID_GPS_LONG_AP 0x1A -#define FRSKY_ID_GPS_LAT_AP 0x1B -#define FRSKY_ID_GPS_COURS_AP 0x1C -#define FRSKY_ID_BARO_ALT_AP 0x21 -#define FRSKY_ID_GPS_LONG_EW 0x22 -#define FRSKY_ID_GPS_LAT_NS 0x23 -#define FRSKY_ID_ACCEL_X 0x24 -#define FRSKY_ID_ACCEL_Y 0x25 -#define FRSKY_ID_ACCEL_Z 0x26 -#define FRSKY_ID_CURRENT 0x28 -#define FRSKY_ID_VARIO 0x30 -#define FRSKY_ID_VFAS 0x39 -#define FRSKY_ID_VOLTS_BP 0x3A -#define FRSKY_ID_VOLTS_AP 0x3B - // Public functions void frsky_init(void); void frsky_send_frame1(int uart); -- cgit v1.2.3 From 5f44be31ad77618d0a7514d129f41666a956a52d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:07:49 +0100 Subject: Update copyright info for 2014. Happy New Year everyone! --- src/drivers/frsky_telemetry/frsky_data.c | 4 ++-- src/drivers/frsky_telemetry/frsky_data.h | 4 ++-- src/drivers/frsky_telemetry/frsky_telemetry.c | 4 ++-- src/drivers/frsky_telemetry/module.mk | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 3a0c4b4b3..9a7b6beef 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/frsky_data.h b/src/drivers/frsky_telemetry/frsky_data.h index cea93ef2b..a7d9eee53 100644 --- a/src/drivers/frsky_telemetry/frsky_data.h +++ b/src/drivers/frsky_telemetry/frsky_data.h @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 5f3837b6e..c4e29fe51 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk index 808966691..1632c74f7 100644 --- a/src/drivers/frsky_telemetry/module.mk +++ b/src/drivers/frsky_telemetry/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions -- cgit v1.2.3 From 8fd909f51911b9ede93b19188ed360231f75de1c Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:08:44 +0100 Subject: Directly write to the voltage field for better precision. --- src/drivers/frsky_telemetry/frsky_data.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 9a7b6beef..edbdbf366 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -191,9 +191,7 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); - float voltage = battery.voltage_v * 11.0f / 21.0f; - frsky_send_data(uart, FRSKY_ID_VOLTS_BP, voltage); - frsky_send_data(uart, FRSKY_ID_VOLTS_AP, frac(voltage) * 10.0f); + frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); frsky_send_data(uart, FRSKY_ID_RPM, 0); -- cgit v1.2.3 From 1e7e65717a4522de59957d20be2b06ccc7b5a71d Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 02:11:52 +0100 Subject: Only send data packets we really support. --- src/drivers/frsky_telemetry/frsky_data.c | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index edbdbf366..9e6d57fac 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -186,14 +186,9 @@ void frsky_send_frame1(int uart) frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); - frsky_send_data(uart, FRSKY_ID_TEMP2, 0); - - frsky_send_data(uart, FRSKY_ID_VOLTS, 0); /* cell voltage. 4 bits cell number, 12 bits voltage in 0.2V steps, scale 0-4.2V */ - frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); - - frsky_send_data(uart, FRSKY_ID_RPM, 0); + frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); frsky_send_startstop(uart); } -- cgit v1.2.3 From 85651218e28968161890ddcae1c670f08774c285 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 10:22:00 +0100 Subject: FMU-inspired PPM config --- src/drivers/boards/px4io-v1/board_config.h | 2 +- src/drivers/boards/px4io-v2/board_config.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 48aadbd76..d136a3dd8 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -92,4 +92,4 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN GPIO_TIM1_CH1IN +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 4d41d0d07..3de6ddbd3 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -122,7 +122,7 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN GPIO_TIM1_CH1IN +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) /* LED definitions ******************************************************************/ /* PX4 has two LEDs that we will encode as: */ -- cgit v1.2.3 From 445b9b2339fe701a4234765bf49022c7d812d922 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 10:45:00 +0100 Subject: Final pin config for F1 PPM decoding, tested to be operational, pending in-application testing --- src/drivers/boards/px4io-v1/board_config.h | 2 +- src/drivers/boards/px4io-v2/board_config.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index d136a3dd8..a5fbfc7fd 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -92,4 +92,4 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 3de6ddbd3..b6e298411 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -122,7 +122,7 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_AFPU|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) /* LED definitions ******************************************************************/ /* PX4 has two LEDs that we will encode as: */ -- cgit v1.2.3 From 6d08e9f6612ef0576e0064764e47817695e02c7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:09:33 +0100 Subject: HOTFIX: Avoid running out of range on the RC config params --- src/drivers/drv_rc_input.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 03f1dfbe5..6b87141e9 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -60,7 +60,7 @@ /** * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 20 +#define RC_INPUT_MAX_CHANNELS 18 /** * Input signal type, value is a control position from zero to 100 -- cgit v1.2.3 From 07fa4e3ec8474041f8c373c2b47f002bfdbb4788 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:26:57 +0100 Subject: Removed bogus 50 MHz setting, only relevant for outputs --- src/drivers/boards/px4io-v1/board_config.h | 4 ++-- src/drivers/boards/px4io-v2/board_config.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index a5fbfc7fd..c3f39addf 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -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 @@ -92,4 +92,4 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index b6e298411..47ec4afe3 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -122,7 +122,7 @@ #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 2 /* use capture/compare channel 2 */ #define HRT_PPM_CHANNEL 1 /* use capture/compare channel 1 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_MODE_50MHz|GPIO_PORTE|GPIO_PIN9) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTE|GPIO_PIN9) /* LED definitions ******************************************************************/ /* PX4 has two LEDs that we will encode as: */ -- cgit v1.2.3 From c11e36ad3deb110b18b43e09ffa4f240e36579df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 2 Jan 2014 17:42:01 +0100 Subject: Board config sweep / cleanup. No further functionality-relevant points found --- src/drivers/boards/px4fmu-v1/board_config.h | 6 +++--- src/drivers/boards/px4fmu-v2/board_config.h | 26 +++++++++++++------------- src/drivers/boards/px4io-v2/board_config.h | 2 +- 3 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 27621211a..6f7166284 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -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 @@ -34,7 +34,7 @@ /** * @file board_config.h * - * PX4FMU internal definitions + * PX4FMUv1 internal definitions */ #pragma once @@ -180,7 +180,7 @@ __BEGIN_DECLS #define HRT_TIMER 1 /* use timer1 for the HRT */ #define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ #define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */ -#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_SPEED_50MHz|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) +#define GPIO_PPM_IN (GPIO_ALT|GPIO_AF1|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN10) /**************************************************************************************************** * Public Types diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 7ab63953f..a19ed9d24 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 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 @@ -34,7 +34,7 @@ /** * @file board_config.h * - * PX4FMU internal definitions + * PX4FMUv2 internal definitions */ #pragma once @@ -82,21 +82,21 @@ __BEGIN_DECLS #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* Data ready pins off */ -#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) -#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) -#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) +#define GPIO_GYRO_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN0) +#define GPIO_MAG_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN1) +#define GPIO_ACCEL_DRDY_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTB|GPIO_PIN4) #define GPIO_EXTI_MPU_DRDY (GPIO_INPUT|GPIO_PULLDOWN|GPIO_EXTI|GPIO_PORTD|GPIO_PIN15) /* SPI1 off */ -#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) -#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6) -#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7) +#define GPIO_SPI1_SCK_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN5) +#define GPIO_SPI1_MISO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN6) +#define GPIO_SPI1_MOSI_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTA|GPIO_PIN7) /* SPI1 chip selects off */ -#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) -#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) -#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) -#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPI_CS_ACCEL_MAG_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN15) +#define GPIO_SPI_CS_BARO_OFF (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTD|GPIO_PIN7) +#define GPIO_SPI_CS_MPU (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_2MHz|GPIO_PORTC|GPIO_PIN2) /* SPI chip selects */ #define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) @@ -177,7 +177,7 @@ __BEGIN_DECLS * * PA9 OTG_FS_VBUS VBUS sensing (also connected to the green LED) */ -#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) +#define GPIO_OTGFS_VBUS (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_100MHz|GPIO_OPENDRAIN|GPIO_PORTA|GPIO_PIN9) /* High-resolution timer */ #define HRT_TIMER 8 /* use timer8 for the HRT */ diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 47ec4afe3..8da555211 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file px4iov2_internal.h + * @file board_config.h * * PX4IOV2 internal definitions */ -- cgit v1.2.3 From 4508972121196d8892520e56e6b374a59281e50a Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Thu, 2 Jan 2014 22:34:08 +0100 Subject: Further data format and code style fixes. --- src/drivers/frsky_telemetry/frsky_data.c | 96 +++++++++++++-------------- src/drivers/frsky_telemetry/frsky_telemetry.c | 19 +++--- 2 files changed, 56 insertions(+), 59 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 9e6d57fac..63b2d2d29 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -53,8 +53,7 @@ #include #include - -// FrSky sensor hub data IDs +/* FrSky sensor hub data IDs */ #define FRSKY_ID_GPS_ALT_BP 0x01 #define FRSKY_ID_TEMP1 0x02 #define FRSKY_ID_RPM 0x03 @@ -87,18 +86,13 @@ #define FRSKY_ID_VOLTS_BP 0x3A #define FRSKY_ID_VOLTS_AP 0x3B - #define frac(f) (f - (int)f) -float frsky_format_gps(float dec); - - static int battery_sub = -1; static int sensor_sub = -1; static int global_position_sub = -1; static int vehicle_status_sub = -1; - /** * Initializes the uORB subscriptions. */ @@ -113,8 +107,7 @@ void frsky_init() /** * Sends a 0x5E start/stop byte. */ -static void -frsky_send_startstop(int uart) +static void frsky_send_startstop(int uart) { static const uint8_t c = 0x5E; write(uart, &c, sizeof(c)); @@ -123,14 +116,12 @@ frsky_send_startstop(int uart) /** * Sends one byte, performing byte-stuffing if necessary. */ -static void -frsky_send_byte(int uart, uint8_t value) +static void frsky_send_byte(int uart, uint8_t value) { - const uint8_t x5E[] = {0x5D, 0x3E}; - const uint8_t x5D[] = {0x5D, 0x3D}; + const uint8_t x5E[] = { 0x5D, 0x3E }; + const uint8_t x5D[] = { 0x5D, 0x3D }; - switch (value) - { + switch (value) { case 0x5E: write(uart, x5E, sizeof(x5E)); break; @@ -148,10 +139,9 @@ frsky_send_byte(int uart, uint8_t value) /** * Sends one data id/value pair. */ -static void -frsky_send_data(int uart, uint8_t id, int16_t data) +static void frsky_send_data(int uart, uint8_t id, int16_t data) { - // Cast data to unsigned, because signed shift might behave incorrectly + /* Cast data to unsigned, because signed shift might behave incorrectly */ uint16_t udata = data; frsky_send_startstop(uart); @@ -163,7 +153,7 @@ frsky_send_data(int uart, uint8_t id, int16_t data) /** * Sends frame 1 (every 200ms): - * acceleration values, altitude (vario), temperatures, current & voltages, RPM + * acceleration values, barometer altitude, temperature, battery voltage & current */ void frsky_send_frame1(int uart) { @@ -178,17 +168,25 @@ void frsky_send_frame1(int uart) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* send formatted frame */ - frsky_send_data(uart, FRSKY_ID_ACCEL_X, raw.accelerometer_m_s2[0] * 1000.0f); - frsky_send_data(uart, FRSKY_ID_ACCEL_Y, raw.accelerometer_m_s2[1] * 1000.0f); - frsky_send_data(uart, FRSKY_ID_ACCEL_Z, raw.accelerometer_m_s2[2] * 1000.0f); - - frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, raw.baro_alt_meter); - frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, frac(raw.baro_alt_meter) * 1000.0f); - - frsky_send_data(uart, FRSKY_ID_TEMP1, raw.baro_temp_celcius * 10.0f); - - frsky_send_data(uart, FRSKY_ID_VFAS, battery.voltage_v * 10.0f); - frsky_send_data(uart, FRSKY_ID_CURRENT, battery.current_a); + frsky_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + frsky_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); + + frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter); + frsky_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter) * 100.0f)); + + frsky_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius)); + + frsky_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + frsky_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); frsky_send_startstop(uart); } @@ -196,47 +194,48 @@ void frsky_send_frame1(int uart) /** * Formats the decimal latitude/longitude to the required degrees/minutes/seconds. */ -float frsky_format_gps(float dec) +static float frsky_format_gps(float dec) { - float dms_deg = (int)dec; - float dec_deg = dec - dms_deg; - float dms_min = (int)(dec_deg * 60); - float dec_min = (dec_deg * 60) - dms_min; - float dms_sec = dec_min * 60; + float dms_deg = (int) dec; + float dec_deg = dec - dms_deg; + float dms_min = (int) (dec_deg * 60); + float dec_min = (dec_deg * 60) - dms_min; + float dms_sec = dec_min * 60; - return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); + return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f); } /** * Sends frame 2 (every 1000ms): - * course, latitude, longitude, speed, altitude (GPS), fuel level + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level */ void frsky_send_frame2(int uart) { - /* get a local copy of the global position data */ + /* get a local copy of the global position data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - /* get a local copy of the vehicle status data */ + /* get a local copy of the vehicle status data */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); /* send formatted frame */ - float course = 0, lat = 0, lon = 0, speed = 0, alt = 0, sec = 0; + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; - if (global_pos.valid) - { + int sec = 0; + if (global_pos.valid) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); - lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) * 25.0f / 46.0f; + speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) + * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; } @@ -258,7 +257,8 @@ void frsky_send_frame2(int uart) frsky_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); frsky_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); - frsky_send_data(uart, FRSKY_ID_FUEL, vehicle_status.battery_remaining); + frsky_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); frsky_send_data(uart, FRSKY_ID_GPS_SEC, sec); @@ -267,11 +267,11 @@ void frsky_send_frame2(int uart) /** * Sends frame 3 (every 5000ms): - * date, time + * GPS date & time */ void frsky_send_frame3(int uart) { - /* get a local copy of the battery data */ + /* get a local copy of the battery data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index c4e29fe51..7b08ca69e 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -72,8 +72,7 @@ __EXPORT int frsky_telemetry_main(int argc, char *argv[]); /** * Opens the UART device and sets all required serial parameters. */ -static int -frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) { /* Open UART */ const int uart = open(uart_name, O_WRONLY | O_NOCTTY); @@ -118,20 +117,19 @@ frsky_open_uart(const char *uart_name, struct termios *uart_config_original) /** * Print command usage information */ -static void -usage() +static void usage() { - fprintf(stderr, "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + fprintf(stderr, + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); exit(1); } /** * The daemon thread. */ -static int -frsky_telemetry_thread_main(int argc, char *argv[]) +static int frsky_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ char *device_name = "/dev/ttyS1"; /* USART2 */ @@ -207,8 +205,7 @@ frsky_telemetry_thread_main(int argc, char *argv[]) * The main command function. * Processes command line arguments and starts the daemon. */ -int -frsky_telemetry_main(int argc, char *argv[]) +int frsky_telemetry_main(int argc, char *argv[]) { if (argc < 2) { warnx("missing command"); -- cgit v1.2.3 From 93a0c2de56e19cb01477a4c99aae4c33707e144b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jan 2014 13:51:20 +0100 Subject: Trimming down number of apps to save flash space --- makefiles/config_px4fmu-v1_backside.mk | 15 --------------- makefiles/config_px4fmu-v1_default.mk | 10 ++++------ 2 files changed, 4 insertions(+), 21 deletions(-) diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk index 5ecf93a3a..c86beacca 100644 --- a/makefiles/config_px4fmu-v1_backside.mk +++ b/makefiles/config_px4fmu-v1_backside.mk @@ -25,14 +25,10 @@ MODULES += drivers/bma180 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled -MODULES += drivers/mkblctrl MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed @@ -71,22 +67,11 @@ MODULES += modules/gpio_led # # Estimation modules (EKF/ SO3 / other filters) # -#MODULES += modules/attitude_estimator_ekf MODULES += modules/att_pos_estimator_ekf -#MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator -MODULES += modules/attitude_estimator_so3 # # Vehicle Control # -#MODULES += modules/segway # XXX Needs GCC 4.7 fix -#MODULES += modules/fw_pos_control_l1 -#MODULES += modules/fw_att_control -#MODULES += modules/multirotor_att_control -#MODULES += modules/multirotor_pos_control -#MODULES += examples/flow_position_control -#MODULES += examples/flow_speed_control MODULES += modules/fixedwing_backside # diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 25c22f1fd..e76967702 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -21,7 +21,7 @@ MODULES += drivers/px4fmu MODULES += drivers/boards/px4fmu-v1 MODULES += drivers/ardrone_interface MODULES += drivers/l3gd20 -MODULES += drivers/bma180 +#MODULES += drivers/bma180 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 @@ -33,7 +33,6 @@ MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl -MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed @@ -75,18 +74,17 @@ MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 MODULES += modules/att_pos_estimator_ekf MODULES += modules/position_estimator_inav -MODULES += examples/flow_position_estimator +#MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX Needs GCC 4.7 fix MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control MODULES += modules/multirotor_pos_control -MODULES += examples/flow_position_control -MODULES += examples/flow_speed_control +#MODULES += examples/flow_position_control +#MODULES += examples/flow_speed_control # # Logging -- cgit v1.2.3 From ba4f881f807e75edc75d2ad2084dd1ebbbec3802 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Jan 2014 01:49:21 +0100 Subject: 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 9fd2dd516..bb729e103 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 # -- cgit v1.2.3 From 76477b205775ddcbce6147d93985ddcdb10a3d52 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 6 Jan 2014 09:25:39 +0100 Subject: 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 f122921c5..66cb3f237 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 -- cgit v1.2.3 From 1a13e66aab3cd88b5447448b577fc44165ab01bc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 15:59:18 +0800 Subject: 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 | 15 +++++---------- 3 files changed, 30 insertions(+), 10 deletions(-) diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 745bd5705..0b8c4a6a8 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -125,6 +125,25 @@ heartbeat_blink(void) LED_BLUE(heartbeat = !heartbeat); } +static uint64_t reboot_time; + +/** + schedule a reboot in time_delta_usec microseconds + */ +void schedule_reboot(uint32_t time_delta_usec) +{ + reboot_time = hrt_absolute_time() + time_delta_usec; +} + +/** + check for a scheduled reboot + */ +static void check_reboot(void) +{ + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } +} static void calculate_fw_crc(void) @@ -249,6 +268,8 @@ user_start(int argc, char *argv[]) heartbeat_blink(); } + check_reboot(); + #if 0 /* check for debug activity */ show_debug_messages(); diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index dea04a663..a0daa97ea 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -220,3 +220,7 @@ extern volatile uint8_t debug_level; /** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); + +/** schedule a reboot */ +extern void schedule_reboot(uint32_t time_delta_usec); + diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0358725db..ad4473073 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: -- cgit v1.2.3 From c4fc730acad12b74f51d9ba7d3ff267e3e1a1ab3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:13:51 +0800 Subject: 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 110bcb363..e20411bca 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" -- cgit v1.2.3 From d4d2571161530848f2a4ac153f2529ab50ec4fcc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:25:09 +0800 Subject: 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 e20411bca..8a282693f 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 -- cgit v1.2.3 From 1f564a95ee89278b3e4eea6c2d4ded378b71542c Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 7 Jan 2014 11:37:40 +0800 Subject: 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 3cd6d6720..a95c4576b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -198,7 +198,9 @@ MEASAirspeed::collect() // uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + if (diff_press_pa < 0.0f) + diff_press_pa = 0.0f; struct differential_pressure_s report; -- cgit v1.2.3 From 3be1a5182db7bd3802b77e7c03fc14f00ca218c3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 22:39:51 +1100 Subject: 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 e43b9c18e..e60120b49 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" -- cgit v1.2.3 From d6088efd34e5482d302e3a253fdd3a170d88490a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 31 Dec 2013 10:32:15 +1100 Subject: 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 87788824a..6326cf7fc 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -124,6 +124,8 @@ protected: int32_t _TEMP; int64_t _OFF; int64_t _SENS; + float _P; + float _T; /* altitude conversion calibration */ unsigned _msl_pressure; /* in kPa */ @@ -623,6 +625,8 @@ MS5611::collect() /* pressure calculation, result in Pa */ int32_t P = (((raw * _SENS) >> 21) - _OFF) >> 15; + _P = P * 0.01f; + _T = _TEMP * 0.01f; /* generate a new report */ report.temperature = _TEMP / 100.0f; @@ -695,6 +699,8 @@ MS5611::print_info() printf("TEMP: %d\n", _TEMP); printf("SENS: %lld\n", _SENS); printf("OFF: %lld\n", _OFF); + printf("P: %.3f\n", _P); + printf("T: %.3f\n", _T); printf("MSL pressure: %10.4f\n", (double)(_msl_pressure / 100.f)); printf("factory_setup %u\n", _prom.factory_setup); -- cgit v1.2.3 From 94b539dfddc5a2e293f51058ee5bf0d6ffc78406 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 18 Sep 2013 10:30:09 +1000 Subject: 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 4d306d6d0..60eda2319 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -203,6 +203,12 @@ dsm_guess_format(bool reset) int dsm_init(const char *device) { + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // enable power on DSM connector + POWER_SPEKTRUM(true); +#endif + if (dsm_fd < 0) dsm_fd = open(device, O_RDONLY | O_NONBLOCK); -- cgit v1.2.3 From 4ef7817d965ec77c04acd4e4173bb6051e7d6836 Mon Sep 17 00:00:00 2001 From: Buzz Date: Fri, 27 Sep 2013 15:10:37 +1000 Subject: 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 843cda722..1749ec9c7 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 000000000..7968637de --- /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 000000000..e80ca9afb --- /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 -- cgit v1.2.3 From 0ef85c133b387f5d5aab26e00985922c9f05c7e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:41:07 +0100 Subject: OTP return value cleanup --- src/modules/systemlib/otp.c | 223 +++++++++++++++++++++++++------------------- src/modules/systemlib/otp.h | 107 ++++++++++----------- 2 files changed, 179 insertions(+), 151 deletions(-) diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 7968637de..695574fdc 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 ) ; - - //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; + volatile int 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 == F_COMPLETE); } - \ No newline at end of file diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h index e80ca9afb..f10e129d8 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_lock { - uint8_t lock_bytes[16]; - }; +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]; - }; +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 -- cgit v1.2.3 From ea4552a53d5bced405b83e455ded691d62bc7fb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:41:54 +0100 Subject: 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 000000000..ad8c2bf83 --- /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 000000000..b14bb4376 --- /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 -- cgit v1.2.3 From 72b9d3a0b1084e8ae9216edf95055ae5e0cb5fb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:42:17 +0100 Subject: 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 6f7166284..02c26b5c0 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 a19ed9d24..4972e6914 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 -- cgit v1.2.3 From 1834a884b2abab0b38612c8d69f15156d0ef9d14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:42:39 +0100 Subject: 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 b878d29bc..b28d318d7 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); } -- cgit v1.2.3 From c463fde0b95eb07a5f2792032d24fbda8626b808 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jan 2014 21:42:51 +0100 Subject: 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 1749ec9c7..8c6c300d6 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 -- cgit v1.2.3