mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 17:23:35 +08:00
Merged master into lockdown_disable
This commit is contained in:
Lorenz Meier
@@ -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));
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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 *************************************************************/
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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
|
||||
@@ -62,6 +62,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 */
|
||||
|
||||
@@ -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 */
|
||||
@@ -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);
|
||||
|
||||
@@ -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,32 +915,17 @@ 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};
|
||||
|
||||
/* 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 };
|
||||
|
||||
warnx("starting mag scale calibration");
|
||||
|
||||
/* 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;
|
||||
// }
|
||||
|
||||
/* start the sensor polling at 50 Hz */
|
||||
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
|
||||
warn("failed to set 2Hz poll rate");
|
||||
@@ -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");
|
||||
}
|
||||
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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);
|
||||
|
||||
|
||||
+210
-45
@@ -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, ®s[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, ®s[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, ®s[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);
|
||||
}
|
||||
|
||||
// tear down the px4io instance
|
||||
delete g_dev;
|
||||
/* 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");
|
||||
}
|
||||
}
|
||||
|
||||
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'");
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
|
||||
|
||||
@@ -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();
|
||||
|
||||
Reference in New Issue
Block a user