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.h6
-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.h18
-rw-r--r--src/drivers/drv_rc_input.h54
-rw-r--r--src/drivers/drv_sbus.h58
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp161
-rw-r--r--src/drivers/meas_airspeed/meas_airspeed.cpp22
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp458
-rw-r--r--src/drivers/px4fmu/fmu.cpp58
-rw-r--r--src/drivers/px4fmu/module.mk3
-rw-r--r--src/drivers/px4io/px4io.cpp350
-rw-r--r--src/drivers/px4io/px4io_uploader.cpp17
-rw-r--r--src/drivers/px4io/uploader.h2
16 files changed, 694 insertions, 531 deletions
diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 71c0b70f0..771f2128f 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -92,7 +92,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 4972e6914..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)
@@ -87,7 +87,7 @@ __BEGIN_DECLS
#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)
+#define GPIO_EXTI_MPU_DRDY_OFF (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)
@@ -98,7 +98,7 @@ __BEGIN_DECLS
#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_MPU_OFF (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)
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 51f916f37..c3eea310f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -189,6 +189,24 @@ 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)
+
+/** get the lockdown override flag to enable outputs in HIL */
+#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22)
+
+/*
+ *
+ *
+ * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE!
+ *
+ *
+ */
+
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
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/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 49b72cf79..4c85c0cda 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -839,42 +839,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 (_class_instance == CLASS_DEVICE_PRIMARY && !(_pub_blocked)) {
if (_mag_topic != -1) {
@@ -886,7 +868,6 @@ HMC5883::collect()
if (_mag_topic < 0)
debug("failed to create sensor_mag publication");
}
-
}
/* post a report to the ring */
@@ -909,6 +890,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;
@@ -931,31 +913,16 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
1.0f,
};
- float avg_excited[3] = {0.0f, 0.0f, 0.0f};
- unsigned i;
-
- warnx("starting mag scale calibration");
+ float sum_excited[3] = {0.0f, 0.0f, 0.0f};
- /* do a simple demand read */
- sz = read(filp, (char *)&report, sizeof(report));
+ /* 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 };
- 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)) {
@@ -964,8 +931,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;
@@ -989,8 +957,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 */
@@ -1008,32 +976,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]);
@@ -1166,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)
@@ -1174,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);
}
@@ -1212,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 51a059e39..05ae21c1f 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;
}
@@ -394,7 +386,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/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 30d6069b3..ec5f77d74 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved.
* Author: Marco Bauer <marco@wtns.de>
*
* Redistribution and use in source and binary forms, with or without
@@ -65,7 +65,6 @@
#include <drivers/device/device.h>
#include <drivers/drv_pwm_output.h>
-#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h>
@@ -93,16 +92,12 @@
#define MOTOR_SPINUP_COUNTER 30
#define ESC_UORB_PUBLISH_DELAY 500000
+
+
+
class MK : public device::I2C
{
public:
- enum Mode {
- MODE_NONE,
- MODE_2PWM,
- MODE_4PWM,
- MODE_6PWM,
- };
-
enum MappingMode {
MAPPING_MK = 0,
MAPPING_PX4,
@@ -120,8 +115,7 @@ public:
virtual int init(unsigned motors);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
- int set_mode(Mode mode);
- int set_pwm_rate(unsigned rate);
+ int set_update_rate(unsigned rate);
int set_motor_count(unsigned count);
int set_motor_test(bool motortest);
int set_overrideSecurityChecks(bool overrideSecurityChecks);
@@ -133,7 +127,6 @@ private:
static const unsigned _max_actuators = MAX_MOTORS;
static const bool showDebug = false;
- Mode _mode;
int _update_rate;
int _current_update_rate;
int _task;
@@ -180,33 +173,15 @@ private:
static const GPIOConfig _gpio_tab[];
static const unsigned _ngpio;
- void gpio_reset(void);
- void gpio_set_function(uint32_t gpios, int function);
- void gpio_write(uint32_t gpios, int function);
- uint32_t gpio_read(void);
- int gpio_ioctl(file *filp, int cmd, unsigned long arg);
int mk_servo_arm(bool status);
-
int mk_servo_set(unsigned int chan, short val);
int mk_servo_set_value(unsigned int chan, short val);
int mk_servo_test(unsigned int chan);
short scaling(float val, float inMin, float inMax, float outMin, float outMax);
-
-
};
-const MK::GPIOConfig MK::_gpio_tab[] = {
- {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0},
- {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0},
- {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, GPIO_USART2_CTS_1},
- {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, GPIO_USART2_RTS_1},
- {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, GPIO_USART2_TX_1},
- {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, GPIO_USART2_RX_1},
- {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, GPIO_CAN2_TX_2},
- {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, GPIO_CAN2_RX_2},
-};
-const unsigned MK::_ngpio = sizeof(MK::_gpio_tab) / sizeof(MK::_gpio_tab[0]);
+
const int blctrlAddr_quad_plus[] = { 2, 2, -2, -2, 0, 0, 0, 0 }; // Addresstranslator for Quad + configuration
const int blctrlAddr_hexa_plus[] = { 0, 2, 2, -2, 1, -3, 0, 0 }; // Addresstranslator for Hexa + configuration
@@ -247,8 +222,7 @@ MK *g_mk;
MK::MK(int bus, const char *_device_path) :
I2C("mkblctrl", "/dev/mkblctrl", bus, 0, I2C_BUS_SPEED),
- _mode(MODE_NONE),
- _update_rate(50),
+ _update_rate(400),
_task(-1),
_t_actuators(-1),
_t_actuator_armed(-1),
@@ -317,26 +291,23 @@ MK::init(unsigned motors)
usleep(500000);
- if (sizeof(_device) > 0) {
- ret = register_driver(_device, &fops, 0666, (void *)this);
+ if (sizeof(_device) > 0) {
+ ret = register_driver(_device, &fops, 0666, (void *)this);
- if (ret == OK) {
+ if (ret == OK) {
log("creating alternate output device");
_primary_pwm_device = true;
}
- }
-
- /* reset GPIOs */
- gpio_reset();
+ }
/* start the IO interface task */
_task = task_spawn_cmd("mkblctrl",
- SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 20,
- 2048,
- (main_t)&MK::task_main_trampoline,
- nullptr);
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 20,
+ 2048,
+ (main_t)&MK::task_main_trampoline,
+ nullptr);
if (_task < 0) {
@@ -354,43 +325,7 @@ MK::task_main_trampoline(int argc, char *argv[])
}
int
-MK::set_mode(Mode mode)
-{
- /*
- * Configure for PWM output.
- *
- * Note that regardless of the configured mode, the task is always
- * listening and mixing; the mode just selects which of the channels
- * are presented on the output pins.
- */
- switch (mode) {
- case MODE_2PWM:
- up_pwm_servo_deinit();
- _update_rate = UPDATE_RATE; /* default output rate */
- break;
-
- case MODE_4PWM:
- up_pwm_servo_deinit();
- _update_rate = UPDATE_RATE; /* default output rate */
- break;
-
- case MODE_NONE:
- debug("MODE_NONE");
- /* disable servo outputs and set a very low update rate */
- up_pwm_servo_deinit();
- _update_rate = UPDATE_RATE;
- break;
-
- default:
- return -EINVAL;
- }
-
- _mode = mode;
- return OK;
-}
-
-int
-MK::set_pwm_rate(unsigned rate)
+MK::set_update_rate(unsigned rate)
{
if ((rate > 500) || (rate < 10))
return -EINVAL;
@@ -621,11 +556,13 @@ MK::task_main()
}
}
- if(!_overrideSecurityChecks) {
+ if (!_overrideSecurityChecks) {
/* don't go under BLCTRL_MIN_VALUE */
+
if (outputs.output[i] < BLCTRL_MIN_VALUE) {
outputs.output[i] = BLCTRL_MIN_VALUE;
}
+
}
/* output to BLCtrl's */
@@ -675,21 +612,24 @@ MK::task_main()
esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
esc.esc[i].esc_rpm = (uint16_t) 0;
esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
+
if (Motor[i].Version == 1) {
// BLCtrl 2.0 (11Bit)
- esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
+ esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits;
+
} else {
// BLCtrl < 2.0 (8Bit)
esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
}
+
esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
esc.esc[i].esc_state = (uint16_t) Motor[i].State;
esc.esc[i].esc_errorcount = (uint16_t) 0;
- // if motortest is requested - do it...
- if (_motortest == true) {
- mk_servo_test(i);
- }
+ // if motortest is requested - do it...
+ if (_motortest == true) {
+ mk_servo_test(i);
+ }
}
@@ -728,7 +668,7 @@ MK::mk_servo_arm(bool status)
unsigned int
MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
{
- if(initI2C) {
+ if (initI2C) {
I2C::init();
}
@@ -781,8 +721,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
}
-
- if(!_overrideSecurityChecks) {
+
+ if (!_overrideSecurityChecks) {
if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
_task_should_exit = true;
}
@@ -811,8 +751,8 @@ MK::mk_servo_set(unsigned int chan, short val)
tmpVal = 0;
}
- Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
- Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
+ Motor[chan].SetPoint = (uint8_t)(tmpVal >> 3) & 0xff;
+ Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal % 8) & 0x07;
if (_armed == false) {
Motor[chan].SetPoint = 0;
@@ -1019,28 +959,6 @@ MK::ioctl(file *filp, int cmd, unsigned long arg)
{
int ret;
- // XXX disabled, confusing users
-
- /* try it as a GPIO ioctl first */
- ret = gpio_ioctl(filp, cmd, arg);
-
- if (ret != -ENOTTY)
- return ret;
-
- /* if we are in valid PWM mode, try it as a PWM ioctl as well */
- /*
- switch (_mode) {
- case MODE_2PWM:
- case MODE_4PWM:
- case MODE_6PWM:
- ret = pwm_ioctl(filp, cmd, arg);
- break;
-
- default:
- debug("not in a PWM mode");
- break;
- }
- */
ret = pwm_ioctl(filp, cmd, arg);
/* if nobody wants it, let CDev have it */
@@ -1075,6 +993,10 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK;
break;
+ case PWM_SERVO_GET_UPDATE_RATE:
+ *(uint32_t *)arg = _update_rate;
+ break;
+
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK;
break;
@@ -1084,6 +1006,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
if (arg < 2150) {
Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
+
} else {
ret = -EINVAL;
}
@@ -1198,139 +1121,10 @@ MK::write(file *filp, const char *buffer, size_t len)
return count * 2;
}
-void
-MK::gpio_reset(void)
-{
- /*
- * Setup default GPIO config - all pins as GPIOs, GPIO driver chip
- * to input mode.
- */
- for (unsigned i = 0; i < _ngpio; i++)
- stm32_configgpio(_gpio_tab[i].input);
-
- stm32_gpiowrite(GPIO_GPIO_DIR, 0);
- stm32_configgpio(GPIO_GPIO_DIR);
-}
-
-void
-MK::gpio_set_function(uint32_t gpios, int function)
-{
- /*
- * GPIOs 0 and 1 must have the same direction as they are buffered
- * by a shared 2-port driver. Any attempt to set either sets both.
- */
- if (gpios & 3) {
- gpios |= 3;
-
- /* flip the buffer to output mode if required */
- if (GPIO_SET_OUTPUT == function)
- stm32_gpiowrite(GPIO_GPIO_DIR, 1);
- }
-
- /* configure selected GPIOs as required */
- for (unsigned i = 0; i < _ngpio; i++) {
- if (gpios & (1 << i)) {
- switch (function) {
- case GPIO_SET_INPUT:
- stm32_configgpio(_gpio_tab[i].input);
- break;
-
- case GPIO_SET_OUTPUT:
- stm32_configgpio(_gpio_tab[i].output);
- break;
-
- case GPIO_SET_ALT_1:
- if (_gpio_tab[i].alt != 0)
- stm32_configgpio(_gpio_tab[i].alt);
-
- break;
- }
- }
- }
-
- /* flip buffer to input mode if required */
- if ((GPIO_SET_INPUT == function) && (gpios & 3))
- stm32_gpiowrite(GPIO_GPIO_DIR, 0);
-}
-
-void
-MK::gpio_write(uint32_t gpios, int function)
-{
- int value = (function == GPIO_SET) ? 1 : 0;
-
- for (unsigned i = 0; i < _ngpio; i++)
- if (gpios & (1 << i))
- stm32_gpiowrite(_gpio_tab[i].output, value);
-}
-
-uint32_t
-MK::gpio_read(void)
-{
- uint32_t bits = 0;
-
- for (unsigned i = 0; i < _ngpio; i++)
- if (stm32_gpioread(_gpio_tab[i].input))
- bits |= (1 << i);
-
- return bits;
-}
-
-int
-MK::gpio_ioctl(struct file *filp, int cmd, unsigned long arg)
-{
- int ret = OK;
-
- lock();
-
- switch (cmd) {
-
- case GPIO_RESET:
- gpio_reset();
- break;
-
- case GPIO_SET_OUTPUT:
- case GPIO_SET_INPUT:
- case GPIO_SET_ALT_1:
- gpio_set_function(arg, cmd);
- break;
-
- case GPIO_SET_ALT_2:
- case GPIO_SET_ALT_3:
- case GPIO_SET_ALT_4:
- ret = -EINVAL;
- break;
-
- case GPIO_SET:
- case GPIO_CLEAR:
- gpio_write(arg, cmd);
- break;
-
- case GPIO_GET:
- *(uint32_t *)arg = gpio_read();
- break;
-
- default:
- ret = -ENOTTY;
- }
-
- unlock();
-
- return ret;
-}
namespace
{
-enum PortMode {
- PORT_MODE_UNSET = 0,
- PORT_FULL_GPIO,
- PORT_FULL_SERIAL,
- PORT_FULL_PWM,
- PORT_GPIO_AND_SERIAL,
- PORT_PWM_AND_SERIAL,
- PORT_PWM_AND_GPIO,
-};
-
enum MappingMode {
MAPPING_MK = 0,
MAPPING_PX4,
@@ -1341,20 +1135,11 @@ enum FrameType {
FRAME_X,
};
-PortMode g_port_mode;
int
-mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
+mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
{
- uint32_t gpio_bits;
int shouldStop = 0;
- MK::Mode servo_mode;
-
- /* reset to all-inputs */
- g_mk->ioctl(0, GPIO_RESET, 0);
-
- gpio_bits = 0;
- servo_mode = MK::MODE_NONE;
/* native PX4 addressing) */
g_mk->set_px4mode(px4mode);
@@ -1368,7 +1153,6 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
/* ovveride security checks if enabled */
g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
-
/* count used motors */
do {
if (g_mk->mk_check_for_blctrl(8, false, false) != 0) {
@@ -1383,86 +1167,54 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
g_mk->set_motor_count(g_mk->mk_check_for_blctrl(8, true, false));
- /* (re)set the PWM output mode */
- g_mk->set_mode(servo_mode);
-
-
- if ((servo_mode != MK::MODE_NONE) && (update_rate != 0))
- g_mk->set_pwm_rate(update_rate);
+ g_mk->set_update_rate(update_rate);
return OK;
}
int
-mk_start(unsigned bus, unsigned motors, char *device_path)
-{
- int ret = OK;
-
- if (g_mk == nullptr) {
-
- g_mk = new MK(bus, device_path);
-
- if (g_mk == nullptr) {
- ret = -ENOMEM;
-
- } else {
- ret = g_mk->init(motors);
-
- if (ret != OK) {
- delete g_mk;
- g_mk = nullptr;
- }
- }
- }
-
- return ret;
-}
-
-
-int
-mk_check_for_i2c_esc_bus(char *device_path, int motors)
+mk_start(unsigned motors, char *device_path)
{
int ret;
- if (g_mk == nullptr) {
+ // try i2c3 first
+ g_mk = new MK(3, device_path);
- g_mk = new MK(3, device_path);
+ if (!g_mk)
+ return -ENOMEM;
- if (g_mk == nullptr) {
- return -1;
-
- } else {
- ret = g_mk->mk_check_for_blctrl(8, false, true);
- delete g_mk;
- g_mk = nullptr;
-
- if (ret > 0) {
- return 3;
- }
+ if (OK == g_mk->init(motors)) {
+ warnx("[mkblctrl] scanning i2c3...\n");
+ ret = g_mk->mk_check_for_blctrl(8, false, true);
+ if (ret > 0) {
+ return OK;
}
+ }
+ delete g_mk;
+ g_mk = nullptr;
- g_mk = new MK(1, device_path);
-
- if (g_mk == nullptr) {
- return -1;
+ // fallback to bus 1
+ g_mk = new MK(1, device_path);
- } else {
- ret = g_mk->mk_check_for_blctrl(8, false, true);
- delete g_mk;
- g_mk = nullptr;
+ if (!g_mk)
+ return -ENOMEM;
- if (ret > 0) {
- return 1;
- }
+ if (OK == g_mk->init(motors)) {
+ warnx("[mkblctrl] scanning i2c1...\n");
+ ret = g_mk->mk_check_for_blctrl(8, false, true);
+ if (ret > 0) {
+ return OK;
}
}
- return -1;
-}
+ delete g_mk;
+ g_mk = nullptr;
+ return -ENXIO;
+}
} // namespace
@@ -1472,10 +1224,8 @@ extern "C" __EXPORT int mkblctrl_main(int argc, char *argv[]);
int
mkblctrl_main(int argc, char *argv[])
{
- PortMode port_mode = PORT_FULL_PWM;
int pwm_update_rate_in_hz = UPDATE_RATE;
int motorcount = 8;
- int bus = -1;
int px4mode = MAPPING_PX4;
int frametype = FRAME_PLUS; // + plus is default
bool motortest = false;
@@ -1489,18 +1239,6 @@ mkblctrl_main(int argc, char *argv[])
*/
for (int i = 1; i < argc; i++) {
- /* look for the optional i2c bus parameter */
- if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
- if (argc > i + 1) {
- bus = atoi(argv[i + 1]);
- newMode = true;
-
- } else {
- errx(1, "missing argument for i2c bus (-b)");
- return 1;
- }
- }
-
/* look for the optional frame parameter */
if (strcmp(argv[i], "-mkmode") == 0 || strcmp(argv[i], "--mkmode") == 0) {
if (argc > i + 1) {
@@ -1560,51 +1298,43 @@ mkblctrl_main(int argc, char *argv[])
fprintf(stderr, "mkblctrl: help:\n");
fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n");
fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
- fprintf(stderr, "\t -b {i2c_bus_number} \t\t Set the i2c bus where the ESCs are connected to (default autoscan).\n");
fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n");
fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
fprintf(stderr, "\n");
fprintf(stderr, "Motortest:\n");
fprintf(stderr, "First you have to start mkblctrl, the you can enter Motortest Mode with:\n");
fprintf(stderr, "mkblctrl -t\n");
- fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
+ fprintf(stderr, "This will spin up once every motor in order of motoraddress. (DANGER !!!)\n");
exit(1);
}
if (!motortest) {
- if (g_mk == nullptr) {
- if (bus == -1) {
- bus = mk_check_for_i2c_esc_bus(devicepath, motorcount);
- }
-
- if (bus != -1) {
- if (mk_start(bus, motorcount, devicepath) != OK) {
- errx(1, "failed to start the MK-BLCtrl driver");
- }
- } else {
- errx(1, "failed to start the MK-BLCtrl driver (cannot find i2c esc's)");
- }
-
- /* parameter set ? */
- if (newMode) {
- /* switch parameter */
- return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
- }
-
- exit(0);
- } else {
- errx(1, "MK-BLCtrl driver already running");
- }
-
- } else {
- if (g_mk == nullptr) {
- errx(1, "MK-BLCtrl driver not running. You have to start it first.");
-
- } else {
- g_mk->set_motor_test(motortest);
- exit(0);
-
- }
- }
+ if (g_mk == nullptr) {
+ if (mk_start(motorcount, devicepath) != OK) {
+ errx(1, "failed to start the MK-BLCtrl driver");
+ }
+
+ /* parameter set ? */
+ if (newMode) {
+ /* switch parameter */
+ return mk_new_mode(pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
+ }
+
+ exit(0);
+
+ } else {
+ errx(1, "MK-BLCtrl driver already running");
+ }
+
+ } else {
+ if (g_mk == nullptr) {
+ errx(1, "MK-BLCtrl driver not running. You have to start it first.");
+
+ } else {
+ g_mk->set_motor_test(motortest);
+ exit(0);
+
+ }
+ }
}
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index b28d318d7..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;
@@ -1108,10 +1150,12 @@ PX4FMU::sensor_reset(int ms)
stm32_configgpio(GPIO_SPI_CS_GYRO_OFF);
stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
stm32_configgpio(GPIO_SPI_CS_BARO_OFF);
+ stm32_configgpio(GPIO_SPI_CS_MPU_OFF);
stm32_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
stm32_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
+ stm32_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
stm32_configgpio(GPIO_SPI1_SCK_OFF);
stm32_configgpio(GPIO_SPI1_MISO_OFF);
@@ -1124,10 +1168,12 @@ PX4FMU::sensor_reset(int ms)
stm32_configgpio(GPIO_GYRO_DRDY_OFF);
stm32_configgpio(GPIO_MAG_DRDY_OFF);
stm32_configgpio(GPIO_ACCEL_DRDY_OFF);
+ stm32_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
stm32_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
stm32_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
+ stm32_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
/* set the sensor rail off */
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
@@ -1160,6 +1206,13 @@ PX4FMU::sensor_reset(int ms)
stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+
+ // // XXX bring up the EXTI pins again
+ // stm32_configgpio(GPIO_GYRO_DRDY);
+ // stm32_configgpio(GPIO_MAG_DRDY);
+ // stm32_configgpio(GPIO_ACCEL_DRDY);
+ // stm32_configgpio(GPIO_EXTI_MPU_DRDY);
+
#endif
#endif
}
@@ -1432,7 +1485,6 @@ void
sensor_reset(int ms)
{
int fd;
- int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index d918abd57..05bc7a5b3 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -3,5 +3,4 @@
#
MODULE_COMMAND = fmu
-SRCS = fmu.cpp \
- ../../modules/systemlib/pwm_limit/pwm_limit.c
+SRCS = fmu.cpp
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 0ca35d2f2..efcf4d12b 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,9 +272,11 @@ 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
float _battery_amp_per_volt; ///< current sensor amps/volt
float _battery_amp_bias; ///< current sensor bias
@@ -443,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;
}
@@ -466,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),
@@ -489,6 +494,7 @@ PX4IO::PX4IO(device::Device *interface) :
_to_servorail(0),
_to_safety(0),
_primary_pwm_device(false),
+ _lockdown_override(false),
_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
_battery_amp_bias(0),
_battery_mamphour_total(0),
@@ -504,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()
@@ -578,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.");
@@ -772,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);
/*
@@ -807,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();
@@ -1043,13 +1052,19 @@ PX4IO::io_set_arming_state()
uint16_t set = 0;
uint16_t clear = 0;
- if (armed.armed && !armed.lockdown) {
+ if (armed.armed) {
set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
} else {
clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
}
+ if (armed.lockdown && !_lockdown_override) {
+ set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+ } else {
+ clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
+ }
+
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
@@ -1300,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
@@ -1331,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);
}
}
@@ -1353,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)
@@ -1390,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)
@@ -1400,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);
@@ -1423,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);
@@ -1448,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 */
@@ -1664,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) {
@@ -1688,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;
@@ -1734,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" : ""),
@@ -1801,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) {
@@ -1828,16 +1893,23 @@ 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\n",
+ printf("arming 0x%04x%s%s%s%s%s%s%s\n",
arming,
- ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
- ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
- ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
- ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""));
+ ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"),
+ ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"),
+ ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""),
+ ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""));
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -2034,6 +2106,14 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
*(unsigned *)arg = _max_actuators;
break;
+ case PWM_SERVO_SET_DISABLE_LOCKDOWN:
+ _lockdown_override = arg;
+ break;
+
+ case PWM_SERVO_GET_DISABLE_LOCKDOWN:
+ *(unsigned *)arg = _lockdown_override;
+ break;
+
case DSM_BIND_START:
io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
usleep(500000);
@@ -2250,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;
@@ -2358,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;
@@ -2621,7 +2735,7 @@ monitor(void)
/* clear screen */
printf("\033[2J");
- unsigned cancels = 3;
+ unsigned cancels = 2;
for (;;) {
pollfd fds[1];
@@ -2635,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");
@@ -2657,13 +2771,72 @@ void
if_test(unsigned mode)
{
device::Device *interface = get_interface();
+ int result;
- int result = interface->ioctl(1, mode); /* XXX magic numbers */
- delete interface;
+ if (interface) {
+ result = interface->ioctl(1, mode); /* XXX magic numbers */
+ delete interface;
+ } else {
+ errx(1, "interface not loaded, exiting");
+ }
errx(0, "test returned %d", result);
}
+void
+lockdown(int argc, char *argv[])
+{
+ if (g_dev != nullptr) {
+
+ if (argc > 2 && !strcmp(argv[2], "disable")) {
+
+ warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?");
+ warnx("Press 'y' to enable, any other key to abort.");
+
+ /* check if user wants to abort */
+ char c;
+
+ struct pollfd fds;
+ int ret;
+ hrt_abstime start = hrt_absolute_time();
+ const unsigned long timeout = 5000000;
+
+ while (hrt_elapsed_time(&start) < timeout) {
+ fds.fd = 0; /* stdin */
+ fds.events = POLLIN;
+ ret = poll(&fds, 1, 0);
+
+ if (ret > 0) {
+
+ read(0, &c, 1);
+
+ if (c != 'y') {
+ exit(0);
+ } else if (c == 'y') {
+ break;
+ }
+ }
+
+ usleep(10000);
+ }
+
+ if (hrt_elapsed_time(&start) > timeout)
+ errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES.");
+
+ (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1);
+
+ warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!");
+ } else {
+ (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
+ warnx("ACTUATORS ARE NOW SAFE IN HIL.");
+ }
+
+ } else {
+ errx(1, "driver not loaded, exiting");
+ }
+ exit(0);
+}
+
} /* namespace */
int
@@ -2688,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;
@@ -2760,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];
@@ -2829,6 +3015,7 @@ px4io_main(int argc, char *argv[])
/* stop the driver */
delete g_dev;
+ g_dev = nullptr;
exit(0);
}
@@ -2883,6 +3070,63 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "bind"))
bind(argc, 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-07 03:02:30 +0000