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

Merge remote-tracking branch 'upstream/master' into fw_att_master

Browse Source
This commit is contained in:
Thomas Gubler
2013-11-01 14:41:01 +01:00
parent 820d19eb02 2fd1aed6be
commit cc47ed886a
55 changed files with 1709 additions and 833 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/fixed_wing_control.odg
BIN
View File
Binary file not shown.
Documentation/l1_control.odg
BIN
View File
Binary file not shown.
ROMFS/px4fmu_common/init.d/10_dji_f330
+10 -4
View File
@@ -59,11 +59,10 @@ then
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
@@ -81,7 +80,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
#
# Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/11_dji_f450
+10 -5
View File
@@ -43,11 +43,9 @@ then
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals (for DJI ESCs)
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
#
# Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/15_tbs_discovery
+10 -5
View File
@@ -43,11 +43,9 @@ then
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals (for DJI ESCs)
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
#
# Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/16_3dr_iris
+10 -5
View File
@@ -43,11 +43,9 @@ then
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
@@ -65,7 +63,14 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_w.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
pwm rate -c 1234 -r 400
#
# Set disarmed, min and max PWM signals
#
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
#
# Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/666_fmu_q_x550
+1 -1
View File
@@ -53,7 +53,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
pwm rate -c 1234 -r 400
#
# Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
+4 -6
View File
@@ -83,10 +83,6 @@ then
usleep 5000
sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs
px4io idle 900 900 900 900
px4io min 1200 1200 1200 1200
px4io max 1800 1800 1800 1800
else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0)
@@ -107,9 +103,11 @@ else
fi
#
# Set PWM output frequency
# Set disarmed, min and max PWM signals
#
#pwm -u 400 -m 0xff
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 1200
pwm max -c 1234 -p 1800
#
# Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/rc.custom_io_esc
+16 -14
View File
@@ -61,20 +61,9 @@ then
mavlink start -d /dev/ttyS1 -b 57600
usleep 5000
sh /etc/init.d/rc.io
commander start
if [ $ESC_MAKER = afro ]
then
# Set PWM values for Afro ESCs
px4io idle 1050 1050 1050 1050
px4io min 1080 1080 1080 1080
px4io max 1860 1860 1860 1860
else
# Set PWM values for typical ESCs
px4io idle 900 900 900 900
px4io min 1110 1100 1100 1100
px4io max 1800 1800 1800 1800
fi
sh /etc/init.d/rc.io
else
fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0)
@@ -84,6 +73,19 @@ else
set EXIT_ON_END yes
fi
if [ $ESC_MAKER = afro ]
then
# Set PWM values for Afro ESCs
pwm disarmed -c 1234 -p 1050
pwm min -c 1234 -p 1080
pwm max -c 1234 -p 1860
else
# Set PWM values for typical ESCs
pwm disarmed -c 1234 -p 900
pwm min -c 1234 -p 980
pwm max -c 1234 -p 1800
fi
#
# Load mixer
#
@@ -105,7 +107,7 @@ fi
#
# Set PWM output frequency
#
pwm -u 400 -m 0xff
pwm rate -r 400 -c 1234
#
# Start common for all multirotors apps
ROMFS/px4fmu_common/init.d/rc.multirotor
-5
View File
@@ -13,11 +13,6 @@ sh /etc/init.d/rc.sensors
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
ROMFS/px4fmu_common/init.d/rcS
+7 -7
View File
@@ -265,13 +265,13 @@ then
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 22
then
set FRAME_GEOMETRY w
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 22
then
set FRAME_GEOMETRY w
sh /etc/init.d/rc.custom_io_esc
set MODE custom
fi
if param compare SYS_AUTOSTART 30
then
ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix
+7
View File
@@ -0,0 +1,7 @@
Multirotor mixer for PX4FMU
===========================
This file defines a single mixer for a Coaxial Octocopter in the X configuration. All controls
are mixed 100%.
R: 8c 10000 10000 10000 0
makefiles/config_px4fmu-v1_default.mk
+1 -1
View File
@@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
MODULES += modules/segway
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
makefiles/config_px4fmu-v2_default.mk
+2 -2
View File
@@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator
#
# Vehicle Control
#
MODULES += modules/segway
#MODULES += modules/segway # XXX Needs GCC 4.7 fix
MODULES += modules/fw_pos_control_l1
MODULES += modules/fw_att_control
MODULES += modules/multirotor_att_control
@@ -120,7 +120,7 @@ MODULES += lib/conversion
#MODULES += examples/math_demo
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/hello_sky
#MODULES += examples/px4_simple_app
MODULES += examples/px4_simple_app
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/daemon
src/drivers/drv_accel.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Accelerometer driver interface.
* @file drv_accel.h
*
* Accelerometer driver interface.
*/
#ifndef _DRV_ACCEL_H
src/drivers/drv_airspeed.h
+4 -1
View File
@@ -32,7 +32,10 @@
****************************************************************************/
/**
* @file Airspeed driver interface.
* @file drv_airspeed.h
*
* Airspeed driver interface.
*
* @author Simon Wilks
*/
src/drivers/drv_baro.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Barometric pressure sensor driver interface.
* @file drv_baro.h
*
* Barometric pressure sensor driver interface.
*/
#ifndef _DRV_BARO_H
src/drivers/drv_gps.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file GPS driver interface.
* @file drv_gps.h
*
* GPS driver interface.
*/
#ifndef _DRV_GPS_H
src/drivers/drv_gyro.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Gyroscope driver interface.
* @file drv_gyro.h
*
* Gyroscope driver interface.
*/
#ifndef _DRV_GYRO_H
src/drivers/drv_pwm_output.h
+72 -8
View File
@@ -64,6 +64,36 @@ __BEGIN_DECLS
*/
#define PWM_OUTPUT_MAX_CHANNELS 16
/**
* Lowest minimum PWM in us
*/
#define PWM_LOWEST_MIN 900
/**
* Default minimum PWM in us
*/
#define PWM_DEFAULT_MIN 1000
/**
* Highest PWM allowed as the minimum PWM
*/
#define PWM_HIGHEST_MIN 1300
/**
* Highest maximum PWM in us
*/
#define PWM_HIGHEST_MAX 2100
/**
* Default maximum PWM in us
*/
#define PWM_DEFAULT_MAX 2000
/**
* Lowest PWM allowed as the maximum PWM
*/
#define PWM_LOWEST_MAX 1700
/**
* Servo output signal type, value is actual servo output pulse
* width in microseconds.
@@ -79,6 +109,7 @@ typedef uint16_t servo_position_t;
struct pwm_output_values {
/** desired pulse widths for each of the supported channels */
servo_position_t values[PWM_OUTPUT_MAX_CHANNELS];
unsigned channel_count;
};
/*
@@ -100,30 +131,63 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** get default servo update rate */
#define PWM_SERVO_GET_DEFAULT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** set alternate servo update rate */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 3)
/** get alternate servo update rate */
#define PWM_SERVO_GET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** get the number of servos in *(unsigned *)arg */
#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3)
#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 5)
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
#define PWM_SERVO_SET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 6)
/** check the selected update rates */
#define PWM_SERVO_GET_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 7)
/** set the 'ARM ok' bit, which activates the safety switch */
#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 5)
#define PWM_SERVO_SET_ARM_OK _IOC(_PWM_SERVO_BASE, 8)
/** clear the 'ARM ok' bit, which deactivates the safety switch */
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6)
#define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 9)
/** start DSM bind */
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7)
#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 10)
#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
/** Power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8)
/** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)
/** set the PWM value for failsafe */
#define PWM_SERVO_SET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 12)
/** get the PWM value for failsafe */
#define PWM_SERVO_GET_FAILSAFE_PWM _IOC(_PWM_SERVO_BASE, 13)
/** set the PWM value when disarmed - should be no PWM (zero) by default */
#define PWM_SERVO_SET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 14)
/** get the PWM value when disarmed */
#define PWM_SERVO_GET_DISARMED_PWM _IOC(_PWM_SERVO_BASE, 15)
/** set the minimum PWM value the output will send */
#define PWM_SERVO_SET_MIN_PWM _IOC(_PWM_SERVO_BASE, 16)
/** get the minimum PWM value the output will send */
#define PWM_SERVO_GET_MIN_PWM _IOC(_PWM_SERVO_BASE, 17)
/** set the maximum PWM value the output will send */
#define PWM_SERVO_SET_MAX_PWM _IOC(_PWM_SERVO_BASE, 18)
/** get the maximum PWM value the output will send */
#define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
src/drivers/drv_rc_input.h
+2 -1
View File
@@ -32,8 +32,9 @@
****************************************************************************/
/**
* @file R/C input interface.
* @file drv_rc_input.h
*
* R/C input interface.
*/
#ifndef _DRV_RC_INPUT_H
src/drivers/drv_sensor.h
+3 -1
View File
@@ -32,7 +32,9 @@
****************************************************************************/
/**
* @file Common sensor API and ioctl definitions.
* @file drv_sensor.h
*
* Common sensor API and ioctl definitions.
*/
#ifndef _DRV_SENSOR_H
src/drivers/drv_tone_alarm.h
+3 -1
View File
@@ -31,7 +31,9 @@
*
****************************************************************************/
/*
/**
* @file drv_tone_alarm.h
*
* Driver for the PX4 audio alarm port, /dev/tone_alarm.
*
* The tone_alarm driver supports a set of predefined "alarm"
src/drivers/hil/hil.cpp
+2 -2
View File
@@ -404,7 +404,7 @@ HIL::task_main()
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < (unsigned)outputs.noutputs &&
if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
@@ -517,7 +517,7 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate
break;
src/drivers/lsm303d/lsm303d.cpp
+29 -3
View File
@@ -241,11 +241,18 @@ private:
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _reg7_resets;
perf_counter_t _reg1_resets;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
// expceted values of reg1 and reg7 to catch in-flight
// brownouts of the sensor
uint8_t _reg7_expected;
uint8_t _reg1_expected;
/**
* Start automatic measurement.
*/
@@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_reg1_expected(0),
_reg7_expected(0)
{
// enable debug() calls
_debug_enabled = true;
@@ -526,10 +537,12 @@ void
LSM303D::reset()
{
/* enable accel*/
write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
write_reg(ADDR_CTRL_REG1, _reg1_expected);
/* enable mag */
write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
_reg7_expected = REG7_CONT_MODE_M;
write_reg(ADDR_CTRL_REG7, _reg7_expected);
write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
@@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency)
}
modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
return OK;
}
@@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg)
void
LSM303D::measure()
{
if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
perf_count(_reg1_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
@@ -1282,6 +1302,12 @@ LSM303D::measure()
void
LSM303D::mag_measure()
{
if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
perf_count(_reg7_resets);
reset();
return;
}
/* status register and data as read back from the device */
#pragma pack(push, 1)
struct {
src/drivers/mkblctrl/mkblctrl.cpp
+1 -1
View File
@@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK;
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK;
break;
src/drivers/px4fmu/fmu.cpp
+289 -72
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -59,11 +59,12 @@
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
# include <board_config.h>
#include <board_config.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
@@ -72,11 +73,17 @@
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
#ifdef HRT_PPM_CHANNEL
# 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_MS 20
class PX4FMU : public device::CDev
{
public:
@@ -100,7 +107,12 @@ public:
int set_pwm_alt_channels(uint32_t channels);
private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
static const unsigned _max_actuators = 4;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
static const unsigned _max_actuators = 6;
#endif
Mode _mode;
unsigned _pwm_default_rate;
@@ -117,11 +129,20 @@ private:
volatile bool _task_should_exit;
bool _armed;
bool _pwm_on;
MixerGroup *_mixers;
actuator_controls_s _controls;
pwm_limit_t _pwm_limit;
uint16_t _failsafe_pwm[_max_actuators];
uint16_t _disarmed_pwm[_max_actuators];
uint16_t _min_pwm[_max_actuators];
uint16_t _max_pwm[_max_actuators];
unsigned _num_failsafe_set;
unsigned _num_disarmed_set;
static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn));
@@ -203,8 +224,18 @@ PX4FMU::PX4FMU() :
_primary_pwm_device(false),
_task_should_exit(false),
_armed(false),
_mixers(nullptr)
_pwm_on(false),
_mixers(nullptr),
_failsafe_pwm({0}),
_disarmed_pwm({0}),
_num_failsafe_set(0),
_num_disarmed_set(0)
{
for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN;
_max_pwm[i] = PWM_DEFAULT_MAX;
}
_debug_enabled = true;
}
@@ -457,6 +488,9 @@ PX4FMU::task_main()
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
#endif
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
log("starting");
/* loop until killed */
@@ -491,90 +525,103 @@ PX4FMU::task_main()
/* sleep waiting for data, stopping to check for PPM
* input at 100Hz */
int ret = ::poll(&fds[0], 2, 10);
int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
} else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
} else {
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* do we have a control update? */
if (fds[0].revents & POLLIN) {
/* can we mix? */
if (_mixers != nullptr) {
/* get controls - must always do this to avoid spinning */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
unsigned num_outputs;
/* can we mix? */
if (_mixers != nullptr) {
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
}
unsigned num_outputs;
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
// XXX output actual limited values
memcpy(&controls_effective, &_controls, sizeof(controls_effective));
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i < outputs.noutputs &&
isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f &&
outputs.output[i] <= 1.0f) {
/* scale for PWM output 900 - 2100us */
outputs.output[i] = 1500 + (600 * outputs.output[i]);
} else {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = 900;
switch (_mode) {
case MODE_2PWM:
num_outputs = 2;
break;
case MODE_4PWM:
num_outputs = 4;
break;
case MODE_6PWM:
num_outputs = 6;
break;
default:
num_outputs = 0;
break;
}
/* output to the servo */
up_pwm_servo_set(i, outputs.output[i]);
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */
for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */
if (i >= outputs.noutputs ||
!isfinite(outputs.output[i]) ||
outputs.output[i] < -1.0f ||
outputs.output[i] > 1.0f) {
/*
* Value is NaN, INF or out of band - set to the minimum value.
* This will be clearly visible on the servo status and will limit the risk of accidentally
* spinning motors. It would be deadly in flight.
*/
outputs.output[i] = -1.0f;
}
}
uint16_t pwm_limited[num_outputs];
pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
/* output actual limited values */
for (unsigned i = 0; i < num_outputs; i++) {
controls_effective.control_effective[i] = (float)pwm_limited[i];
}
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
/* output to the servos */
for (unsigned i = 0; i < num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
/* and publish for anyone that cares to see */
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
}
}
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
actuator_armed_s aa;
/* how about an arming update? */
if (fds[1].revents & POLLIN) {
actuator_armed_s aa;
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* get new value */
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update PWM servo armed status if armed and not locked down */
bool set_armed = aa.armed && !aa.lockdown;
if (set_armed != _armed) {
_armed = set_armed;
up_pwm_servo_arm(set_armed);
/* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown;
if (_armed != set_armed)
_armed = set_armed;
/* update PWM status if armed or if disarmed PWM values are set */
bool pwm_on = (aa.armed || _num_disarmed_set > 0);
if (_pwm_on != pwm_on) {
_pwm_on = pwm_on;
up_pwm_servo_arm(pwm_on);
}
}
}
@@ -685,14 +732,164 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
up_pwm_servo_arm(false);
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_default_rate;
break;
case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate;
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_alt_rate_channels;
break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_failsafe_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_failsafe_pwm[i] = PWM_LOWEST_MIN;
} else {
_failsafe_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_failsafe_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_failsafe_pwm[i] > 0)
_num_failsafe_set++;
}
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _failsafe_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_disarmed_pwm[i] = PWM_HIGHEST_MAX;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_disarmed_pwm[i] = PWM_LOWEST_MIN;
} else {
_disarmed_pwm[i] = pwm->values[i];
}
}
/*
* update the counter
* this is needed to decide if disarmed PWM output should be turned on or not
*/
_num_disarmed_set = 0;
for (unsigned i = 0; i < _max_actuators; i++) {
if (_disarmed_pwm[i] > 0)
_num_disarmed_set++;
}
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _disarmed_pwm[i];
}
pwm->channel_count = _max_actuators;
break;
}
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
_min_pwm[i] = PWM_HIGHEST_MIN;
} else if (pwm->values[i] < PWM_LOWEST_MIN) {
_min_pwm[i] = PWM_LOWEST_MIN;
} else {
_min_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _min_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
/* discard if too many values are sent */
if (pwm->channel_count > _max_actuators) {
ret = -EINVAL;
break;
}
for (unsigned i = 0; i < pwm->channel_count; i++) {
if (pwm->values[i] == 0) {
/* ignore 0 */
} else if (pwm->values[i] < PWM_LOWEST_MAX) {
_max_pwm[i] = PWM_LOWEST_MAX;
} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
_max_pwm[i] = PWM_HIGHEST_MAX;
} else {
_max_pwm[i] = pwm->values[i];
}
}
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values*)arg;
for (unsigned i = 0; i < _max_actuators; i++) {
pwm->values[i] = _max_pwm[i];
}
pwm->channel_count = _max_actuators;
arg = (unsigned long)&pwm;
break;
}
case PWM_SERVO_SET(5):
case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) {
@@ -711,7 +908,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0):
if (arg < 2100) {
if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
@@ -1093,6 +1290,20 @@ fmu_start(void)
return ret;
}
int
fmu_stop(void)
{
int ret = OK;
if (g_fmu != nullptr) {
delete g_fmu;
g_fmu = nullptr;
}
return ret;
}
void
test(void)
{
@@ -1136,7 +1347,7 @@ test(void)
}
} else {
// and use write interface for the other direction
int ret = write(fd, servos, sizeof(servos));
ret = write(fd, servos, sizeof(servos));
if (ret != (int)sizeof(servos))
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
}
@@ -1227,6 +1438,12 @@ fmu_main(int argc, char *argv[])
PortMode new_mode = PORT_MODE_UNSET;
const char *verb = argv[1];
if (!strcmp(verb, "stop")) {
fmu_stop();
errx(0, "FMU driver stopped");
}
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
src/drivers/px4fmu/module.mk
+2 -1
View File
@@ -3,4 +3,5 @@
#
MODULE_COMMAND = fmu
SRCS = fmu.cpp
SRCS = fmu.cpp \
../../modules/systemlib/pwm_limit/pwm_limit.c
src/drivers/px4io/px4io.cpp
+96 -214
View File
@@ -183,21 +183,6 @@ public:
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Set the minimum PWM signals when armed
*/
int set_min_values(const uint16_t *vals, unsigned len);
/**
* Set the maximum PWM signal when armed
*/
int set_max_values(const uint16_t *vals, unsigned len);
/**
* Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE
*/
int set_idle_values(const uint16_t *vals, unsigned len);
/**
* Disable RC input handling
*/
@@ -953,55 +938,6 @@ PX4IO::io_set_control_state()
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
}
int
PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len);
}
int
PX4IO::set_min_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len);
}
int
PX4IO::set_max_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len);
}
int
PX4IO::set_idle_values(const uint16_t *vals, unsigned len)
{
if (len > _max_actuators)
/* fail with error */
return E2BIG;
printf("Sending IDLE values\n");
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len);
}
int
PX4IO::io_set_arming_state()
@@ -1874,10 +1810,10 @@ PX4IO::print_status()
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
printf("\nidle values");
printf("\ndisarmed values");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i));
printf(" %u", io_reg_get(PX4IO_PAGE_DISARMED_PWM, i));
printf("\n");
}
@@ -1910,12 +1846,22 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
/* get the default update rate */
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE);
break;
case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE: {
case PWM_SERVO_GET_UPDATE_RATE:
/* get the alternative update rate */
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE);
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE: {
/* blindly clear the PWM update alarm - might be set for some other reason */
io_reg_set(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS, PX4IO_P_STATUS_ALARMS_PWM_ERROR);
@@ -1934,6 +1880,87 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(unsigned *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES);
break;
case PWM_SERVO_SET_FAILSAFE_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_FAILSAFE_PWM:
ret = io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_SET_DISARMED_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_DISARMED_PWM, 0, pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_DISARMED_PWM:
ret = io_reg_get(PX4IO_PAGE_DISARMED_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_SET_MIN_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_MIN_PWM:
ret = io_reg_get(PX4IO_PAGE_CONTROL_MIN_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_SET_MAX_PWM: {
struct pwm_output_values* pwm = (struct pwm_output_values*)arg;
if (pwm->channel_count > _max_actuators)
/* fail with error */
return E2BIG;
/* copy values to registers in IO */
ret = io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, pwm->values, pwm->channel_count);
break;
}
case PWM_SERVO_GET_MAX_PWM:
ret = io_reg_get(PX4IO_PAGE_CONTROL_MAX_PWM, 0, (uint16_t*)arg, _max_actuators);
if (ret != OK) {
ret = -EIO;
}
break;
case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = _max_actuators;
break;
@@ -1958,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
/* TODO: we could go lower for e.g. TurboPWM */
unsigned channel = cmd - PWM_SERVO_SET(0);
if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) {
ret = -EINVAL;
} else {
@@ -2591,152 +2618,7 @@ px4io_main(int argc, char *argv[])
exit(0);
}
if (!strcmp(argv[1], "failsafe")) {
if (argc < 3) {
errx(1, "failsafe command needs at least one channel value (ppm)");
}
/* set values for first 8 channels, fill unassigned channels with 1500. */
uint16_t failsafe[8];
for (unsigned i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) {
/* set channel to commandline argument or to 900 for non-provided channels */
if (argc > i + 2) {
failsafe[i] = atoi(argv[i + 2]);
if (failsafe[i] < 800 || failsafe[i] > 2200) {
errx(1, "value out of range of 800 < value < 2200. Aborting.");
}
} else {
/* a zero value will result in stopping to output any pulse */
failsafe[i] = 0;
}
}
int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
if (ret != OK)
errx(ret, "failed setting failsafe values");
exit(0);
}
if (!strcmp(argv[1], "min")) {
if (argc < 3) {
errx(1, "min command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 900. */
uint16_t min[8];
for (unsigned i = 0; i < sizeof(min) / sizeof(min[0]); i++) {
/* set channel to commanline argument or to 900 for non-provided channels */
if (argc > i + 2) {
min[i] = atoi(argv[i + 2]);
if (min[i] < 900 || min[i] > 1200) {
errx(1, "value out of range of 900 < value < 1200. Aborting.");
}
} else {
/* a zero value will the default */
min[i] = 0;
}
}
int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0]));
if (ret != OK)
errx(ret, "failed setting min values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "max")) {
if (argc < 3) {
errx(1, "max command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 2100. */
uint16_t max[8];
for (unsigned i = 0; i < sizeof(max) / sizeof(max[0]); i++) {
/* set channel to commanline argument or to 2100 for non-provided channels */
if (argc > i + 2) {
max[i] = atoi(argv[i + 2]);
if (max[i] < 1800 || max[i] > 2100) {
errx(1, "value out of range of 1800 < value < 2100. Aborting.");
}
} else {
/* a zero value will the default */
max[i] = 0;
}
}
int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0]));
if (ret != OK)
errx(ret, "failed setting max values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "idle")) {
if (argc < 3) {
errx(1, "max command needs at least one channel value (PWM)");
}
if (g_dev != nullptr) {
/* set values for first 8 channels, fill unassigned channels with 0. */
uint16_t idle[8];
for (unsigned i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) {
/* set channel to commanline argument or to 0 for non-provided channels */
if (argc > i + 2) {
idle[i] = atoi(argv[i + 2]);
if (idle[i] < 900 || idle[i] > 2100) {
errx(1, "value out of range of 900 < value < 2100. Aborting.");
}
} else {
/* a zero value will the default */
idle[i] = 0;
}
}
int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0]));
if (ret != OK)
errx(ret, "failed setting idle values");
} else {
errx(1, "not loaded");
}
exit(0);
}
if (!strcmp(argv[1], "recovery")) {
@@ -2808,5 +2690,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv);
out:
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind' or 'update'");
}
src/drivers/px4io/px4io_serial.cpp
+17 -11
View File
@@ -48,6 +48,7 @@
#include <time.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <arch/board/board.h>
@@ -262,7 +263,8 @@ PX4IO_serial::init()
up_enable_irq(PX4IO_SERIAL_VECTOR);
/* enable UART in DMA mode, enable error and line idle interrupts */
/* rCR3 = USART_CR3_EIE;*/
rCR3 = USART_CR3_EIE;
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
/* create semaphores */
@@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete()
DMA_SCR_PBURST_SINGLE |
DMA_SCR_MBURST_SINGLE);
stm32_dmastart(_tx_dma, nullptr, nullptr, false);
//rCR1 &= ~USART_CR1_TE;
//rCR1 |= USART_CR1_TE;
rCR3 |= USART_CR3_DMAT;
perf_end(_pc_dmasetup);
/* compute the deadline for a 5ms timeout */
/* compute the deadline for a 10ms timeout */
struct timespec abstime;
clock_gettime(CLOCK_REALTIME, &abstime);
#if 0
abstime.tv_sec++; /* long timeout for testing */
#else
abstime.tv_nsec += 10000000; /* 0ms timeout */
if (abstime.tv_nsec > 1000000000) {
abstime.tv_nsec += 10*1000*1000;
if (abstime.tv_nsec >= 1000*1000*1000) {
abstime.tv_sec++;
abstime.tv_nsec -= 1000000000;
abstime.tv_nsec -= 1000*1000*1000;
}
#endif
/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
int ret;
@@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt()
*/
if (_rx_dma_status == _dma_status_waiting) {
_abort_dma();
perf_count(_pc_uerrs);
perf_count(_pc_uerrs);
/* complete DMA as though in error */
_do_rx_dma_callback(DMA_STATUS_TEIF);
@@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt()
unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
perf_count(_pc_badidle);
/* stop the receive DMA */
stm32_dmastop(_rx_dma);
/* complete the short reception */
_do_rx_dma_callback(DMA_STATUS_TEIF);
return;
}
@@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma()
stm32_dmastop(_rx_dma);
}
#endif /* PX4IO_SERIAL_BASE */
#endif /* PX4IO_SERIAL_BASE */
src/examples/px4_simple_app/px4_simple_app.c
+1 -1
View File
@@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[])
int error_counter = 0;
while (true) {
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);
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+1 -1
View File
@@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float xtrackErr = vector_A_to_airplane % vector_AB;
float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
/* limit output to 45 degrees */
sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f);
sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
float eta1 = asinf(sine_eta1);
eta = eta1 + eta2;
/* bearing from current position to L1 point */
src/lib/external_lgpl/tecs/tecs.h
+5
View File
@@ -94,6 +94,11 @@ public:
// Rate of change of velocity along X body axis in m/s^2
float get_VXdot(void) { return _vel_dot; }
float get_speed_weight() {
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
src/modules/commander/commander.cpp
+41 -35
View File
@@ -199,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
*/
int commander_thread_main(int argc, char *argv[]);
void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
@@ -379,7 +379,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
// TODO remove debug code
//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
/* set arming state */
arming_res = TRANSITION_NOT_CHANGED;
@@ -496,11 +496,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
@@ -521,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
tune_negative();
if (result == VEHICLE_CMD_RESULT_DENIED) {
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
} else if (result == VEHICLE_CMD_RESULT_FAILED) {
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
}
}
@@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) {
orb_copy(ORB_ID(safety), safety_sub, &safety);
// XXX this would be the right approach to do it, but do we *WANT* this?
// /* disarm if safety is now on and still armed */
// if (safety.safety_switch_available && !safety.safety_off) {
// (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
// }
}
/* update global position estimate */
@@ -874,10 +880,10 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
if (status.condition_landed) {
mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
mavlink_log_critical(mavlink_fd, "#audio: LANDED");
} else {
mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
}
}
}
@@ -955,7 +961,7 @@ int commander_thread_main(int argc, char *argv[])
//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
status_changed = true;
battery_tune_played = false;
@@ -967,7 +973,7 @@ int commander_thread_main(int argc, char *argv[])
/* critical battery voltage, this is rather an emergency, change state machine */
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
battery_tune_played = false;
@@ -1069,12 +1075,12 @@ int commander_thread_main(int argc, char *argv[])
/* handle the case where RC signal was regained */
if (!status.rc_signal_found_once) {
status.rc_signal_found_once = true;
mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
status_changed = true;
} else {
if (status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
status_changed = true;
}
}
@@ -1143,7 +1149,7 @@ int commander_thread_main(int argc, char *argv[])
} else if (res == TRANSITION_DENIED) {
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
}
/* fill current_status according to mode switches */
@@ -1159,12 +1165,12 @@ int commander_thread_main(int argc, char *argv[])
} else if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
}
} else {
if (!status.rc_signal_lost) {
mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
status.rc_signal_lost = true;
status_changed = true;
}
@@ -1189,7 +1195,7 @@ int commander_thread_main(int argc, char *argv[])
if (res == TRANSITION_DENIED) {
/* DENIED here indicates bug in the commander */
warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
}
/* check which state machines for changes, clear "changed" flag */
@@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* play arming and battery warning tunes */
if (!arm_tune_played && armed.armed) {
if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
/* play tune when armed */
if (tune_arm() == OK)
arm_tune_played = true;
@@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[])
}
/* reset arm_tune_played when disarmed */
if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
arm_tune_played = false;
}
@@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
}
void
control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed)
{
/* driving rgbled */
if (changed) {
@@ -1356,11 +1362,11 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
if (armed->armed) {
if (actuator_armed->armed) {
/* armed, solid */
led_on(LED_BLUE);
} else if (armed->ready_to_arm) {
} else if (actuator_armed->ready_to_arm) {
/* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0)
led_toggle(LED_BLUE);
@@ -1506,7 +1512,7 @@ print_reject_mode(const char *msg)
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
char s[80];
sprintf(s, "[cmd] WARNING: reject %s", msg);
sprintf(s, "#audio: warning: reject %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
}
@@ -1520,7 +1526,7 @@ print_reject_arm(const char *msg)
if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
last_print_mode_reject_time = t;
char s[80];
sprintf(s, "[cmd] %s", msg);
sprintf(s, "#audio: %s", msg);
mavlink_log_critical(mavlink_fd, s);
tune_negative();
}
@@ -1617,14 +1623,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
if (res == TRANSITION_CHANGED) {
if (control_mode->flag_control_position_enabled) {
mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
} else {
if (status->condition_landed) {
mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
} else {
mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
}
}
}
@@ -1660,22 +1666,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
tune_negative();
break;
@@ -1814,14 +1820,14 @@ void *commander_low_prio_loop(void *arg)
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
ret = -ret;
if (ret < 1000)
mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
@@ -1834,14 +1840,14 @@ void *commander_low_prio_loop(void *arg)
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
/* convenience as many parts of NuttX use negative errno */
if (ret < 0)
ret = -ret;
if (ret < 1000)
mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+152 -17
View File
@@ -158,6 +158,12 @@ private:
float _launch_alt;
bool _launch_valid;
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn;
/* heading hold */
float target_bearing;
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
bool _airspeed_valid; ///< flag if a valid airspeed estimate exists
@@ -197,6 +203,8 @@ private:
float throttle_max;
float throttle_cruise;
float throttle_land_max;
float loiter_hold_radius;
} _parameters; /**< local copies of interesting parameters */
@@ -229,6 +237,8 @@ private:
param_t throttle_max;
param_t throttle_cruise;
param_t throttle_land_max;
param_t loiter_hold_radius;
} _parameter_handles; /**< handles for interesting parameters */
@@ -327,7 +337,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_airspeed_error(0.0f),
_airspeed_valid(false),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false)
_global_pos_valid(false),
land_noreturn(false)
{
_nav_capabilities.turn_distance = 0.0f;
@@ -345,6 +356,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@@ -408,6 +420,8 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
@@ -630,6 +644,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
/* no throttle limit as default */
float throttle_max = 1.0f;
/* AUTONOMOUS FLIGHT */
// XXX this should only execute if auto AND safety off (actuators active),
@@ -639,11 +656,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* execute navigation once we have a setpoint */
if (_setpoint_valid) {
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
/* current waypoint (the one currently heading for) */
math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
@@ -711,27 +729,87 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < 15.0f || land_noreturn) {
/* heading hold, along the line connecting this and the last waypoint */
// if (global_triplet.previous_valid) {
// target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
// } else {
if (!land_noreturn)
target_bearing = _att.yaw;
//}
warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
if (altitude_error > -5.0f)
land_noreturn = true;
} else {
/* normal navigation */
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
}
/* do not go down too early */
if (wp_distance > 50.0f) {
altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
}
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
if (altitude_error > -20.0f) {
float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1)
float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
float land_pitch_min = math::radians(5.0f);
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = _parameters.airspeed_min;
float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
if (altitude_error > -4.0f) {
/* land with minimal speed */
/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
_tecs.set_speed_weight(2.0f);
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, flare_angle_rad,
false, flare_angle_rad,
0.0f, _parameters.throttle_max, throttle_land,
math::radians(-10.0f), math::radians(15.0f));
/* kill the throttle if param requests it */
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
/* minimize speed to approach speed */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_angle_rad,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
} else {
/* normal cruise speed */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
@@ -790,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_loiter_hold = true;
}
float altitude_error = _loiter_hold_alt - _global_pos.alt;
altitude_error = _loiter_hold_alt - _global_pos.alt;
math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
@@ -801,7 +879,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_att_sp.yaw_body = _l1_control.nav_bearing();
/* climb with full throttle if the altitude error is bigger than 5 meters */
bool climb_out = (altitude_error > 5);
bool climb_out = (altitude_error > 3);
float min_pitch;
@@ -824,11 +902,53 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
}
/* reset land state */
if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
land_noreturn = false;
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
} else if (0/* easy mode enabled */) {
/** EASY FLIGHT **/
if (0/* switched from another mode to easy */) {
_seatbelt_hold_heading = _att.yaw;
}
if (0/* easy on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
/* if in seatbelt mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else if (0/* seatbelt mode enabled */) {
/** SEATBELT FLIGHT **/
@@ -848,13 +968,28 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
/* user switched off throttle */
if (_manual.throttle < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.roll_body = _manual.roll;
_att_sp.yaw_body = _manual.yaw;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
@@ -867,7 +1002,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.thrust = _tecs.get_throttle_demand();
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
return setpoint;
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+1
View File
@@ -75,6 +75,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
src/modules/mavlink/missionlib.c
+1 -1
View File
@@ -220,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z;
sp.altitude_is_relative = false;
sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F);
set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize setpoint publication if necessary */
src/modules/mavlink/waypoints.c
+25 -12
View File
@@ -398,7 +398,14 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
if (time_elapsed) {
/* safeguard against invalid missions with last wp autocontinue on */
if (wpm->current_active_wp_id == wpm->size - 1) {
/* stop handling missions here */
cur_wp->autocontinue = false;
}
if (cur_wp->autocontinue) {
cur_wp->current = 0;
float navigation_lat = -1.0f;
@@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
navigation_frame = MAV_FRAME_LOCAL_NED;
}
/* guard against missions without final land waypoint */
/* only accept supported navigation waypoints, skip unknown ones */
do {
/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
/* this is a navigation waypoint */
navigation_frame = cur_wp->frame;
@@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
navigation_alt = cur_wp->z;
}
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
* activated keep the system loitering there.
*/
cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
cur_wp->frame = navigation_frame;
cur_wp->x = navigation_lat;
cur_wp->y = navigation_lon;
cur_wp->z = navigation_alt;
cur_wp->autocontinue = false;
if (wpm->current_active_wp_id == wpm->size - 1) {
/* if we're not landing at the last nav waypoint, we're falling back to loiter */
if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
/* the last waypoint was reached, if auto continue is
* activated AND it is NOT a land waypoint, keep the system loitering there.
*/
cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
cur_wp->frame = navigation_frame;
cur_wp->x = navigation_lat;
cur_wp->y = navigation_lon;
cur_wp->z = navigation_alt;
}
/* we risk an endless loop for missions without navigation waypoints, abort. */
break;
src/modules/px4iofirmware/i2c.c
-6
View File
@@ -149,12 +149,6 @@ interface_init(void)
#endif
}
void
interface_tick()
{
}
/*
reset the I2C bus
used to recover from lockups
src/modules/px4iofirmware/mixer.cpp
+31 -114
View File
@@ -47,6 +47,7 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/mixer/mixer.h>
extern "C" {
@@ -59,12 +60,6 @@ extern "C" {
*/
#define FMU_INPUT_DROP_LIMIT_US 200000
/*
* Time that the ESCs need to initialize
*/
#define ESC_INIT_TIME_US 1000000
#define ESC_RAMP_TIME_US 2000000
/* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0
#define PITCH 1
@@ -76,15 +71,6 @@ extern "C" {
static bool mixer_servos_armed = false;
static bool should_arm = false;
static bool should_always_enable_pwm = false;
static uint64_t esc_init_time;
enum esc_state_e {
ESC_OFF,
ESC_INIT,
ESC_RAMP,
ESC_ON
};
static esc_state_e esc_state;
/* selected control values and count for mixing */
enum mixer_source {
@@ -165,102 +151,6 @@ mixer_tick(void)
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE);
}
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
uint16_t ramp_promille;
/* update esc init state, but only if we are truely armed and not just PWM enabled */
if (mixer_servos_armed && should_arm) {
switch (esc_state) {
/* after arming, some ESCs need an initalization period, count the time from here */
case ESC_OFF:
esc_init_time = hrt_absolute_time();
esc_state = ESC_INIT;
break;
/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
case ESC_INIT:
if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
esc_state = ESC_RAMP;
}
break;
/* then ramp until the min speed is reached */
case ESC_RAMP:
if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
esc_state = ESC_ON;
}
break;
case ESC_ON:
default:
break;
}
} else {
esc_state = ESC_OFF;
}
/* do the calculations during the ramp for all at once */
if (esc_state == ESC_RAMP) {
ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
}
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
switch (esc_state) {
case ESC_INIT:
r_page_servos[i] = (outputs[i] * 600 + 1500);
break;
case ESC_RAMP:
r_page_servos[i] = (outputs[i]
* (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+ (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
break;
case ESC_ON:
r_page_servos[i] = (outputs[i]
* (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+ (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
break;
case ESC_OFF:
default:
break;
}
}
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
/*
* Decide whether the servos should be armed right now.
*
@@ -285,6 +175,34 @@ mixer_tick(void)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
/*
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_servos[i] = r_page_servo_failsafe[i];
/* safe actuators for FMU feedback */
r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
}
} else if (source != MIX_NONE) {
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
@@ -298,7 +216,6 @@ mixer_tick(void)
mixer_servos_armed = false;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> PWM disabled");
}
if (mixer_servos_armed && should_arm) {
@@ -307,9 +224,9 @@ mixer_tick(void)
up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) {
/* set the idle servo outputs. */
/* set the disarmed servo outputs. */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servo_idle[i]);
up_pwm_servo_set(i, r_page_servo_disarmed[i]);
}
}
src/modules/px4iofirmware/module.mk
+1
View File
@@ -14,6 +14,7 @@ SRCS = adc.c \
../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \
../systemlib/pwm_limit/pwm_limit.c
ifeq ($(BOARD),px4io-v1)
SRCS += i2c.c
src/modules/px4iofirmware/protocol.h
+2 -2
View File
@@ -220,8 +220,8 @@ enum { /* DSM bind states */
/* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM idle values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
src/modules/px4iofirmware/px4io.c
+8 -8
View File
@@ -50,6 +50,7 @@
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <stm32_uart.h>
@@ -64,6 +65,8 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
pwm_limit_t pwm_limit;
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
@@ -159,9 +162,6 @@ user_start(int argc, char *argv[])
/* start the FMU interface */
interface_init();
/* add a performance counter for the interface */
perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -174,6 +174,9 @@ user_start(int argc, char *argv[])
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
@@ -203,11 +206,6 @@ user_start(int argc, char *argv[])
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the interface */
perf_begin(interface_perf);
interface_tick();
perf_end(interface_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
@@ -218,6 +216,7 @@ user_start(int argc, char *argv[])
controls_tick();
perf_end(controls_perf);
#if 0
/* check for debug activity */
show_debug_messages();
@@ -234,6 +233,7 @@ user_start(int argc, char *argv[])
(unsigned)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
#endif
}
}
src/modules/px4iofirmware/px4io.h
+8 -1
View File
@@ -46,6 +46,8 @@
#include "protocol.h"
#include <systemlib/pwm_limit/pwm_limit.h>
/*
* Constants and limits.
*/
@@ -80,7 +82,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */
extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
/*
* Register aliases.
@@ -122,6 +124,11 @@ struct sys_state_s {
extern struct sys_state_s system_state;
/*
* PWM limit structure
*/
extern pwm_limit_t pwm_limit;
/*
* GPIO handling.
*/
src/modules/px4iofirmware/registers.c
+62 -45
View File
@@ -199,7 +199,7 @@ uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRI
*
* Disable pulses as default.
*/
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
/**
* PAGE 106
@@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
* minimum PWM values when armed
*
*/
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN };
/**
* PAGE 107
@@ -215,15 +215,15 @@ uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 90
* maximum PWM values when armed
*
*/
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX };
/**
* PAGE 108
*
* idle PWM values for difficult ESCs
* disarmed PWM values for difficult ESCs
*
*/
uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
uint16_t r_page_servo_disarmed[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
@@ -276,8 +276,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servo_failsafe[offset] = *values;
if (*values == 0) {
/* ignore 0 */
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
} else {
r_page_servo_failsafe[offset] = *values;
}
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
@@ -293,16 +300,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_control_min[offset] = 900;
else if (*values > 1200)
r_page_servo_control_min[offset] = 1200;
else if (*values < 900)
r_page_servo_control_min[offset] = 900;
else
if (*values == 0) {
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MIN) {
r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
} else {
r_page_servo_control_min[offset] = *values;
}
offset++;
num_values--;
@@ -315,16 +321,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_control_max[offset] = 2100;
else if (*values > 2100)
r_page_servo_control_max[offset] = 2100;
else if (*values < 1800)
r_page_servo_control_max[offset] = 1800;
else
if (*values == 0) {
/* ignore 0 */
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
} else if (*values < PWM_LOWEST_MAX) {
r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
} else {
r_page_servo_control_max[offset] = *values;
}
offset++;
num_values--;
@@ -332,28 +337,40 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
}
break;
case PX4IO_PAGE_IDLE_PWM:
case PX4IO_PAGE_DISARMED_PWM:
{
/* flag for all outputs */
bool all_disarmed_off = true;
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0)
/* set to default */
r_page_servo_idle[offset] = 0;
if (*values == 0) {
/* 0 means disabling always PWM */
r_page_servo_disarmed[offset] = 0;
} else if (*values < PWM_LOWEST_MIN) {
r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
all_disarmed_off = false;
} else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
all_disarmed_off = false;
} else {
r_page_servo_disarmed[offset] = *values;
all_disarmed_off = false;
}
else if (*values < 900)
r_page_servo_idle[offset] = 900;
else if (*values > 2100)
r_page_servo_idle[offset] = 2100;
else
r_page_servo_idle[offset] = *values;
offset++;
num_values--;
values++;
}
/* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
offset++;
num_values--;
values++;
if (all_disarmed_off) {
/* disable PWM output if disarmed */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
} else {
/* enable PWM output always */
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
}
}
break;
@@ -767,8 +784,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max);
break;
case PX4IO_PAGE_IDLE_PWM:
SELECT_PAGE(r_page_servo_idle);
case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_disarmed);
break;
default:
src/modules/px4iofirmware/serial.c
+3 -17
View File
@@ -74,9 +74,6 @@ static DMA_HANDLE rx_dma;
static int serial_interrupt(int irq, void *context);
static void dma_reset(void);
/* if we spend this many ticks idle, reset the DMA */
static unsigned idle_ticks;
static struct IOPacket dma_packet;
/* serial register accessors */
@@ -150,16 +147,6 @@ interface_init(void)
debug("serial init");
}
void
interface_tick()
{
/* XXX look for stuck/damaged DMA and reset? */
if (idle_ticks++ > 100) {
dma_reset();
idle_ticks = 0;
}
}
static void
rx_handle_packet(void)
{
@@ -230,9 +217,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
/* disable UART DMA */
rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
/* reset the idle counter */
idle_ticks = 0;
/* handle the received packet */
rx_handle_packet();
@@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context)
/* it was too short - possibly truncated */
perf_count(pc_badidle);
dma_reset();
return 0;
}
@@ -343,7 +328,8 @@ dma_reset(void)
sizeof(dma_packet),
DMA_CCR_MINC |
DMA_CCR_PSIZE_8BITS |
DMA_CCR_MSIZE_8BITS);
DMA_CCR_MSIZE_8BITS |
DMA_CCR_PRIVERYHI);
/* start receive DMA ready for the next packet */
stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
src/modules/systemlib/mixer/mixer.h
+1
View File
@@ -449,6 +449,7 @@ public:
HEX_PLUS, /**< hex in + configuration */
OCTA_X,
OCTA_PLUS,
OCTA_COX,
MAX_GEOMETRY
};
src/modules/systemlib/mixer/mixer_multirotor.cpp
+15
View File
@@ -130,6 +130,16 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
{ 1.000000, 0.000000, -1.00 },
{ -1.000000, 0.000000, -1.00 },
};
const MultirotorMixer::Rotor _config_octa_cox[] = {
{ -0.707107, 0.707107, 1.00 },
{ 0.707107, 0.707107, -1.00 },
{ 0.707107, -0.707107, 1.00 },
{ -0.707107, -0.707107, -1.00 },
{ 0.707107, 0.707107, 1.00 },
{ -0.707107, 0.707107, -1.00 },
{ -0.707107, -0.707107, 1.00 },
{ 0.707107, -0.707107, -1.00 },
};
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_quad_x[0],
&_config_quad_plus[0],
@@ -139,6 +149,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_hex_plus[0],
&_config_octa_x[0],
&_config_octa_plus[0],
&_config_octa_cox[0],
};
const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
4, /* quad_x */
@@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
6, /* hex_plus */
8, /* octa_x */
8, /* octa_plus */
8, /* octa_cox */
};
}
@@ -240,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X;
} else if (!strcmp(geomname, "8c")) {
geometry = MultirotorMixer::OCTA_COX;
} else {
debug("unrecognised geometry '%s'", geomname);
src/modules/systemlib/mixer/multi_tables
+12 -1
View File
@@ -74,7 +74,18 @@ set octa_plus {
90 CW
}
set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus}
set octa_cox {
45 CCW
-45 CW
-135 CCW
135 CW
-45 CCW
45 CW
135 CCW
-135 CW
}
set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox}
proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
src/modules/systemlib/pwm_limit/pwm_limit.c
+133
View File
@@ -0,0 +1,133 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
*
* 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 pwm_limit.c
*
* Lib to limit PWM output
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#include "pwm_limit.h"
#include <math.h>
#include <stdbool.h>
#include <drivers/drv_hrt.h>
void pwm_limit_init(pwm_limit_t *limit)
{
limit->state = LIMIT_STATE_OFF;
limit->time_armed = 0;
return;
}
void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit)
{
/* first evaluate state changes */
switch (limit->state) {
case LIMIT_STATE_OFF:
if (armed)
limit->state = LIMIT_STATE_RAMP;
limit->time_armed = hrt_absolute_time();
break;
case LIMIT_STATE_INIT:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US)
limit->state = LIMIT_STATE_RAMP;
break;
case LIMIT_STATE_RAMP:
if (!armed)
limit->state = LIMIT_STATE_OFF;
else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US)
limit->state = LIMIT_STATE_ON;
break;
case LIMIT_STATE_ON:
if (!armed)
limit->state = LIMIT_STATE_OFF;
break;
default:
break;
}
unsigned progress;
uint16_t temp_pwm;
/* then set effective_pwm based on state */
switch (limit->state) {
case LIMIT_STATE_OFF:
case LIMIT_STATE_INIT:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = disarmed_pwm[i];
output[i] = 0.0f;
}
break;
case LIMIT_STATE_RAMP:
progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US;
for (unsigned i=0; i<num_channels; i++) {
uint16_t ramp_min_pwm;
/* if a disarmed pwm value was set, blend between disarmed and min */
if (disarmed_pwm[i] > 0) {
/* safeguard against overflows */
uint16_t disarmed = disarmed_pwm[i];
if (disarmed > min_pwm[i])
disarmed = min_pwm[i];
uint16_t disarmed_min_diff = min_pwm[i] - disarmed;
ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000;
} else {
/* no disarmed pwm value set, choose min pwm */
ramp_min_pwm = min_pwm[i];
}
effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
output[i] = (float)progress/10000.0f * output[i];
}
break;
case LIMIT_STATE_ON:
for (unsigned i=0; i<num_channels; i++) {
effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
/* effective_output stays the same */
}
break;
default:
break;
}
return;
}
src/modules/systemlib/pwm_limit/pwm_limit.h
+77
View File
@@ -0,0 +1,77 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
*
* 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 pwm_limit.h
*
* Lib to limit PWM output
*
* @author Julian Oes <joes@student.ethz.ch>
*/
#ifndef PWM_LIMIT_H_
#define PWM_LIMIT_H_
#include <stdint.h>
#include <stdbool.h>
/*
* time for the ESCs to initialize
* (this is not actually needed if PWM is sent right after boot)
*/
#define INIT_TIME_US 500000
/*
* time to slowly ramp up the ESCs
*/
#define RAMP_TIME_US 2500000
typedef struct {
enum {
LIMIT_STATE_OFF = 0,
LIMIT_STATE_INIT,
LIMIT_STATE_RAMP,
LIMIT_STATE_ON
} state;
uint64_t time_armed;
} pwm_limit_t;
__BEGIN_DECLS
__EXPORT void pwm_limit_init(pwm_limit_t *limit);
__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit);
__END_DECLS
#endif /* PWM_LIMIT_H_ */
src/modules/uORB/topics/actuator_outputs.h
+1 -1
View File
@@ -60,7 +60,7 @@
struct actuator_outputs_s {
uint64_t timestamp; /**< output timestamp in us since system boot */
float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */
int noutputs; /**< valid outputs */
unsigned noutputs; /**< valid outputs */
};
/**
src/systemcmds/config/config.c
+1 -1
View File
@@ -272,7 +272,7 @@ do_accel(int argc, char *argv[])
}
} else {
errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t");
}
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
src/systemcmds/esc_calib/esc_calib.c
+155 -61
View File
@@ -62,23 +62,27 @@
#include "systemlib/err.h"
#include "drivers/drv_pwm_output.h"
#include <uORB/topics/actuator_controls.h>
static void usage(const char *reason);
__EXPORT int esc_calib_main(int argc, char *argv[]);
#define MAX_CHANNELS 14
static void
usage(const char *reason)
{
if (reason != NULL)
warnx("%s", reason);
errx(1,
"usage:\n"
"esc_calib [-d <device>] <channels>\n"
" <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
" <channels> Provide channels (e.g.: 1 2 3 4)\n"
);
errx(1,
"usage:\n"
"esc_calib\n"
" [-d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
" [-l <pwm> Low PWM value in us (default: %dus)\n"
" [-h <pwm> High PWM value in us (default: %dus)\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Use all outputs\n"
, PWM_DEFAULT_MIN, PWM_DEFAULT_MAX);
}
int
@@ -86,157 +90,247 @@ esc_calib_main(int argc, char *argv[])
{
char *dev = PWM_OUTPUT_DEVICE_PATH;
char *ep;
bool channels_selected[MAX_CHANNELS] = {false};
int ch;
int ret;
char c;
unsigned max_channels = 0;
uint32_t set_mask = 0;
unsigned long channels;
unsigned single_ch = 0;
uint16_t pwm_high = PWM_DEFAULT_MAX;
uint16_t pwm_low = PWM_DEFAULT_MIN;
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
if (argc < 2)
usage(NULL);
if (argc < 2) {
usage("no channels provided");
}
while ((ch = getopt(argc-1, argv, "d:")) != EOF) {
int arg_consumed = 0;
while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) {
switch (ch) {
case 'd':
dev = optarg;
argc-=2;
arg_consumed += 2;
break;
case 'c':
/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
channels = strtoul(optarg, &ep, 0);
while ((single_ch = channels % 10)) {
set_mask |= 1<<(single_ch-1);
channels /= 10;
}
break;
case 'm':
/* Read in mask directly */
set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
break;
case 'a':
/* Choose all channels */
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
set_mask |= 1<<i;
}
break;
case 'l':
/* Read in custom low value */
if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN)
usage("low PWM invalid");
break;
case 'h':
/* Read in custom high value */
pwm_high = strtoul(optarg, &ep, 0);
if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX)
usage("high PWM invalid");
break;
default:
usage(NULL);
}
}
if(argc < 2) {
usage("no channels provided");
}
if (set_mask == 0)
usage("no channels chosen");
while (--argc) {
const char *arg = argv[argc];
unsigned channel_number = strtol(arg, &ep, 0);
/* make sure no other source is publishing control values now */
struct actuator_controls_s actuators;
int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
if (*ep == '\0') {
if (channel_number > MAX_CHANNELS || channel_number <= 0) {
err(1, "invalid channel number: %d", channel_number);
} else {
channels_selected[channel_number-1] = true;
/* clear changed flag */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators);
}
}
/* wait 50 ms */
usleep(50000);
/* now expect nothing changed on that topic */
bool orb_updated;
orb_check(act_sub, &orb_updated);
if (orb_updated) {
errx(1, "ABORTING! Attitude control still active. Please ensure to shut down all controllers:\n"
"\tmultirotor_att_control stop\n"
"\tfw_att_control stop\n");
}
printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
"\n"
"Make sure\n"
"\t - that the ESCs are not powered\n"
"\t - that safety is off (two short blinks)\n"
"\t - that the controllers are stopped\n"
"\n"
"Do you want to start calibration now: y or n?\n");
"\n"
"Make sure\n"
"\t - that the ESCs are not powered\n"
"\t - that safety is off (two short blinks)\n"
"\t - that the controllers are stopped\n"
"\n"
"Do you want to start calibration now: y or n?\n");
/* wait for user input */
while (1) {
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 'y' || c == 'Y') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
printf("ESC calibration exited\n");
exit(0);
} else if (c == 'n' || c == 'N') {
printf("ESC calibration aborted\n");
exit(0);
} else {
printf("Unknown input, ESC calibration aborted\n");
exit(0);
}
}
}
/* rate limit to ~ 20 Hz */
usleep(50000);
}
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* Wait for user confirmation */
printf("\nHigh PWM set\n"
"\n"
"Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
"\n");
/* get number of channels available on the device */
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
/* tell IO/FMU that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
warnx("Outputs armed");
/* wait for user confirmation */
printf("\nHigh PWM set: %d\n"
"\n"
"Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
"\n", pwm_high);
fflush(stdout);
while (1) {
/* set max PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high);
/* First set high PWM */
for (unsigned i = 0; i<MAX_CHANNELS; i++) {
if(channels_selected[i]) {
ret = ioctl(fd, PWM_SERVO_SET(i), 2100);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_high);
}
}
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 13) {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("ESC calibration exited");
exit(0);
}
}
/* rate limit to ~ 20 Hz */
usleep(50000);
}
/* we don't need any more user input */
printf("Low PWM set, hit ENTER when finished\n"
"\n");
printf("Low PWM set: %d\n"
"\n"
"Hit ENTER when finished\n"
"\n", pwm_low);
while (1) {
/* Then set low PWM */
for (unsigned i = 0; i<MAX_CHANNELS; i++) {
if(channels_selected[i]) {
ret = ioctl(fd, PWM_SERVO_SET(i), 900);
/* set disarmed PWM */
for (unsigned i = 0; i < max_channels; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_low);
}
}
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 13) {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
printf("ESC calibration exited\n");
exit(0);
}
}
/* rate limit to ~ 20 Hz */
usleep(50000);
}
/* disarm */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
warnx("Outputs disarmed");
printf("ESC calibration finished\n");
exit(0);
src/systemcmds/pwm/pwm.c
+362 -134
View File
@@ -45,6 +45,7 @@
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
@@ -71,16 +72,36 @@ usage(const char *reason)
warnx("%s", reason);
errx(1,
"usage:\n"
"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n"
" -v Print information about the PWM device\n"
" <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
" <alt_rate> PWM update rate for channels in <alt_channel_mask>\n"
" <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n"
" arm | disarm Arm or disarm the ouptut\n"
" <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
" <chanmask> Directly supply alt rate channel mask (debug use only)\n"
"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\n"
"\n"
"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
" arm Arm output\n"
" disarm Disarm output\n"
"\n"
" rate ... Configure PWM rates\n"
" [-g <channel group>] Channel group that should update at the alternate rate\n"
" [-m <chanmask> ] Directly supply channel mask\n"
" [-a] Configure all outputs\n"
" -r <alt_rate> PWM rate (50 to 400 Hz)\n"
"\n"
" failsafe ... Configure failsafe PWM values\n"
" disarmed ... Configure disarmed PWM values\n"
" min ... Configure minimum PWM values\n"
" max ... Configure maximum PWM values\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n"
"\n"
" test ... Directly set PWM values\n"
" [-c <channels>] Supply channels (e.g. 1234)\n"
" [-m <chanmask> ] Directly supply channel mask (e.g. 0xF)\n"
" [-a] Configure all outputs\n"
" -p <pwm value> PWM value\n"
"\n"
" info Print information about the PWM device\n"
"\n"
" -v Print verbose information\n"
" -d <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
);
}
@@ -92,152 +113,389 @@ pwm_main(int argc, char *argv[])
unsigned alt_rate = 0;
uint32_t alt_channel_groups = 0;
bool alt_channels_set = false;
bool print_info = false;
bool print_verbose = false;
int ch;
int ret;
char *ep;
uint32_t set_mask = 0;
unsigned group;
int32_t set_mask = -1;
unsigned long channels;
unsigned single_ch = 0;
unsigned pwm_value = 0;
if (argc < 2)
if (argc < 1)
usage(NULL);
while ((ch = getopt(argc, argv, "c:d:u:vm:")) != EOF) {
while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
switch (ch) {
case 'd':
if (NULL == strstr(optarg, "/dev/")) {
warnx("device %s not valid", optarg);
usage(NULL);
}
dev = optarg;
break;
case 'v':
print_verbose = true;
break;
case 'c':
/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
channels = strtoul(optarg, &ep, 0);
while ((single_ch = channels % 10)) {
set_mask |= 1<<(single_ch-1);
channels /= 10;
}
break;
case 'g':
group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32))
usage("bad channel_group value");
alt_channel_groups |= (1 << group);
alt_channels_set = true;
break;
case 'd':
dev = optarg;
break;
case 'u':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
warnx("alt channels set, group: %d", group);
break;
case 'm':
set_mask = strtol(optarg, &ep, 0);
/* Read in mask directly */
set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad set_mask value");
break;
case 'v':
print_info = true;
case 'a':
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
set_mask |= 1<<i;
}
break;
case 'p':
pwm_value = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad PWM value provided");
break;
case 'r':
alt_rate = strtoul(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alternative rate provided");
break;
default:
usage(NULL);
break;
}
}
argc -= optind;
argv += optind;
if (print_verbose && set_mask > 0) {
warnx("Chose channels: ");
printf(" ");
for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
if (set_mask & 1<<i)
printf("%d ", i+1);
}
printf("\n");
}
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
/* get the number of servo channels */
unsigned servo_count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
if (!strcmp(argv[1], "arm")) {
/* tell IO that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* directly supplied channel mask */
if (set_mask != -1) {
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
err(1, "PWM_SERVO_ARM");
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
if (print_verbose)
warnx("Outputs armed");
exit(0);
for (unsigned group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
} else if (!strcmp(argv[1], "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
if (print_verbose)
warnx("Outputs disarmed");
exit(0);
mask |= group_mask;
} else if (!strcmp(argv[1], "rate")) {
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* directly supplied channel mask */
if (set_mask > 0) {
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, set_mask);
if (ret != OK)
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SET_SELECT_UPDATE_RATE");
}
exit(0);
} else if (!strcmp(argv[1], "min")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: min PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting min values");
}
exit(0);
} else if (!strcmp(argv[1], "max")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: max PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting max values");
}
exit(0);
} else if (!strcmp(argv[1], "disarmed")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
warnx("reading disarmed value of zero, disabling disarmed PWM");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("channel %d: disarmed PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_DISARMED_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting disarmed values");
}
exit(0);
} else if (!strcmp(argv[1], "failsafe")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
pwm_values.values[i] = pwm_value;
if (print_verbose)
warnx("Channel %d: failsafe PWM: %d", i+1, pwm_value);
}
pwm_values.channel_count++;
}
if (pwm_values.channel_count == 0) {
usage("no PWM values added");
} else {
ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
if (ret != OK)
errx(ret, "failed setting failsafe values");
}
exit(0);
} else if (!strcmp(argv[1], "test")) {
if (set_mask == 0) {
usage("no channels set");
}
if (pwm_value == 0)
usage("no PWM value provided");
/* perform PWM output */
/* Open console directly to grab CTRL-C signal */
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
while (1) {
for (unsigned i = 0; i < servo_count; i++) {
if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
}
/* abort on user request */
char c;
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
exit(0);
}
}
}
exit(0);
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
/* iterate remaining arguments */
unsigned nchannels = 0;
unsigned channel[8] = {0};
while (argc--) {
const char *arg = argv[0];
argv++;
if (!strcmp(arg, "arm")) {
/* tell IO that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
/* tell IO that the system is armed (it will output values if safety is off) */
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
continue;
} else if (!strcmp(argv[1], "info")) {
printf("device: %s\n", dev);
uint32_t info_default_rate;
uint32_t info_alt_rate;
uint32_t info_alt_rate_mask;
ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
if (ret != OK) {
err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
}
if (!strcmp(arg, "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
continue;
ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
if (ret != OK) {
err(1, "PWM_SERVO_GET_UPDATE_RATE");
}
unsigned pwm_value = strtol(arg, &ep, 0);
if (*ep == '\0') {
if (nchannels > sizeof(channel) / sizeof(channel[0]))
err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0]));
channel[nchannels] = pwm_value;
nchannels++;
continue;
ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
if (ret != OK) {
err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
}
usage("unrecognized option");
}
/* print verbose info */
if (print_info) {
/* get the number of servo channels */
unsigned count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
struct pwm_output_values failsafe_pwm;
struct pwm_output_values disarmed_pwm;
struct pwm_output_values min_pwm;
struct pwm_output_values max_pwm;
ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_FAILSAFE_PWM");
}
ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, (unsigned long)&disarmed_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_DISARMED_PWM");
}
ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, (unsigned long)&min_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_MIN_PWM");
}
ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, (unsigned long)&max_pwm);
if (ret != OK) {
err(1, "PWM_SERVO_GET_MAX_PWM");
}
/* print current servo values */
for (unsigned i = 0; i < count; i++) {
for (unsigned i = 0; i < servo_count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("channel %u: %uus\n", i, spos);
printf("channel %u: %u us", i+1, spos);
if (info_alt_rate_mask & (1<<i))
printf(" (alternative rate: %d Hz", info_alt_rate);
else
printf(" (default rate: %d Hz", info_default_rate);
printf(" failsafe: %d, disarmed: %d us, min: %d us, max: %d us)",
failsafe_pwm.values[i], disarmed_pwm.values[i], min_pwm.values[i], max_pwm.values[i]);
printf("\n");
} else {
printf("%u: ERROR\n", i);
}
}
/* print rate groups */
for (unsigned i = 0; i < count; i++) {
for (unsigned i = 0; i < servo_count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
@@ -247,44 +505,14 @@ pwm_main(int argc, char *argv[])
printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j))
printf(" %u", j);
printf(" %u", j+1);
printf("\n");
}
}
fflush(stdout);
exit(0);
}
usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
return 0;
}
/* perform PWM output */
if (nchannels) {
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
warnx("Press CTRL-C or 'c' to abort.");
while (1) {
for (int i = 0; i < nchannels; i++) {
ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", i);
}
/* abort on user request */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
close(console);
exit(0);
}
}
/* rate limit to ~ 20 Hz */
usleep(50000);
}
}
exit(0);
}