fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 08:50:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merged master into ekf2

Browse Source
This commit is contained in:
Lorenz Meier
2015-12-24 09:53:54 +01:00
parent b958928095 45e4def3eb
commit d3149153f6
24 changed files with 1606 additions and 168 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/sitl_run.sh
+12 -3
View File
@@ -14,6 +14,15 @@ echo program: $program
echo model: $model
echo build_path: $build_path
if [ "$chroot" == "1" ]
then
chroot_enabled=-c
sudo_enabled=sudo
else
chroot_enabled=""
sudo_enabled=""
fi
if [ "$model" == "" ] || [ "$model" == "none" ]
then
echo "empty model, setting iris as default"
@@ -48,7 +57,7 @@ if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ]
then
cd Tools/jMAVSim
ant
nice -n -10 java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 &
SIM_PID=`echo $!`
elif [ "$program" == "gazebo" ] && [ "$no_sim" == "" ]
then
@@ -65,7 +74,7 @@ then
cd Tools/sitl_gazebo/Build
cmake -Wno-dev ..
make -j4
nice -n -10 gzserver --verbose ../worlds/${model}.world &
gzserver --verbose ../worlds/${model}.world &
SIM_PID=`echo $!`
gzclient --verbose &
GUI_PID=`echo $!`
@@ -96,7 +105,7 @@ elif [ "$debugger" == "valgrind" ]
then
valgrind ./mainapp ../../../../${rc_script}_${program}_${model}
else
nice -n -10 ./mainapp ../../../../${rc_script}_${program}_${model}
$sudo_enabled ./mainapp $chroot_enabled ../../../../${rc_script}_${program}_${model}
fi
if [ "$program" == "jmavsim" ]
cmake/configs/nuttx_px4fmu-v4_default.cmake
+1 -1
View File
@@ -144,7 +144,7 @@ set(config_module_list
#examples/math_demo
# Tutorial code from
# https://px4.io/dev/px4_simple_app
#examples/px4_simple_app
examples/px4_simple_app
# Tutorial code from
# https://px4.io/dev/daemon
cmake/configs/posix_sitl_default.cmake
+1
View File
@@ -59,6 +59,7 @@ set(config_module_list
lib/launchdetection
lib/terrain_estimation
lib/runway_takeoff
examples/px4_simple_app
)
set(config_extra_builtin_cmds
src/drivers/boards/aerocore/board_config.h
+1
View File
@@ -150,6 +150,7 @@ __BEGIN_DECLS
#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3
#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2
#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2
#define DIRECT_PWM_OUTPUT_CHANNELS 8
/* High-resolution timer */
#define HRT_TIMER 8 /* use timer 8 for the HRT */
src/drivers/boards/px4fmu-v1/board_config.h
+1
View File
@@ -182,6 +182,7 @@ __BEGIN_DECLS
#define GPIO_TIM2_CH2OUT GPIO_TIM2_CH2OUT_1
#define GPIO_TIM2_CH3OUT GPIO_TIM2_CH3OUT_1
#define GPIO_TIM2_CH4OUT GPIO_TIM2_CH4OUT_1
#define DIRECT_PWM_OUTPUT_CHANNELS 4
/* USB OTG FS
*
src/drivers/boards/px4fmu-v2/board_config.h
+1
View File
@@ -210,6 +210,7 @@ __BEGIN_DECLS
#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2
#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2
#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2
#define DIRECT_PWM_OUTPUT_CHANNELS 6
/* USB OTG FS
*
src/drivers/boards/px4fmu-v4/board_config.h
+12 -3
View File
@@ -65,7 +65,7 @@ __BEGIN_DECLS
//{0, GPIO_PERIPH_3V3_EN, 0}, Owned by the 8266 driver
//{0, GPIO_SBUS_INV, 0}, https://github.com/PX4/Firmware/blob/master/src/modules/px4iofirmware/sbus.c
//{GPIO_8266_GPIO0, 0, 0}, Owned by the 8266 driver
//{0, GPIO_SPEKTRUM_POWER, 0}, Owned Spektum driver input to auto pilot
//{0, GPIO_SPEKTRUM_PWR_EN, 0}, Owned Spektum driver input to auto pilot
//{0, GPIO_8266_PD, 0}, Owned by the 8266 driver
//{0, GPIO_8266_RST, 0}, Owned by the 8266 driver
@@ -200,6 +200,7 @@ __BEGIN_DECLS
#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_2
#define GPIO_TIM4_CH2OUT GPIO_TIM4_CH2OUT_2
#define GPIO_TIM4_CH3OUT GPIO_TIM4_CH3OUT_2
#define DIRECT_PWM_OUTPUT_CHANNELS 6
/* USB OTG FS
*
@@ -223,15 +224,23 @@ __BEGIN_DECLS
#define GPIO_RSSI_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1)
#define GPIO_LED_SAFETY (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
#define GPIO_SAFETY_SWITCH_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
#define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4)
#define GPIO_PERIPH_3V3_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5)
#define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13)
#define GPIO_8266_GPIO0 (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN2)
#define GPIO_SPEKTRUM_POWER (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4)
#define GPIO_8266_PD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5)
#define GPIO_8266_RST (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6)
/* Power switch controls ******************************************************/
#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7)
#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX)
#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)
/****************************************************************************************************
* Public Types
****************************************************************************************************/
src/drivers/boards/px4fmu-v4/px4fmu_init.c
+1 -1
View File
@@ -230,7 +230,7 @@ __EXPORT int nsh_archinitialize(void)
stm32_configgpio(GPIO_SBUS_INV);
stm32_configgpio(GPIO_8266_GPIO0);
stm32_configgpio(GPIO_SPEKTRUM_POWER);
stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
stm32_configgpio(GPIO_8266_PD);
stm32_configgpio(GPIO_8266_RST);
src/drivers/boards/px4io-v1/board_config.h
+7
View File
@@ -83,6 +83,13 @@
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
#define GPIO_SPEKTRUM_PWR_EN GPIO_RELAY1_EN
#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX)
#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)
/* Analog inputs ********************************************************************/
#define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
src/drivers/boards/px4io-v2/board_config.h
+6 -1
View File
@@ -79,7 +79,7 @@
#define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN13)
#define GPIO_LED4 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN11)
#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10)
#define GPIO_USART1_RX_SPEKTRUM (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10)
/* Safety switch button *******************************************************/
@@ -88,6 +88,11 @@
/* Power switch controls ******************************************************/
#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13)
#define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
#define SPEKTRUM_RX_HIGH(_s) stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, (_s))
#define SPEKTRUM_RX_AS_UART() stm32_configgpio(GPIO_USART1_RX)
#define SPEKTRUM_RX_AS_GPIO() stm32_configgpio(GPIO_USART1_RX_SPEKTRUM)
#define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15)
src/drivers/px4fmu/fmu.cpp
+10 -24
View File
@@ -90,12 +90,8 @@
# include <systemlib/ppm_decode.h>
#endif
/*
* This is the analog to FMU_INPUT_DROP_LIMIT_US on the IO side
*/
#define CONTROL_INPUT_DROP_LIMIT_US 2000
#define NAN_VALUE (0.0f/0.0f)
#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */
#define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */
class PX4FMU : public device::CDev
{
@@ -123,15 +119,7 @@ public:
int set_i2c_bus_clock(unsigned bus, unsigned clock_hz);
private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
static const unsigned _max_actuators = 4;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
static const unsigned _max_actuators = 6;
#endif
#if defined(CONFIG_ARCH_BOARD_AEROCORE)
static const unsigned _max_actuators = 8;
#endif
static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
Mode _mode;
unsigned _pwm_default_rate;
@@ -189,7 +177,7 @@ private:
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
void update_pwm_rev_mask();
void publish_pwm_outputs(uint16_t *values, size_t numvalues);
void publish_pwm_outputs(uint16_t *values, size_t numvalues);
struct GPIOConfig {
uint32_t input;
@@ -321,11 +309,9 @@ PX4FMU::PX4FMU() :
memset(_controls, 0, sizeof(_controls));
memset(_poll_fds, 0, sizeof(_poll_fds));
#ifdef HRT_PPM_CHANNEL
// rc input, published to ORB
memset(&_rc_in, 0, sizeof(_rc_in));
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
#endif
#ifdef GPIO_SBUS_INV
// this board has a GPIO to control SBUS inversion
@@ -728,8 +714,8 @@ PX4FMU::cycle()
//main_out_latency = hrt_absolute_time() - _controls[i].timestamp - 250;
/* do only correct within the current phase */
if (abs(main_out_latency) > CONTROL_INPUT_DROP_LIMIT_US) {
main_out_latency = CONTROL_INPUT_DROP_LIMIT_US;
if (abs(main_out_latency) > SCHEDULE_INTERVAL) {
main_out_latency = SCHEDULE_INTERVAL;
}
if (main_out_latency < 250) {
@@ -905,7 +891,7 @@ PX4FMU::cycle()
}
work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this,
USEC2TICK(CONTROL_INPUT_DROP_LIMIT_US - main_out_latency));
USEC2TICK(SCHEDULE_INTERVAL - main_out_latency));
}
void PX4FMU::work_stop()
@@ -1683,9 +1669,9 @@ PX4FMU::peripheral_reset(int ms)
stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 0);
bool last = stm32_gpioread(GPIO_SPEKTRUM_POWER);
bool last = stm32_gpioread(GPIO_SPEKTRUM_PWR_EN);
/* Keep Spektum on to discharge rail*/
stm32_gpiowrite(GPIO_SPEKTRUM_POWER, 1);
stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, 1);
/* wait for the peripheral rail to reach GND */
usleep(ms * 1000);
@@ -1694,7 +1680,7 @@ PX4FMU::peripheral_reset(int ms)
/* re-enable power */
/* switch the peripheral rail back on */
stm32_gpiowrite(GPIO_SPEKTRUM_POWER, last);
stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, last);
stm32_gpiowrite(GPIO_PERIPH_3V3_EN, 1);
#endif
}
src/examples/px4_simple_app/px4_simple_app.c
+14 -10
View File
@@ -39,6 +39,8 @@
*/
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
@@ -52,7 +54,7 @@ __EXPORT int px4_simple_app_main(int argc, char *argv[]);
int px4_simple_app_main(int argc, char *argv[])
{
printf("Hello Sky!\n");
PX4_INFO("Hello Sky!");
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
@@ -64,7 +66,7 @@ int px4_simple_app_main(int argc, char *argv[])
orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
/* one could wait for multiple topics with this technique, just using one here */
struct pollfd fds[] = {
px4_pollfd_struct_t fds[] = {
{ .fd = sensor_sub_fd, .events = POLLIN },
/* there could be more file descriptors here, in the form like:
* { .fd = other_sub_fd, .events = POLLIN },
@@ -75,19 +77,19 @@ int px4_simple_app_main(int argc, char *argv[])
for (int i = 0; i < 5; i++) {
/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
int poll_ret = poll(fds, 1, 1000);
int poll_ret = px4_poll(fds, 1, 1000);
/* handle the poll result */
if (poll_ret == 0) {
/* this means none of our providers is giving us data */
printf("[px4_simple_app] Got no data within a second\n");
PX4_ERR("[px4_simple_app] Got no data within a second");
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
printf("[px4_simple_app] ERROR return value from poll(): %d\n"
, poll_ret);
PX4_ERR("[px4_simple_app] ERROR return value from poll(): %d"
, poll_ret);
}
error_counter++;
@@ -99,10 +101,10 @@ int px4_simple_app_main(int argc, char *argv[])
struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
printf("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f\n",
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0];
@@ -117,5 +119,7 @@ int px4_simple_app_main(int argc, char *argv[])
}
}
PX4_INFO("exiting");
return 0;
}
src/lib/rc/dsm.c
+251 -111
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2015 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
@@ -40,7 +40,8 @@
*/
#include <px4_config.h>
#include <nuttx/arch.h>
#include <board_config.h>
#include <px4_defines.h>
#include <fcntl.h>
#include <unistd.h>
@@ -49,27 +50,32 @@
#include "dsm.h"
#include <drivers/drv_hrt.h>
#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
#include <px4io.h>
#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
#define dsm_udelay(arg) usleep(arg)
#else
#include <nuttx/arch.h>
#define dsm_udelay(arg) up_udelay(arg)
#endif
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#include <px4io.h>
#endif
//#define DSM_DEBUG
#define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/
#define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/
static enum DSM_DECODE_STATE {
DSM_DECODE_STATE_DESYNC = 0,
DSM_DECODE_STATE_SYNC
} dsm_decode_state = DSM_DECODE_STATE_DESYNC;
static int dsm_fd = -1; /**< File handle to the DSM UART */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */
static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */
static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received data */
static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last valid dsm frame */
static uint8_t dsm_frame[DSM_BUFFER_SIZE]; /**< DSM dsm frame receive buffer */
static uint8_t dsm_buf[DSM_FRAME_SIZE * 2];
static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */
static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */
static unsigned dsm_channel_shift = 0; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
static unsigned dsm_frame_drops = 0; /**< Count of incomplete DSM frames */
static uint16_t dsm_chan_count = 0; /**< DSM channel count */
static bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values);
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values);
/**
* Attempt to decode a single channel raw channel datum
@@ -119,12 +125,12 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
*
* @param[in] reset true=reset the 10/11 bit state to unknown
*/
static void
static bool
dsm_guess_format(bool reset)
{
static uint32_t cs10;
static uint32_t cs11;
static unsigned samples;
static uint32_t cs10 = 0;
static uint32_t cs11 = 0;
static unsigned samples = 0;
/* reset the 10/11 bit sniffed channel masks */
if (reset) {
@@ -132,7 +138,7 @@ dsm_guess_format(bool reset)
cs11 = 0;
samples = 0;
dsm_channel_shift = 0;
return;
return false;
}
/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
@@ -154,9 +160,16 @@ dsm_guess_format(bool reset)
/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
}
samples++;
#ifdef DSM_DEBUG
printf("dsm guess format: samples: %d %s\n", samples,
(reset) ? "RESET" : "");
#endif
/* wait until we have seen plenty of frames - 5 should normally be enough */
if (samples++ < 5) {
return;
if (samples < 5) {
return false;
}
/*
@@ -195,40 +208,37 @@ dsm_guess_format(bool reset)
if ((votes11 == 1) && (votes10 == 0)) {
dsm_channel_shift = 11;
//debug("DSM: 11-bit format");
return;
#ifdef DSM_DEBUG
printf("DSM: 11-bit format\n");
#endif
return true;
}
if ((votes10 == 1) && (votes11 == 0)) {
dsm_channel_shift = 10;
//debug("DSM: 10-bit format");
return;
#ifdef DSM_DEBUG
printf("DSM: 10-bit format\n");
#endif
return true;
}
/* call ourselves to reset our state ... we have to try again */
//debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
#ifdef DSM_DEBUG
printf("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d\n", cs10, votes10, cs11, votes11);
#endif
dsm_guess_format(true);
return false;
}
/**
* Initialize the DSM receive functionality
*
* Open the UART for receiving DSM frames and configure it appropriately
*
* @param[in] device Device name of DSM UART
*/
int
dsm_init(const char *device)
dsm_config(int dsm_fd)
{
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
#ifdef GPIO_SPEKTRUM_PWR_EN
// enable power on DSM connector
POWER_SPEKTRUM(true);
#endif
if (dsm_fd < 0) {
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
}
int ret = -1;
if (dsm_fd >= 0) {
@@ -247,17 +257,43 @@ dsm_init(const char *device)
/* reset the format detector */
dsm_guess_format(true);
//debug("DSM: ready");
} else {
//debug("DSM: open failed");
ret = 0;
}
return dsm_fd;
return ret;
}
/**
* Initialize the DSM receive functionality
*
* Open the UART for receiving DSM frames and configure it appropriately
*
* @param[in] device Device name of DSM UART
*/
int
dsm_init(const char *device)
{
if (dsm_fd < 0) {
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
}
dsm_channel_shift = 0;
dsm_frame_drops = 0;
dsm_chan_count = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
int ret = dsm_config(dsm_fd);
if (!ret) {
return dsm_fd;
} else {
return -1;
}
}
#ifdef GPIO_SPEKTRUM_PWR_EN
/**
* Handle DSM satellite receiver bind mode handler
*
@@ -267,64 +303,52 @@ dsm_init(const char *device)
void
dsm_bind(uint16_t cmd, int pulses)
{
#if !defined(GPIO_USART1_RX_SPEKTRUM)
#else
if (dsm_fd < 0) {
return;
}
switch (cmd) {
case dsm_bind_power_down:
case DSM_CMD_BIND_POWER_DOWN:
/*power down DSM satellite*/
#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(0);
#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(0);
#endif
break;
case dsm_bind_power_up:
case DSM_CMD_BIND_POWER_UP:
/*power up DSM satellite*/
#if defined(CONFIG_ARCH_BOARD_PX4IO_V1)
POWER_RELAY1(1);
#elif defined(CONFIG_ARCH_BOARD_PX4IO_V2)
POWER_SPEKTRUM(1);
#endif
dsm_guess_format(true);
break;
case dsm_bind_set_rx_out:
case DSM_CMD_BIND_SET_RX_OUT:
/*Set UART RX pin to active output mode*/
stm32_configgpio(GPIO_USART1_RX_SPEKTRUM);
SPEKTRUM_RX_AS_GPIO();
break;
case dsm_bind_send_pulses:
case DSM_CMD_BIND_SEND_PULSES:
/*Pulse RX pin a number of times*/
for (int i = 0; i < pulses; i++) {
up_udelay(120);
stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, false);
up_udelay(120);
stm32_gpiowrite(GPIO_USART1_RX_SPEKTRUM, true);
dsm_udelay(120);
SPEKTRUM_RX_HIGH(false);
dsm_udelay(120);
SPEKTRUM_RX_HIGH(true);
}
break;
case dsm_bind_reinit_uart:
case DSM_CMD_BIND_REINIT_UART:
/*Restore USART RX pin to RS232 receive mode*/
stm32_configgpio(GPIO_USART1_RX);
SPEKTRUM_RX_AS_UART();
break;
}
#endif
}
#endif
/**
* Decode the entire dsm frame (all contained channels)
@@ -335,7 +359,7 @@ dsm_bind(uint16_t cmd, int pulses)
* @return true=DSM frame successfully decoded, false=no update
*/
bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsigned max_values)
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, unsigned max_values)
{
/*
debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
@@ -350,13 +374,11 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
dsm_guess_format(true);
}
/* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time;
/* if we don't know the dsm_frame format, update the guessing state machine */
if (dsm_channel_shift == 0) {
dsm_guess_format(false);
return false;
if (!dsm_guess_format(false)) {
return false;
}
}
/*
@@ -380,6 +402,12 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
continue;
}
/* reset bit guessing state machine if the channel index is out of bounds */
if (channel > DSM_MAX_CHANNEL_COUNT) {
dsm_guess_format(true);
return false;
}
/* ignore channels out of range */
if (channel >= max_values) {
continue;
@@ -447,14 +475,32 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
*num_values = 12;
}
if (dsm_channel_shift == 11) {
/* Set the 11-bit data indicator */
*num_values |= 0x8000;
}
/* Set the 11-bit data indicator */
*dsm_11_bit = (dsm_channel_shift == 11);
/* we have received something we think is a dsm_frame */
dsm_last_frame_time = frame_time;
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/
#ifdef DSM_DEBUG
printf("PARSED PACKET\n");
#endif
/* check all values */
for (unsigned i = 0; i < *num_values; i++) {
/* if the value is unrealistic, fail the parsing entirely */
if (values[i] < 600 || values[i] > 2400) {
#ifdef DSM_DEBUG
printf("DSM: VALUE RANGE FAIL\n");
#endif
dsm_chan_count = 0;
return false;
}
}
return true;
}
@@ -480,27 +526,35 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, unsig
* @return true=decoded raw channel values updated, false=no update
*/
bool
dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values)
dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes, uint8_t **bytes,
unsigned max_values)
{
ssize_t ret;
int ret = 1;
hrt_abstime now;
/*
* The S.BUS protocol doesn't provide reliable framing,
* so we detect frame boundaries by the inter-frame delay.
*
* The minimum frame spacing is 7ms; with 25 bytes at 100000bps
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
* and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
if ((now - dsm_last_rx_time) > 5000) {
if (dsm_partial_frame_count > 0) {
dsm_frame_drops++;
dsm_partial_frame_count = 0;
}
}
/*
* Fetch bytes, but no more than we would need to complete
* the current dsm frame.
* a complete frame.
*/
ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
ret = read(dsm_fd, &dsm_buf[0], sizeof(dsm_buf) / sizeof(dsm_buf[0]));
/* if the read failed for any reason, just give up here */
if (ret < 1) {
@@ -508,27 +562,113 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by
} else {
*n_bytes = ret;
*bytes = &dsm_frame[dsm_partial_frame_count];
*bytes = &dsm_buf[0];
}
/*
* Try to decode something with what we got
*/
return dsm_parse(now, &dsm_buf[0], ret, values, num_values, dsm_11_bit, &dsm_frame_drops, max_values);
}
bool
dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels)
{
/* this is set by the decoding state machine and will default to false
* once everything that was decodable has been decoded.
*/
bool decode_ret = false;
/* keep decoding until we have consumed the buffer */
for (unsigned d = 0; d < len; d++) {
/* overflow check */
if (dsm_partial_frame_count == sizeof(dsm_frame) / sizeof(dsm_frame[0])) {
dsm_partial_frame_count = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
#ifdef DSM_DEBUG
printf("DSM: RESET (BUF LIM)\n");
#endif
}
if (dsm_partial_frame_count == DSM_FRAME_SIZE) {
dsm_partial_frame_count = 0;
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
#ifdef DSM_DEBUG
printf("DSM: RESET (PACKET LIM)\n");
#endif
}
#ifdef DSM_DEBUG
#if 1
printf("dsm state: %s%s, count: %d, val: %02x\n",
(dsm_decode_state == DSM_DECODE_STATE_DESYNC) ? "DSM_DECODE_STATE_DESYNC" : "",
(dsm_decode_state == DSM_DECODE_STATE_SYNC) ? "DSM_DECODE_STATE_SYNC" : "",
dsm_partial_frame_count,
(unsigned)frame[d]);
#endif
#endif
switch (dsm_decode_state) {
case DSM_DECODE_STATE_DESYNC:
/* we are de-synced and only interested in the frame marker */
if ((now - dsm_last_rx_time) > 5000) {
dsm_decode_state = DSM_DECODE_STATE_SYNC;
dsm_partial_frame_count = 0;
dsm_chan_count = 0;
dsm_frame[dsm_partial_frame_count++] = frame[d];
}
break;
case DSM_DECODE_STATE_SYNC: {
dsm_frame[dsm_partial_frame_count++] = frame[d];
/* decode whatever we got and expect */
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
break;
}
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
decode_ret = dsm_decode(now, values, &dsm_chan_count, dsm_11_bit, max_channels);
/* we consumed the partial frame, reset */
dsm_partial_frame_count = 0;
/* if decoding failed, set proto to desync */
if (decode_ret == false) {
dsm_decode_state = DSM_DECODE_STATE_DESYNC;
dsm_frame_drops++;
}
}
break;
default:
#ifdef DSM_DEBUG
printf("UNKNOWN PROTO STATE");
#endif
decode_ret = false;
}
}
if (frame_drops) {
*frame_drops = dsm_frame_drops;
}
if (decode_ret) {
*num_values = dsm_chan_count;
}
dsm_last_rx_time = now;
/*
* Add bytes to the current dsm frame
*/
dsm_partial_frame_count += ret;
/*
* If we don't have a full dsm frame, return
*/
if (dsm_partial_frame_count < DSM_FRAME_SIZE) {
return false;
}
/*
* Great, it looks like we might have a dsm frame. Go ahead and
* decode it.
*/
dsm_partial_frame_count = 0;
return dsm_decode(now, values, num_values, max_values);
/* return false as default */
return decode_ret;
}
src/lib/rc/dsm.h
+25 -1
View File
@@ -42,11 +42,35 @@
#pragma once
#include <stdint.h>
#include <px4_config.h>
#include <board_config.h>
#include <px4_defines.h>
__BEGIN_DECLS
#define DSM_FRAME_SIZE 16 /**< DSM frame size in bytes */
#define DSM_FRAME_CHANNELS 7 /**< Max supported DSM channels per frame */
#define DSM_MAX_CHANNEL_COUNT 18 /**< Max channel count of any DSM RC */
#define DSM_BUFFER_SIZE (DSM_FRAME_SIZE + DSM_FRAME_SIZE / 2)
__EXPORT int dsm_init(const char *device);
__EXPORT bool dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **bytes, unsigned max_values);
__EXPORT int dsm_config(int dsm_fd);
__EXPORT bool dsm_input(int dsm_fd, uint16_t *values, uint16_t *num_values, bool *dsm_11_bit, uint8_t *n_bytes,
uint8_t **bytes, unsigned max_values);
__EXPORT bool dsm_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
uint16_t *num_values, bool *dsm_11_bit, unsigned *frame_drops, uint16_t max_channels);
#ifdef GPIO_SPEKTRUM_PWR_EN
__EXPORT void dsm_bind(uint16_t cmd, int pulses);
#endif
enum DSM_CMD { /* DSM bind states */
DSM_CMD_BIND_POWER_DOWN = 0,
DSM_CMD_BIND_POWER_UP,
DSM_CMD_BIND_SET_RX_OUT,
DSM_CMD_BIND_SEND_PULSES,
DSM_CMD_BIND_REINIT_UART
};
__END_DECLS
src/lib/rc/sbus.c
+24 -3
View File
@@ -44,6 +44,10 @@
#include <termios.h>
#include <string.h>
#ifdef TIOCSSINGLEWIRE
#include <sys/ioctl.h>
#endif
#include "sbus.h"
#include <drivers/drv_hrt.h>
@@ -122,6 +126,21 @@ sbus_init(const char *device, bool singlewire)
{
int sbus_fd = open(device, O_RDWR | O_NONBLOCK);
int ret = sbus_config(sbus_fd, singlewire);
if (!ret) {
return sbus_fd;
} else {
return -1;
}
}
int
sbus_config(int sbus_fd, bool singlewire)
{
int ret = -1;
if (sbus_fd >= 0) {
struct termios t;
@@ -133,8 +152,8 @@ sbus_init(const char *device, bool singlewire)
if (singlewire) {
/* only defined in configs capable of IOCTL */
#ifdef SBUS_SERIAL_PORT
//ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#ifdef TIOCSSINGLEWIRE
ioctl(sbus_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
#endif
}
@@ -143,9 +162,11 @@ sbus_init(const char *device, bool singlewire)
last_rx_time = hrt_absolute_time();
last_frame_time = last_rx_time;
sbus_frame_drops = 0;
ret = 0;
}
return sbus_fd;
return ret;
}
void
src/lib/rc/sbus.h
+1
View File
@@ -68,6 +68,7 @@ __EXPORT int sbus_init(const char *device, bool singlewire);
* provides a degree of protection. Of course, it would be better
* if we didn't drop bytes...
*/
__EXPORT int sbus_config(int sbus_fd, bool singlewire);
__EXPORT bool sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe,
bool *sbus_frame_drop,
uint16_t max_channels);
src/modules/navigator/mission.cpp
+5 -3
View File
@@ -303,7 +303,7 @@ Mission::advance_mission()
}
}
int
float
Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
{
if (_mission_item.altitude_is_relative) {
@@ -656,8 +656,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
for (int i = 0; i < 10; i++) {
if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
/* mission item index out of bounds */
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
/* mission item index out of bounds - if they are equal, we just reached the end */
if (*mission_index_ptr != (int)mission->count) {
mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
}
return false;
}
src/modules/navigator/mission.h
+1 -1
View File
@@ -133,7 +133,7 @@ private:
*/
void altitude_sp_foh_reset();
int get_absolute_altitude_for_item(struct mission_item_s &mission_item);
float get_absolute_altitude_for_item(struct mission_item_s &mission_item);
/**
* Read current or next mission item from the dataman and watch out for DO_JUMPS
src/modules/px4iofirmware/controls.c
+4 -4
View File
@@ -74,15 +74,15 @@ static unsigned _rssi_adc_counts = 0;
bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
{
perf_begin(c_gather_dsm);
uint16_t temp_count = r_raw_rc_count;
uint8_t n_bytes = 0;
uint8_t *bytes;
*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes, PX4IO_RC_INPUT_CHANNELS);
bool dsm_11_bit;
*dsm_updated = dsm_input(_dsm_fd, r_raw_rc_values, &r_raw_rc_count, &dsm_11_bit, &n_bytes, &bytes,
PX4IO_RC_INPUT_CHANNELS);
if (*dsm_updated) {
r_raw_rc_count = temp_count & 0x7fff;
if (temp_count & 0x8000) {
if (dsm_11_bit) {
r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
} else {
src/modules/px4iofirmware/px4io.h
-1
View File
@@ -170,7 +170,6 @@ extern pwm_limit_t pwm_limit;
#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
# define PX4IO_RELAY_CHANNELS 0
# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
# define ENABLE_SBUS_OUT(_s) stm32_gpiowrite(GPIO_SBUS_OENABLE, !(_s))
# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
src/platforms/posix/main.cpp
+42 -1
View File
@@ -44,6 +44,8 @@
#include <sstream>
#include <vector>
#include <signal.h>
#include <unistd.h>
#include <stdio.h>
#include "apps.h"
#include "px4_middleware.h"
#include "DriverFramework.hpp"
@@ -157,6 +159,7 @@ static void process_line(string &line, bool exit_on_fail)
int main(int argc, char **argv)
{
bool daemon_mode = false;
bool chroot_on = false;
signal(SIGINT, _SigIntHandler);
signal(SIGFPE, _SigFpeHandler);
@@ -174,6 +177,9 @@ int main(int argc, char **argv)
usage();
return 0;
} else if (strcmp(argv[index], "-c") == 0) {
chroot_on = true;
} else {
PX4_WARN("Unknown/unhandled parameter: %s", argv[index]);
return 1;
@@ -203,7 +209,7 @@ int main(int argc, char **argv)
px4::init(argc, argv, "mainapp");
//if commandfile is present, process the commands from the file
// if commandfile is present, process the commands from the file
if (commands_file != nullptr) {
ifstream infile(commands_file);
@@ -217,6 +223,41 @@ int main(int argc, char **argv)
}
}
if (chroot_on) {
// Lock this application in the current working dir
// this is not an attempt to secure the environment,
// rather, to replicate a deployed file system.
#ifdef PATH_MAX
const unsigned path_max_len = PATH_MAX;
#else
const unsigned path_max_len = 1024;
#endif
char pwd_path[path_max_len];
const char *folderpath = "/rootfs/";
if (nullptr == getcwd(pwd_path, sizeof(pwd_path))) {
PX4_ERR("Failed aquiring working dir, abort.");
exit(1);
}
if (nullptr == strcat(pwd_path, folderpath)) {
PX4_ERR("Failed completing path, abort.");
exit(1);
}
if (chroot(pwd_path)) {
PX4_ERR("Failed chrooting application, path: %s, error: %s.", pwd_path, strerror(errno));
exit(1);
}
if (chdir("/")) {
PX4_ERR("Failed changing to root dir, path: %s, error: %s.", pwd_path, strerror(errno));
exit(1);
}
}
if (!daemon_mode) {
string mystr = "";
string string_buffer[CMD_BUFF_SIZE];
unittests/CMakeLists.txt
+6
View File
@@ -153,6 +153,12 @@ add_executable(sbus2_test sbus2_test.cpp hrt.cpp
target_link_libraries( sbus2_test px4_platform )
add_gtest(sbus2_test)
# DSM test
add_executable(dsm_test dsm_test.cpp hrt.cpp
${PX_SRC}/lib/rc/dsm.c)
target_link_libraries( dsm_test px4_platform )
add_gtest(dsm_test)
# st24_test
add_executable(rc_input_test st24_test.cpp hrt.cpp ${PX_SRC}/lib/rc/st24.c sumd_test.cpp ${PX_SRC}/lib/rc/sumd.c)
target_link_libraries(rc_input_test px4_platform)
unittests/dsm_test.cpp
+74
View File
@@ -0,0 +1,74 @@
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "../../src/systemcmds/tests/tests.h"
#include <drivers/drv_hrt.h>
// Enable DSM parser output
#define DSM_DEBUG
#include <rc/dsm.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include "gtest/gtest.h"
TEST(DSMTest, DSM)
{
const char *filepath = "testdata/dsm_x_data.txt";
FILE *fp;
fp = fopen(filepath, "rt");
ASSERT_TRUE(fp);
warnx("loading data from: %s", filepath);
float f;
unsigned x;
int ret;
// Trash the first 20 lines
for (unsigned i = 0; i < 20; i++) {
char buf[200];
(void)fgets(buf, sizeof(buf), fp);
}
// Init the parser
uint8_t frame[20];
uint16_t rc_values[18];
uint16_t num_values;
bool dsm_11_bit;
unsigned dsm_frame_drops = 0;
uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]);
int rate_limiter = 0;
unsigned last_drop = 0;
while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
ASSERT_GT(ret, 0);
frame[0] = x;
unsigned len = 1;
// Pipe the data into the parser
bool result = dsm_parse(f*1e6, &frame[0], len, rc_values, &num_values,
&dsm_11_bit, &dsm_frame_drops, max_channels);
if (result) {
warnx("decoded packet with %d channels and %s encoding:", num_values, (dsm_11_bit) ? "11 bit" : "10 bit");
for (unsigned i = 0; i < num_values; i++) {
printf("chan #%u:\t%d\n", i, (int)rc_values[i]);
}
}
if (last_drop != (dsm_frame_drops)) {
warnx("frame dropped, now #%d", (dsm_frame_drops));
last_drop = dsm_frame_drops;
}
rate_limiter++;
}
ASSERT_EQ(ret, EOF);
}
unittests/testdata/dsm_x_data.txt
Vendored
+1106
View File
File diff suppressed because it is too large Load Diff