aboutsummaryrefslogtreecommitdiff
path: root/src/drivers
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers')
-rw-r--r--src/drivers/airspeed/airspeed.cpp2
-rw-r--r--src/drivers/boards/px4fmu-v2/board_config.h2
-rw-r--r--src/drivers/boards/px4io-v1/board_config.h6
-rw-r--r--src/drivers/boards/px4io-v2/board_config.h8
-rw-r--r--src/drivers/boards/px4io-v2/px4iov2_init.c2
-rw-r--r--src/drivers/drv_pwm_output.h4
-rw-r--r--src/drivers/drv_rc_input.h54
-rw-r--r--src/drivers/drv_sbus.h58
-rw-r--r--src/drivers/gps/gps.cpp2
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp160
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp22
-rw-r--r--src/drivers/px4fmu/fmu.cpp47
-rw-r--r--src/drivers/px4io/px4io.cpp253
-rw-r--r--src/drivers/px4io/px4io_uploader.cpp17
-rw-r--r--src/drivers/px4io/uploader.h2
15 files changed, 485 insertions, 154 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 5e45cc936..f73a3ef01 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -91,7 +91,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
{
// enable debug() calls
- _debug_enabled = true;
+ _debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index 264d911f3..7cfca7656 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -75,7 +75,7 @@ __BEGIN_DECLS
/* PX4FMU GPIOs ***********************************************************************************/
/* LEDs */
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN12)
/* External interrupts */
#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN0)
diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h
index c3f39addf..1be4877ba 100644
--- a/src/drivers/boards/px4io-v1/board_config.h
+++ b/src/drivers/boards/px4io-v1/board_config.h
@@ -58,11 +58,11 @@
/* PX4IO GPIOs **********************************************************************/
/* LEDs */
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\
GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
/* Safety switch button *************************************************************/
diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h
index 8da555211..ef9bb5cad 100644
--- a/src/drivers/boards/px4io-v2/board_config.h
+++ b/src/drivers/boards/px4io-v2/board_config.h
@@ -74,9 +74,9 @@
/* LEDS **********************************************************************/
-#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
-#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
-#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
+#define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14)
+#define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15)
+#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
/* Safety switch button *******************************************************/
@@ -114,7 +114,7 @@
/* XXX these should be UART pins */
#define GPIO_SBUS_INPUT (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN11)
#define GPIO_SBUS_OUTPUT (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
-#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN4)
+#define GPIO_SBUS_OENABLE (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN4)
/*
* High-resolution timer
diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c
index ccd01edf5..9f8c0eeb2 100644
--- a/src/drivers/boards/px4io-v2/px4iov2_init.c
+++ b/src/drivers/boards/px4io-v2/px4iov2_init.c
@@ -124,8 +124,6 @@ __EXPORT void stm32_boardinitialize(void)
stm32_configgpio(GPIO_ADC_VSERVO);
stm32_configgpio(GPIO_SBUS_INPUT); /* xxx alternate function */
-
- stm32_gpiowrite(GPIO_SBUS_OUTPUT, false);
stm32_configgpio(GPIO_SBUS_OUTPUT);
/* sbus output enable is active low - disable it by default */
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 53065f8eb..c3eea310f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -189,6 +189,10 @@ ORB_DECLARE(output_pwm);
/** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19)
+/** set the number of servos in (unsigned)arg - allows change of
+ * split between servos and GPIO */
+#define PWM_SERVO_SET_COUNT _IOC(_PWM_SERVO_BASE, 20)
+
/** set the lockdown override flag to enable outputs in HIL */
#define PWM_SERVO_SET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 21)
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 6b87141e9..20763e265 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -63,6 +63,11 @@
#define RC_INPUT_MAX_CHANNELS 18
/**
+ * Maximum RSSI value
+ */
+#define RC_INPUT_RSSI_MAX 255
+
+/**
* Input signal type, value is a control position from zero to 100
* percent.
*/
@@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE {
* on the board involved.
*/
struct rc_input_values {
- /** decoding time */
- uint64_t timestamp;
+ /** publication time */
+ uint64_t timestamp_publication;
+
+ /** last valid reception time */
+ uint64_t timestamp_last_signal;
/** number of channels actually being seen */
uint32_t channel_count;
@@ -92,6 +100,40 @@ struct rc_input_values {
/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */
int32_t rssi;
+ /**
+ * explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
+ * Only the true state is reliable, as there are some (PPM) receivers on the market going
+ * into failsafe without telling us explicitly.
+ * */
+ bool rc_failsafe;
+
+ /**
+ * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
+ * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
+ * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
+ * */
+ bool rc_lost;
+
+ /**
+ * Number of lost RC frames.
+ * Note: intended purpose: observe the radio link quality if RSSI is not available
+ * This value must not be used to trigger any failsafe-alike funtionality.
+ * */
+ uint16_t rc_lost_frame_count;
+
+ /**
+ * Number of total RC frames.
+ * Note: intended purpose: observe the radio link quality if RSSI is not available
+ * This value must not be used to trigger any failsafe-alike funtionality.
+ * */
+ uint16_t rc_total_frame_count;
+
+ /**
+ * Length of a single PPM frame.
+ * Zero for non-PPM systems
+ */
+ uint16_t rc_ppm_frame_length;
+
/** Input source */
enum RC_INPUT_SOURCE input_source;
@@ -107,8 +149,12 @@ ORB_DECLARE(input_rc);
#define _RC_INPUT_BASE 0x2b00
/** Fetch R/C input values into (rc_input_values *)arg */
-
#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0)
+/** Enable RSSI input via ADC */
+#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1)
+
+/** Enable RSSI input via PWM signal */
+#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2)
#endif /* _DRV_RC_INPUT_H */
diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h
new file mode 100644
index 000000000..927c904ec
--- /dev/null
+++ b/src/drivers/drv_sbus.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ * Copyright (c) 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 drv_sbus.h
+ *
+ * Futaba S.BUS / S.BUS 2 compatible interface.
+ */
+
+#ifndef _DRV_SBUS_H
+#define _DRV_SBUS_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_orb_dev.h"
+
+/**
+ * Path for the default S.BUS device
+ */
+#define SBUS_DEVICE_PATH "/dev/sbus"
+
+#define _SBUS_BASE 0x2c00
+
+/** Enable S.BUS version 1 / 2 output (0 to disable) */
+#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0)
+
+#endif /* _DRV_SBUS_H */
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6b72d560f..f2faf711b 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -209,7 +209,7 @@ GPS::init()
goto out;
/* start the GPS driver worker task */
- _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+ _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr);
if (_task < 0) {
warnx("task start failed: %d", errno);
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index d3b99ae66..9b9c11af2 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -849,42 +849,24 @@ HMC5883::collect()
/* scale values for output */
- /*
- * 1) Scale raw value to SI units using scaling from datasheet.
- * 2) Subtract static offset (in SI units)
- * 3) Scale the statically calibrated values with a linear
- * dynamically obtained factor
- *
- * Note: the static sensor offset is the number the sensor outputs
- * at a nominally 'zero' input. Therefore the offset has to
- * be subtracted.
- *
- * Example: A gyro outputs a value of 74 at zero angular rate
- * the offset is 74 from the origin and subtracting
- * 74 from all measurements centers them around zero.
- */
-
#ifdef PX4_I2C_BUS_ONBOARD
if (_bus == PX4_I2C_BUS_ONBOARD) {
- /* to align the sensor axes with the board, x and y need to be flipped */
- new_report.x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
- /* flip axes and negate value for y */
- new_report.y = ((-report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
- /* z remains z */
- new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
- } else {
-#endif
- /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
- * therefore switch x and y and invert y */
- new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
- /* flip axes and negate value for y */
- new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
- /* z remains z */
- new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
-#ifdef PX4_I2C_BUS_ONBOARD
- }
+ // convert onboard so it matches offboard for the
+ // scaling below
+ report.y = -report.y;
+ report.x = -report.x;
+ }
#endif
+ /* the standard external mag by 3DR has x pointing to the
+ * right, y pointing backwards, and z down, therefore switch x
+ * and y and invert y */
+ new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
+ /* flip axes and negate value for y */
+ new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
+ /* z remains z */
+ new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
+
if (_mag_topic != -1) {
/* publish it */
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
@@ -910,6 +892,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
struct mag_report report;
ssize_t sz;
int ret = 1;
+ uint8_t good_count = 0;
// XXX do something smarter here
int fd = (int)enable;
@@ -932,31 +915,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
1.0f,
};
- float avg_excited[3] = {0.0f, 0.0f, 0.0f};
- unsigned i;
+ float sum_excited[3] = {0.0f, 0.0f, 0.0f};
- warnx("starting mag scale calibration");
+ /* expected axis scaling. The datasheet says that 766 will
+ * be places in the X and Y axes and 713 in the Z
+ * axis. Experiments show that in fact 766 is placed in X,
+ * and 713 in Y and Z. This is relative to a base of 660
+ * LSM/Ga, giving 1.16 and 1.08 */
+ float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
- /* do a simple demand read */
- sz = read(filp, (char *)&report, sizeof(report));
-
- if (sz != sizeof(report)) {
- warn("immediate read failed");
- ret = 1;
- goto out;
- }
-
- warnx("current measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
- warnx("time: %lld", report.timestamp);
- warnx("sampling 500 samples for scaling offset");
-
- /* set the queue depth to 10 */
- /* don't do this for now, it can lead to a crash in start() respectively work_queue() */
-// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
-// warn("failed to set queue depth");
-// ret = 1;
-// goto out;
-// }
+ warnx("starting mag scale calibration");
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
@@ -965,8 +933,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out;
}
- /* Set to 2.5 Gauss */
- if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
+ /* Set to 2.5 Gauss. We ask for 3 to get the right part of
+ * the chained if statement above. */
+ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
warnx("failed to set 2.5 Ga range");
ret = 1;
goto out;
@@ -990,8 +959,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
goto out;
}
- /* read the sensor 10x and report each value */
- for (i = 0; i < 500; i++) {
+ // discard 10 samples to let the sensor settle
+ for (uint8_t i = 0; i < 10; i++) {
struct pollfd fds;
/* wait for data to be ready */
@@ -1009,32 +978,69 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
if (sz != sizeof(report)) {
warn("periodic read failed");
+ ret = -EIO;
goto out;
+ }
+ }
- } else {
- avg_excited[0] += report.x;
- avg_excited[1] += report.y;
- avg_excited[2] += report.z;
+ /* read the sensor up to 50x, stopping when we have 10 good values */
+ for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
+ struct pollfd fds;
+
+ /* wait for data to be ready */
+ fds.fd = fd;
+ fds.events = POLLIN;
+ ret = ::poll(&fds, 1, 2000);
+
+ if (ret != 1) {
+ warn("timed out waiting for sensor data");
+ goto out;
+ }
+
+ /* now go get it */
+ sz = ::read(fd, &report, sizeof(report));
+
+ if (sz != sizeof(report)) {
+ warn("periodic read failed");
+ ret = -EIO;
+ goto out;
+ }
+ float cal[3] = {fabsf(expected_cal[0] / report.x),
+ fabsf(expected_cal[1] / report.y),
+ fabsf(expected_cal[2] / report.z)};
+
+ if (cal[0] > 0.7f && cal[0] < 1.35f &&
+ cal[1] > 0.7f && cal[1] < 1.35f &&
+ cal[2] > 0.7f && cal[2] < 1.35f) {
+ good_count++;
+ sum_excited[0] += cal[0];
+ sum_excited[1] += cal[1];
+ sum_excited[2] += cal[2];
}
//warnx("periodic read %u", i);
//warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
+ //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
- avg_excited[0] /= i;
- avg_excited[1] /= i;
- avg_excited[2] /= i;
+ if (good_count < 5) {
+ warn("failed calibration");
+ ret = -EIO;
+ goto out;
+ }
- warnx("done. Performed %u reads", i);
- warnx("measurement avg: %.6f %.6f %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
+#if 0
+ warnx("measurement avg: %.6f %.6f %.6f",
+ (double)sum_excited[0]/good_count,
+ (double)sum_excited[1]/good_count,
+ (double)sum_excited[2]/good_count);
+#endif
float scaling[3];
- /* calculate axis scaling */
- scaling[0] = fabsf(1.16f / avg_excited[0]);
- /* second axis inverted */
- scaling[1] = fabsf(1.16f / -avg_excited[1]);
- scaling[2] = fabsf(1.08f / avg_excited[2]);
+ scaling[0] = sum_excited[0] / good_count;
+ scaling[1] = sum_excited[1] / good_count;
+ scaling[2] = sum_excited[2] / good_count;
warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
@@ -1165,6 +1171,8 @@ int HMC5883::set_excitement(unsigned enable)
conf_reg &= ~0x03;
}
+ // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)conf_reg);
+
ret = write_reg(ADDR_CONF_A, conf_reg);
if (OK != ret)
@@ -1173,6 +1181,8 @@ int HMC5883::set_excitement(unsigned enable)
uint8_t conf_reg_ret;
read_reg(ADDR_CONF_A, conf_reg_ret);
+ //print_info();
+
return !(conf_reg == conf_reg_ret);
}
@@ -1211,6 +1221,10 @@ HMC5883::print_info()
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _measure_ticks);
+ printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset);
+ printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n",
+ (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale,
+ (double)1.0/_range_scale, (double)_range_ga);
_reports->print_info("report queue");
}
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index a95c4576b..9251cff7b 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -77,7 +77,6 @@
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
-#include <mathlib/mathlib.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
@@ -178,24 +177,17 @@ MEASAirspeed::collect()
return ret;
}
- //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
- uint16_t temp = (val[3] & 0xE0) << 8 | val[2];
-
- // XXX leaving this in until new calculation method has been cross-checked
- //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
- //diff_pres_pa -= _diff_pres_offset;
int16_t dp_raw = 0, dT_raw = 0;
dp_raw = (val[0] << 8) + val[1];
- dp_raw = 0x3FFF & dp_raw; //mask the used bits
+ /* mask the used bits */
+ dp_raw = 0x3FFF & dp_raw;
dT_raw = (val[2] << 8) + val[3];
dT_raw = (0xFFE0 & dT_raw) >> 5;
float temperature = ((200 * dT_raw) / 2047) - 50;
- // XXX we may want to smooth out the readings to remove noise.
-
- // Calculate differential pressure. As its centered around 8000
- // and can go positive or negative, enforce absolute value
-// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+ /* calculate differential pressure. As its centered around 8000
+ * and can go positive or negative, enforce absolute value
+ */
const float P_min = -1.0f;
const float P_max = 1.0f;
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;
@@ -204,7 +196,7 @@ MEASAirspeed::collect()
struct differential_pressure_s report;
- // Track maximum differential pressure measured (so we can work out top speed).
+ /* track maximum differential pressure measured (so we can work out top speed). */
if (diff_press_pa > _max_differential_pressure_pa) {
_max_differential_pressure_pa = diff_press_pa;
}
@@ -392,7 +384,7 @@ test()
err(1, "immediate read failed");
warnx("single read");
- warnx("diff pressure: %d pa", report.differential_pressure_pa);
+ warnx("diff pressure: %d pa", (double)report.differential_pressure_pa);
/* start the sensor polling at 2Hz */
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4b1b0ed0b..0fbd84924 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -626,7 +626,7 @@ PX4FMU::task_main()
#ifdef HRT_PPM_CHANNEL
// see if we have new PPM input data
- if (ppm_last_valid_decode != rc_in.timestamp) {
+ if (ppm_last_valid_decode != rc_in.timestamp_last_signal) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
@@ -638,7 +638,15 @@ PX4FMU::task_main()
rc_in.values[i] = ppm_buffer[i];
}
- rc_in.timestamp = ppm_last_valid_decode;
+ rc_in.timestamp_publication = ppm_last_valid_decode;
+ rc_in.timestamp_last_signal = ppm_last_valid_decode;
+
+ rc_in.rc_ppm_frame_length = ppm_frame_length;
+ rc_in.rssi = RC_INPUT_RSSI_MAX;
+ rc_in.rc_failsafe = false;
+ rc_in.rc_lost = false;
+ rc_in.rc_lost_frame_count = 0;
+ rc_in.rc_total_frame_count = 0;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
@@ -1006,6 +1014,40 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
+ case PWM_SERVO_SET_COUNT: {
+ /* change the number of outputs that are enabled for
+ * PWM. This is used to change the split between GPIO
+ * and PWM under control of the flight config
+ * parameters. Note that this does not allow for
+ * changing a set of pins to be used for serial on
+ * FMUv1
+ */
+ switch (arg) {
+ case 0:
+ set_mode(MODE_NONE);
+ break;
+
+ case 2:
+ set_mode(MODE_2PWM);
+ break;
+
+ case 4:
+ set_mode(MODE_4PWM);
+ break;
+
+#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
+ case 6:
+ set_mode(MODE_6PWM);
+ break;
+#endif
+
+ default:
+ ret = -EINVAL;
+ break;
+ }
+ break;
+ }
+
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
@@ -1443,7 +1485,6 @@ void
sensor_reset(int ms)
{
int fd;
- int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 658bcd8d8..5da288661 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -61,6 +61,7 @@
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
+#include <drivers/drv_sbus.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
@@ -238,6 +239,7 @@ private:
unsigned _update_interval; ///< Subscription interval limiting send rate
bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values
unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels
+ uint64_t _rc_last_valid; ///< last valid timestamp
volatile int _task; ///< worker task id
volatile bool _task_should_exit; ///< worker terminate flag
@@ -270,7 +272,8 @@ private:
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
- actuator_outputs_s _outputs; ///<mixed outputs
+ actuator_outputs_s _outputs; ///< mixed outputs
+ servorail_status_s _servorail_status; ///< servorail status
bool _primary_pwm_device; ///< true if we are the default PWM output
bool _lockdown_override; ///< allow to override the safety lockdown
@@ -444,14 +447,14 @@ private:
* @param vservo vservo register
* @param vrssi vrssi register
*/
- void io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
+ void io_handle_vservo(uint16_t vservo, uint16_t vrssi);
};
namespace
{
-PX4IO *g_dev;
+PX4IO *g_dev = nullptr;
}
@@ -467,6 +470,7 @@ PX4IO::PX4IO(device::Device *interface) :
_update_interval(0),
_rc_handling_disabled(false),
_rc_chan_count(0),
+ _rc_last_valid(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
@@ -506,7 +510,8 @@ PX4IO::PX4IO(device::Device *interface) :
/* open MAVLink text channel */
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
- _debug_enabled = true;
+ _debug_enabled = false;
+ _servorail_status.rssi_v = 0;
}
PX4IO::~PX4IO()
@@ -580,6 +585,12 @@ PX4IO::init()
/* get some parameters */
unsigned protocol = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION);
+ if (protocol == _io_reg_get_error) {
+ log("failed to communicate with IO");
+ mavlink_log_emergency(_mavlink_fd, "[IO] failed to communicate with IO, abort.");
+ return -1;
+ }
+
if (protocol != PX4IO_PROTOCOL_VERSION) {
log("protocol/firmware mismatch");
mavlink_log_emergency(_mavlink_fd, "[IO] protocol/firmware mismatch, abort.");
@@ -750,7 +761,7 @@ PX4IO::init()
}
/* start the IO interface task */
- _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr);
+ _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
@@ -774,8 +785,6 @@ PX4IO::task_main()
hrt_abstime poll_last = 0;
hrt_abstime orb_check_last = 0;
- log("starting");
-
_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
/*
@@ -809,8 +818,6 @@ PX4IO::task_main()
fds[0].fd = _t_actuator_controls_0;
fds[0].events = POLLIN;
- log("ready");
-
/* lock against the ioctl handler */
lock();
@@ -1308,6 +1315,7 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
/* voltage is scaled to mV */
battery_status.voltage_v = vbatt / 1000.0f;
+ battery_status.voltage_filtered_v = vbatt / 1000.0f;
/*
ibatt contains the raw ADC count, as 12 bit ADC
@@ -1339,19 +1347,18 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
void
PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
{
- servorail_status_s servorail_status;
- servorail_status.timestamp = hrt_absolute_time();
+ _servorail_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
- servorail_status.voltage_v = vservo * 0.001f;
- servorail_status.rssi_v = vrssi * 0.001f;
+ _servorail_status.voltage_v = vservo * 0.001f;
+ _servorail_status.rssi_v = vrssi * 0.001f;
/* lazily publish the servorail voltages */
if (_to_servorail > 0) {
- orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
+ orb_publish(ORB_ID(servorail_status), _to_servorail, &_servorail_status);
} else {
- _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
+ _to_servorail = orb_advertise(ORB_ID(servorail_status), &_servorail_status);
}
}
@@ -1361,7 +1368,10 @@ PX4IO::io_get_status()
uint16_t regs[6];
int ret;
- /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+ /* get
+ * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT,
+ * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI
+ * in that order */
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
if (ret != OK)
@@ -1398,7 +1408,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
*
* This should be the common case (9 channel R/C control being a reasonable upper bound).
*/
- input_rc.timestamp = hrt_absolute_time();
+ input_rc.timestamp_publication = hrt_absolute_time();
+
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &regs[0], prolog + 9);
if (ret != OK)
@@ -1408,13 +1419,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
* Get the channel count any any extra channels. This is no more expensive than reading the
* channel count once.
*/
- channel_count = regs[0];
+ channel_count = regs[PX4IO_P_RAW_RC_COUNT];
if (channel_count != _rc_chan_count)
perf_count(_perf_chan_count);
_rc_chan_count = channel_count;
+ input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA];
+ input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI];
+ input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
+ input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT];
+ input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT];
+
+ /* rc_lost has to be set before the call to this function */
+ if (!input_rc.rc_lost && !input_rc.rc_failsafe)
+ _rc_last_valid = input_rc.timestamp_publication;
+
+ input_rc.timestamp_last_signal = _rc_last_valid;
+
if (channel_count > 9) {
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, &regs[prolog + 9], channel_count - 9);
@@ -1431,13 +1454,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
int
PX4IO::io_publish_raw_rc()
{
- /* if no raw RC, just don't publish */
- if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
- return OK;
/* fetch values from IO */
rc_input_values rc_val;
- rc_val.timestamp = hrt_absolute_time();
+
+ /* set the RC status flag ORDER MATTERS! */
+ rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK);
int ret = io_get_raw_rc_input(rc_val);
@@ -1456,6 +1478,11 @@ PX4IO::io_publish_raw_rc()
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
+
+ /* we do not know the RC input, only publish if RC OK flag is set */
+ /* if no raw RC, just don't publish */
+ if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
+ return OK;
}
/* lazily advertise on first publication */
@@ -1672,7 +1699,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
total_len++;
}
- int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+ int ret;
+
+ for (int i = 0; i < 30; i++) {
+ /* failed, but give it a 2nd shot */
+ ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
+
+ if (ret) {
+ usleep(333);
+ } else {
+ break;
+ }
+ }
/* print mixer chunk */
if (debuglevel > 5 || ret) {
@@ -1696,7 +1734,18 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
msg->text[0] = '\n';
msg->text[1] = '\0';
- int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+ int ret;
+
+ for (int i = 0; i < 30; i++) {
+ /* failed, but give it a 2nd shot */
+ ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+
+ if (ret) {
+ usleep(333);
+ } else {
+ break;
+ }
+ }
if (ret)
return ret;
@@ -1742,15 +1791,14 @@ PX4IO::print_status()
printf("%u bytes free\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
- printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
+ uint16_t io_status_flags = flags;
+ printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
- (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
- (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -1809,8 +1857,17 @@ PX4IO::print_status()
printf("\n");
- if (raw_inputs > 0) {
- int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA);
+ flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS);
+ printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags,
+ (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+ (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""),
+ ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""),
+ ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""),
+ ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "")
+ );
+
+ if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) {
+ int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA);
printf("RC data (PPM frame len) %u us\n", frame_len);
if ((frame_len - raw_inputs * 2000 - 3000) < 0) {
@@ -1836,7 +1893,13 @@ PX4IO::print_status()
printf("\n");
/* setup and state */
- printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
+ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES);
+ printf("features 0x%04x%s%s%s%s\n", features,
+ ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""),
+ ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""),
+ ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""),
+ ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
+ );
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s%s%s%s\n",
arming,
@@ -2267,6 +2330,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break;
+ case RC_INPUT_ENABLE_RSSI_ANALOG:
+
+ if (arg) {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+ } else {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0);
+ }
+
+ break;
+
+ case RC_INPUT_ENABLE_RSSI_PWM:
+
+ if (arg) {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI);
+ } else {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0);
+ }
+
+ break;
+
+ case SBUS_SET_PROTO_VERSION:
+
+ if (arg == 1) {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
+ } else if (arg == 2) {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+ } else {
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0);
+ }
+
+ break;
+
default:
/* not a recognized value */
ret = -ENOTTY;
@@ -2375,8 +2470,10 @@ start(int argc, char *argv[])
/* create the driver - it will set g_dev */
(void)new PX4IO(interface);
- if (g_dev == nullptr)
+ if (g_dev == nullptr) {
+ delete interface;
errx(1, "driver alloc failed");
+ }
if (OK != g_dev->init()) {
delete g_dev;
@@ -2638,7 +2735,7 @@ monitor(void)
/* clear screen */
printf("\033[2J");
- unsigned cancels = 3;
+ unsigned cancels = 2;
for (;;) {
pollfd fds[1];
@@ -2652,17 +2749,17 @@ monitor(void)
read(0, &c, 1);
if (cancels-- == 0) {
- printf("\033[H"); /* move cursor home and clear screen */
+ printf("\033[2J\033[H"); /* move cursor home and clear screen */
exit(0);
}
}
if (g_dev != nullptr) {
- printf("\033[H"); /* move cursor home and clear screen */
+ printf("\033[2J\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_debug();
- printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
+ printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
} else {
errx(1, "driver not loaded, exiting");
@@ -2764,6 +2861,7 @@ px4io_main(int argc, char *argv[])
printf("[px4io] loaded, detaching first\n");
/* stop the driver */
delete g_dev;
+ g_dev = nullptr;
}
PX4IO_Uploader *up;
@@ -2836,18 +2934,30 @@ px4io_main(int argc, char *argv[])
}
if (g_dev == nullptr) {
warnx("px4io is not started, still attempting upgrade");
- } else {
- uint16_t arg = atol(argv[2]);
- int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
- if (ret != OK) {
- printf("reboot failed - %d\n", ret);
- exit(1);
+
+ /* allocate the interface */
+ device::Device *interface = get_interface();
+
+ /* create the driver - it will set g_dev */
+ (void)new PX4IO(interface);
+
+ if (g_dev == nullptr) {
+ delete interface;
+ errx(1, "driver alloc failed");
}
+ }
- // tear down the px4io instance
- delete g_dev;
+ uint16_t arg = atol(argv[2]);
+ int ret = g_dev->ioctl(nullptr, PX4IO_REBOOT_BOOTLOADER, arg);
+ if (ret != OK) {
+ printf("reboot failed - %d\n", ret);
+ exit(1);
}
+ // tear down the px4io instance
+ delete g_dev;
+ g_dev = nullptr;
+
// upload the specified firmware
const char *fn[2];
fn[0] = argv[3];
@@ -2905,6 +3015,7 @@ px4io_main(int argc, char *argv[])
/* stop the driver */
delete g_dev;
+ g_dev = nullptr;
exit(0);
}
@@ -2962,6 +3073,60 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "lockdown"))
lockdown(argc, argv);
+ if (!strcmp(argv[1], "sbus1_out")) {
+ /* we can cheat and call the driver directly, as it
+ * doesn't reference filp in ioctl()
+ */
+ int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1);
+
+ if (ret != 0) {
+ errx(ret, "S.BUS v1 failed");
+ }
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "sbus2_out")) {
+ /* we can cheat and call the driver directly, as it
+ * doesn't reference filp in ioctl()
+ */
+ int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2);
+
+ if (ret != 0) {
+ errx(ret, "S.BUS v2 failed");
+ }
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "rssi_analog")) {
+ /* we can cheat and call the driver directly, as it
+ * doesn't reference filp in ioctl()
+ */
+ int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1);
+
+ if (ret != 0) {
+ errx(ret, "RSSI analog failed");
+ }
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "rssi_pwm")) {
+ /* we can cheat and call the driver directly, as it
+ * doesn't reference filp in ioctl()
+ */
+ int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1);
+
+ if (ret != 0) {
+ errx(ret, "RSSI PWM failed");
+ }
+
+ exit(0);
+ }
+
out:
- errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'");
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug <level>',\n"
+ "'recovery', 'limit <rate>', 'current', 'bind', 'checkcrc',\n"
+ "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'");
}
diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp
index 41f93a8ee..dd8abbac5 100644
--- a/src/drivers/px4io/px4io_uploader.cpp
+++ b/src/drivers/px4io/px4io_uploader.cpp
@@ -51,6 +51,7 @@
#include <poll.h>
#include <termios.h>
#include <sys/stat.h>
+#include <nuttx/arch.h>
#include <crc32.h>
@@ -120,8 +121,15 @@ PX4IO_Uploader::upload(const char *filenames[])
cfsetspeed(&t, 115200);
tcsetattr(_io_fd, TCSANOW, &t);
- /* look for the bootloader */
- ret = sync();
+ /* look for the bootloader for 150 ms */
+ for (int i = 0; i < 15; i++) {
+ ret = sync();
+ if (ret == OK) {
+ break;
+ } else {
+ usleep(10000);
+ }
+ }
if (ret != OK) {
/* this is immediately fatal */
@@ -226,6 +234,11 @@ PX4IO_Uploader::upload(const char *filenames[])
close(_fw_fd);
close(_io_fd);
_io_fd = -1;
+
+ // sleep for enough time for the IO chip to boot. This makes
+ // forceupdate more reliably startup IO again after update
+ up_udelay(100*1000);
+
return ret;
}
diff --git a/src/drivers/px4io/uploader.h b/src/drivers/px4io/uploader.h
index 22387a3e2..55f63eef9 100644
--- a/src/drivers/px4io/uploader.h
+++ b/src/drivers/px4io/uploader.h
@@ -91,7 +91,7 @@ private:
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);
- int get_sync(unsigned timeout = 1000);
+ int get_sync(unsigned timeout = 40);
int sync();
int get_info(int param, uint32_t &val);
int erase();
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-04 16:01:43 +0000