dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' of github.com:PX4/Firmware

This commit is contained in:
Lorenz Meier 2013-11-01 11:09:07 +01:00
parent a06b3e50ab 5781b58640
commit 0c762b6d1b
33 changed files with 1417 additions and 717 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ROMFS/px4fmu_common/init.d/10_dji_f330
View File

@ -59,11 +59,10 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io sh /etc/init.d/rc.io
# Set PWM values for DJI ESCs # 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 else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/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 # 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 # Start common for all multirotors apps

15
ROMFS/px4fmu_common/init.d/11_dji_f450
View File

@ -43,11 +43,9 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io 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 else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/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 # 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 # Start common for all multirotors apps

15
ROMFS/px4fmu_common/init.d/15_tbs_discovery
View File

@ -43,11 +43,9 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io 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 else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/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 # 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 # Start common for all multirotors apps

15
ROMFS/px4fmu_common/init.d/16_3dr_iris
View File

@ -43,11 +43,9 @@ then
mavlink start mavlink start
usleep 5000 usleep 5000
commander start
sh /etc/init.d/rc.io 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 else
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/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 # 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 # Start common for all multirotors apps

2
ROMFS/px4fmu_common/init.d/666_fmu_q_x550
View File

@ -53,7 +53,7 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
# #
# Set PWM output frequency # Set PWM output frequency
# #
pwm -u 400 -m 0xff pwm rate -c 1234 -r 400
# #
# Start common for all multirotors apps # Start common for all multirotors apps

10
ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl
View File

@ -83,10 +83,6 @@ then
usleep 5000 usleep 5000
sh /etc/init.d/rc.io 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 else
fmu mode_pwm fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
@ -107,9 +103,11 @@ else
fi 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 # Start common for all multirotors apps

30
ROMFS/px4fmu_common/init.d/rc.custom_io_esc
View File

@ -61,20 +61,9 @@ then
mavlink start -d /dev/ttyS1 -b 57600 mavlink start -d /dev/ttyS1 -b 57600
usleep 5000 usleep 5000
sh /etc/init.d/rc.io commander start
if [ $ESC_MAKER = afro ] sh /etc/init.d/rc.io
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
else else
fmu mode_pwm fmu mode_pwm
# Start MAVLink (on UART1 / ttyS0) # Start MAVLink (on UART1 / ttyS0)
@ -84,6 +73,19 @@ else
set EXIT_ON_END yes set EXIT_ON_END yes
fi 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 # Load mixer
# #
@ -105,7 +107,7 @@ fi
# #
# Set PWM output frequency # Set PWM output frequency
# #
pwm -u 400 -m 0xff pwm rate -r 400 -c 1234
# #
# Start common for all multirotors apps # Start common for all multirotors apps

5
ROMFS/px4fmu_common/init.d/rc.multirotor
View File

@ -13,11 +13,6 @@ sh /etc/init.d/rc.sensors
# #
sh /etc/init.d/rc.logging sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
# #
# Start GPS interface (depends on orb) # Start GPS interface (depends on orb)
# #

14
ROMFS/px4fmu_common/init.d/rcS
View File

@ -265,13 +265,13 @@ then
set MODE custom set MODE custom
fi fi
# PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame # PX4FMU v1 with IO + PPM-based ESCs on Quad X-shape frame
if param compare SYS_AUTOSTART 22 if param compare SYS_AUTOSTART 22
then then
set FRAME_GEOMETRY w set FRAME_GEOMETRY w
sh /etc/init.d/rc.custom_io_esc sh /etc/init.d/rc.custom_io_esc
set MODE custom set MODE custom
fi fi
if param compare SYS_AUTOSTART 30 if param compare SYS_AUTOSTART 30
then then

7
ROMFS/px4fmu_common/mixers/FMU_octo_cox.mix Normal file
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

80
src/drivers/drv_pwm_output.h
View File

@ -64,6 +64,36 @@ __BEGIN_DECLS
*/ */
#define PWM_OUTPUT_MAX_CHANNELS 16 #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 * Servo output signal type, value is actual servo output pulse
* width in microseconds. * width in microseconds.
@ -79,6 +109,7 @@ typedef uint16_t servo_position_t;
struct pwm_output_values { struct pwm_output_values {
/** desired pulse widths for each of the supported channels */ /** desired pulse widths for each of the supported channels */
servo_position_t values[PWM_OUTPUT_MAX_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) */ /** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) #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 */ /** 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 */ /** 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 */ /** 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 */ /** 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 */ /** 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 */ /** 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 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 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 */ #define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
/** Power up DSM receiver */ /** power up DSM receiver */
#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) #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 */ /** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)

4
src/drivers/hil/hil.cpp
View File

@ -404,7 +404,7 @@ HIL::task_main()
for (unsigned i = 0; i < num_outputs; i++) { for (unsigned i = 0; i < num_outputs; i++) {
/* last resort: catch NaN, INF and out-of-band errors */ /* last resort: catch NaN, INF and out-of-band errors */
if (i < (unsigned)outputs.noutputs && if (i < outputs.noutputs &&
isfinite(outputs.output[i]) && isfinite(outputs.output[i]) &&
outputs.output[i] >= -1.0f && outputs.output[i] >= -1.0f &&
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); g_hil->set_pwm_rate(arg);
break; break;
case PWM_SERVO_SELECT_UPDATE_RATE: case PWM_SERVO_SET_SELECT_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate // HIL always outputs at the alternate (usually faster) rate
break; break;

2
src/drivers/mkblctrl/mkblctrl.cpp
View File

@ -1071,7 +1071,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = OK; ret = OK;
break; break;
case PWM_SERVO_SELECT_UPDATE_RATE: case PWM_SERVO_SET_SELECT_UPDATE_RATE:
ret = OK; ret = OK;
break; break;

361
src/drivers/px4fmu/fmu.cpp
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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -59,11 +59,12 @@
#include <drivers/drv_gpio.h> #include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
# include <board_config.h> #include <board_config.h>
#include <systemlib/systemlib.h> #include <systemlib/systemlib.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <systemlib/mixer/mixer.h> #include <systemlib/mixer/mixer.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <drivers/drv_mixer.h> #include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h> #include <drivers/drv_rc_input.h>
@ -72,11 +73,17 @@
#include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <systemlib/err.h>
#ifdef HRT_PPM_CHANNEL #ifdef HRT_PPM_CHANNEL
# include <systemlib/ppm_decode.h> # include <systemlib/ppm_decode.h>
#endif #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 class PX4FMU : public device::CDev
{ {
public: public:
@ -100,7 +107,12 @@ public:
int set_pwm_alt_channels(uint32_t channels); int set_pwm_alt_channels(uint32_t channels);
private: private:
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
static const unsigned _max_actuators = 4; static const unsigned _max_actuators = 4;
#endif
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
static const unsigned _max_actuators = 6;
#endif
Mode _mode; Mode _mode;
unsigned _pwm_default_rate; unsigned _pwm_default_rate;
@ -117,11 +129,20 @@ private:
volatile bool _task_should_exit; volatile bool _task_should_exit;
bool _armed; bool _armed;
bool _pwm_on;
MixerGroup *_mixers; MixerGroup *_mixers;
actuator_controls_s _controls; 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[]); static void task_main_trampoline(int argc, char *argv[]);
void task_main() __attribute__((noreturn)); void task_main() __attribute__((noreturn));
@ -203,8 +224,18 @@ PX4FMU::PX4FMU() :
_primary_pwm_device(false), _primary_pwm_device(false),
_task_should_exit(false), _task_should_exit(false),
_armed(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; _debug_enabled = true;
} }
@ -457,6 +488,9 @@ PX4FMU::task_main()
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
#endif #endif
/* initialize PWM limit lib */
pwm_limit_init(&_pwm_limit);
log("starting"); log("starting");
/* loop until killed */ /* loop until killed */
@ -491,90 +525,103 @@ PX4FMU::task_main()
/* sleep waiting for data, stopping to check for PPM /* sleep waiting for data, stopping to check for PPM
* input at 100Hz */ * 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... */ /* this would be bad... */
if (ret < 0) { if (ret < 0) {
log("poll error %d", errno); log("poll error %d", errno);
usleep(1000000); usleep(1000000);
continue; continue;
} } else if (ret == 0) {
/* timeout: no control data, switch to failsafe values */
// warnx("no PWM: failsafe");
/* do we have a control update? */ } else {
if (fds[0].revents & POLLIN) {
/* get controls - must always do this to avoid spinning */ /* do we have a control update? */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); if (fds[0].revents & POLLIN) {
/* can we mix? */ /* get controls - must always do this to avoid spinning */
if (_mixers != nullptr) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
unsigned num_outputs; /* can we mix? */
if (_mixers != nullptr) {
switch (_mode) { unsigned num_outputs;
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;
}
/* do mixing */ switch (_mode) {
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); case MODE_2PWM:
outputs.timestamp = hrt_absolute_time(); num_outputs = 2;
break;
// XXX output actual limited values case MODE_4PWM:
memcpy(&controls_effective, &_controls, sizeof(controls_effective)); num_outputs = 4;
break;
orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective); case MODE_6PWM:
num_outputs = 6;
/* iterate actuators */ break;
for (unsigned i = 0; i < num_outputs; i++) { default:
num_outputs = 0;
/* last resort: catch NaN, INF and out-of-band errors */ break;
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;
} }
/* output to the servo */ /* do mixing */
up_pwm_servo_set(i, outputs.output[i]); 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? */ /* how about an arming update? */
if (fds[1].revents & POLLIN) { if (fds[1].revents & POLLIN) {
actuator_armed_s aa; actuator_armed_s aa;
/* get new value */ /* get new value */
orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
/* update PWM servo armed status if armed and not locked down */ /* update the armed status and check that we're not locked down */
bool set_armed = aa.armed && !aa.lockdown; bool set_armed = aa.armed && !aa.lockdown;
if (set_armed != _armed) { if (_armed != set_armed)
_armed = set_armed; _armed = set_armed;
up_pwm_servo_arm(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); up_pwm_servo_arm(false);
break; break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = _pwm_default_rate;
break;
case PWM_SERVO_SET_UPDATE_RATE: case PWM_SERVO_SET_UPDATE_RATE:
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg); ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break; 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); ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break; 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(5):
case PWM_SERVO_SET(4): case PWM_SERVO_SET(4):
if (_mode < MODE_6PWM) { if (_mode < MODE_6PWM) {
@ -711,7 +908,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */ /* FALLTHROUGH */
case PWM_SERVO_SET(1): case PWM_SERVO_SET(1):
case PWM_SERVO_SET(0): case PWM_SERVO_SET(0):
if (arg < 2100) { if (arg <= 2100) {
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg); up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else { } else {
ret = -EINVAL; ret = -EINVAL;
@ -1093,6 +1290,20 @@ fmu_start(void)
return ret; return ret;
} }
int
fmu_stop(void)
{
int ret = OK;
if (g_fmu != nullptr) {
delete g_fmu;
g_fmu = nullptr;
}
return ret;
}
void void
test(void) test(void)
{ {
@ -1136,7 +1347,7 @@ test(void)
} }
} else { } else {
// and use write interface for the other direction // 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)) if (ret != (int)sizeof(servos))
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); 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; PortMode new_mode = PORT_MODE_UNSET;
const char *verb = argv[1]; const char *verb = argv[1];
if (!strcmp(verb, "stop")) {
fmu_stop();
errx(0, "FMU driver stopped");
}
if (fmu_start() != OK) if (fmu_start() != OK)
errx(1, "failed to start the FMU driver"); errx(1, "failed to start the FMU driver");

3
src/drivers/px4fmu/module.mk
View File

@ -3,4 +3,5 @@
# #
MODULE_COMMAND = fmu MODULE_COMMAND = fmu
SRCS = fmu.cpp SRCS = fmu.cpp \
../../modules/systemlib/pwm_limit/pwm_limit.c

310
src/drivers/px4io/px4io.cpp
View File

@ -183,21 +183,6 @@ public:
*/ */
int set_failsafe_values(const uint16_t *vals, unsigned len); 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 * Disable RC input handling
*/ */
@ -953,55 +938,6 @@ PX4IO::io_set_control_state()
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); 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 int
PX4IO::io_set_arming_state() PX4IO::io_set_arming_state()
@ -1874,10 +1810,10 @@ PX4IO::print_status()
for (unsigned i = 0; i < _max_actuators; i++) for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 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++) 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"); 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); ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
break; 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: case PWM_SERVO_SET_UPDATE_RATE:
/* set the requested alternate rate */ /* set the requested alternate rate */
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg); ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE, arg);
break; 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 */ /* 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); 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; 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: case PWM_SERVO_GET_COUNT:
*(unsigned *)arg = _max_actuators; *(unsigned *)arg = _max_actuators;
break; break;
@ -1958,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
/* TODO: we could go lower for e.g. TurboPWM */ /* TODO: we could go lower for e.g. TurboPWM */
unsigned channel = cmd - PWM_SERVO_SET(0); 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; ret = -EINVAL;
} else { } else {
@ -2591,152 +2618,7 @@ px4io_main(int argc, char *argv[])
exit(0); 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")) { if (!strcmp(argv[1], "recovery")) {
@ -2808,5 +2690,5 @@ px4io_main(int argc, char *argv[])
bind(argc, argv); bind(argc, argv);
out: 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'");
} }

18
src/modules/commander/commander.cpp
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[]); 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); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[])
if (updated) { if (updated) {
orb_copy(ORB_ID(safety), safety_sub, &safety); 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 */ /* update global position estimate */
@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[])
} }
/* play arming and battery warning tunes */ /* 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 */ /* play tune when armed */
if (tune_arm() == OK) if (tune_arm() == OK)
arm_tune_played = true; arm_tune_played = true;
@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[])
} }
/* reset arm_tune_played when disarmed */ /* 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; arm_tune_played = false;
} }
@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
} }
void 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 */ /* driving rgbled */
if (changed) { 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 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
if (armed->armed) { if (actuator_armed->armed) {
/* armed, solid */ /* armed, solid */
led_on(LED_BLUE); led_on(LED_BLUE);
} else if (armed->ready_to_arm) { } else if (actuator_armed->ready_to_arm) {
/* ready to arm, blink at 1Hz */ /* ready to arm, blink at 1Hz */
if (leds_counter % 20 == 0) if (leds_counter % 20 == 0)
led_toggle(LED_BLUE); led_toggle(LED_BLUE);

2
src/modules/mavlink/missionlib.c
View File

@ -220,7 +220,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
sp.lon = param6_lon_y * 1e7f; sp.lon = param6_lon_y * 1e7f;
sp.altitude = param7_alt_z; sp.altitude = param7_alt_z;
sp.altitude_is_relative = false; 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); set_special_fields(param1, param2, param3, param4, command, &sp);
/* Initialize setpoint publication if necessary */ /* Initialize setpoint publication if necessary */

145
src/modules/px4iofirmware/mixer.cpp
View File

@ -47,6 +47,7 @@
#include <drivers/drv_pwm_output.h> #include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/mixer/mixer.h> #include <systemlib/mixer/mixer.h>
extern "C" { extern "C" {
@ -59,12 +60,6 @@ extern "C" {
*/ */
#define FMU_INPUT_DROP_LIMIT_US 200000 #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 */ /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
#define ROLL 0 #define ROLL 0
#define PITCH 1 #define PITCH 1
@ -76,15 +71,6 @@ extern "C" {
static bool mixer_servos_armed = false; static bool mixer_servos_armed = false;
static bool should_arm = false; static bool should_arm = false;
static bool should_always_enable_pwm = 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 */ /* selected control values and count for mixing */
enum mixer_source { enum mixer_source {
@ -165,102 +151,6 @@ mixer_tick(void)
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); 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. * 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_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_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) { if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
/* need to arm, but not armed */ /* need to arm, but not armed */
up_pwm_servo_arm(true); up_pwm_servo_arm(true);
@ -298,7 +216,6 @@ mixer_tick(void)
mixer_servos_armed = false; mixer_servos_armed = false;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
isr_debug(5, "> PWM disabled"); isr_debug(5, "> PWM disabled");
} }
if (mixer_servos_armed && should_arm) { if (mixer_servos_armed && should_arm) {
@ -307,9 +224,9 @@ mixer_tick(void)
up_pwm_servo_set(i, r_page_servos[i]); up_pwm_servo_set(i, r_page_servos[i]);
} else if (mixer_servos_armed && should_always_enable_pwm) { } 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++) 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]);
} }
} }

1
src/modules/px4iofirmware/module.mk
View File

@ -14,6 +14,7 @@ SRCS = adc.c \
../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_group.cpp \
../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \
../systemlib/mixer/mixer_simple.cpp \ ../systemlib/mixer/mixer_simple.cpp \
../systemlib/pwm_limit/pwm_limit.c
ifeq ($(BOARD),px4io-v1) ifeq ($(BOARD),px4io-v1)
SRCS += i2c.c SRCS += i2c.c

4
src/modules/px4iofirmware/protocol.h
View File

@ -220,8 +220,8 @@ enum { /* DSM bind states */
/* PWM maximum values for certain ESCs */ /* PWM maximum values for certain ESCs */
#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM idle values that are active, even when SAFETY_SAFE */ /* PWM disarmed values that are active, even when SAFETY_SAFE */
#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ #define PX4IO_PAGE_DISARMED_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/** /**
* As-needed mixer data upload. * As-needed mixer data upload.

6
src/modules/px4iofirmware/px4io.c
View File

@ -50,6 +50,7 @@
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <stm32_uart.h> #include <stm32_uart.h>
@ -64,6 +65,8 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call; 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 * a set of debug buffers to allow us to send debug information from ISRs
*/ */
@ -171,6 +174,9 @@ user_start(int argc, char *argv[])
struct mallinfo minfo = mallinfo(); struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
#if 0 #if 0
/* not enough memory, lock down */ /* not enough memory, lock down */
if (minfo.mxordblk < 500) { if (minfo.mxordblk < 500) {

9
src/modules/px4iofirmware/px4io.h
View File

@ -46,6 +46,8 @@
#include "protocol.h" #include "protocol.h"
#include <systemlib/pwm_limit/pwm_limit.h>
/* /*
* Constants and limits. * 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_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_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_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. * Register aliases.
@ -122,6 +124,11 @@ struct sys_state_s {
extern struct sys_state_s system_state; extern struct sys_state_s system_state;
/*
* PWM limit structure
*/
extern pwm_limit_t pwm_limit;
/* /*
* GPIO handling. * GPIO handling.
*/ */

107
src/modules/px4iofirmware/registers.c
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. * 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 * PAGE 106
@ -207,7 +207,7 @@ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
* minimum PWM values when armed * 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 * 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 * 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 * 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 int
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) 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 */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
/* XXX range-check value? */ if (*values == 0) {
r_page_servo_failsafe[offset] = *values; /* 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 */ /* flag the failsafe values as custom */
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_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 */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) if (*values == 0) {
/* set to default */ /* ignore 0 */
r_page_servo_control_min[offset] = 900; } else if (*values > PWM_HIGHEST_MIN) {
r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
else if (*values > 1200) } else if (*values < PWM_LOWEST_MIN) {
r_page_servo_control_min[offset] = 1200; r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
else if (*values < 900) } else {
r_page_servo_control_min[offset] = 900;
else
r_page_servo_control_min[offset] = *values; r_page_servo_control_min[offset] = *values;
}
offset++; offset++;
num_values--; num_values--;
@ -315,16 +321,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) if (*values == 0) {
/* set to default */ /* ignore 0 */
r_page_servo_control_max[offset] = 2100; } else if (*values > PWM_HIGHEST_MAX) {
r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
else if (*values > 2100) } else if (*values < PWM_LOWEST_MAX) {
r_page_servo_control_max[offset] = 2100; r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
else if (*values < 1800) } else {
r_page_servo_control_max[offset] = 1800;
else
r_page_servo_control_max[offset] = *values; r_page_servo_control_max[offset] = *values;
}
offset++; offset++;
num_values--; num_values--;
@ -332,28 +337,40 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
} }
break; break;
case PX4IO_PAGE_IDLE_PWM: case PX4IO_PAGE_DISARMED_PWM:
{
/* flag for all outputs */
bool all_disarmed_off = true;
/* copy channel data */ /* copy channel data */
while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
if (*values == 0) if (*values == 0) {
/* set to default */ /* 0 means disabling always PWM */
r_page_servo_idle[offset] = 0; 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) offset++;
r_page_servo_idle[offset] = 900; num_values--;
else if (*values > 2100) values++;
r_page_servo_idle[offset] = 2100; }
else
r_page_servo_idle[offset] = *values;
/* flag the failsafe values as custom */ if (all_disarmed_off) {
r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; /* disable PWM output if disarmed */
r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE);
offset++; } else {
num_values--; /* enable PWM output always */
values++; r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
}
} }
break; 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: case PX4IO_PAGE_CONTROL_MAX_PWM:
SELECT_PAGE(r_page_servo_control_max); SELECT_PAGE(r_page_servo_control_max);
break; break;
case PX4IO_PAGE_IDLE_PWM: case PX4IO_PAGE_DISARMED_PWM:
SELECT_PAGE(r_page_servo_idle); SELECT_PAGE(r_page_servo_disarmed);
break; break;
default: default:

1
src/modules/systemlib/mixer/mixer.h
View File

@ -449,6 +449,7 @@ public:
HEX_PLUS, /**< hex in + configuration */ HEX_PLUS, /**< hex in + configuration */
OCTA_X, OCTA_X,
OCTA_PLUS, OCTA_PLUS,
OCTA_COX,
MAX_GEOMETRY MAX_GEOMETRY
}; };

15
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -130,6 +130,16 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
{ 1.000000, 0.000000, -1.00 }, { 1.000000, 0.000000, -1.00 },
{ -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] = { const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_quad_x[0], &_config_quad_x[0],
&_config_quad_plus[0], &_config_quad_plus[0],
@ -139,6 +149,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_hex_plus[0], &_config_hex_plus[0],
&_config_octa_x[0], &_config_octa_x[0],
&_config_octa_plus[0], &_config_octa_plus[0],
&_config_octa_cox[0],
}; };
const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
4, /* quad_x */ 4, /* quad_x */
@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
6, /* hex_plus */ 6, /* hex_plus */
8, /* octa_x */ 8, /* octa_x */
8, /* octa_plus */ 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")) { } else if (!strcmp(geomname, "8x")) {
geometry = MultirotorMixer::OCTA_X; geometry = MultirotorMixer::OCTA_X;
} else if (!strcmp(geomname, "8c")) {
geometry = MultirotorMixer::OCTA_COX;
} else { } else {
debug("unrecognised geometry '%s'", geomname); debug("unrecognised geometry '%s'", geomname);

13
src/modules/systemlib/mixer/multi_tables
View File

@ -74,7 +74,18 @@ set octa_plus {
90 CW 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]]} proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}

133
src/modules/systemlib/pwm_limit/pwm_limit.c Normal file
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;
}

77
src/modules/systemlib/pwm_limit/pwm_limit.h Normal file
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_ */

2
src/modules/uORB/topics/actuator_outputs.h
View File

@ -60,7 +60,7 @@
struct actuator_outputs_s { struct actuator_outputs_s {
uint64_t timestamp; /**< output timestamp in us since system boot */ uint64_t timestamp; /**< output timestamp in us since system boot */
float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */ float output[NUM_ACTUATOR_OUTPUTS]; /**< output data, in natural output units */
int noutputs; /**< valid outputs */ unsigned noutputs; /**< valid outputs */
}; };
/** /**

2
src/systemcmds/config/config.c
View File

@ -272,7 +272,7 @@ do_accel(int argc, char *argv[])
} }
} else { } 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); int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);

216
src/systemcmds/esc_calib/esc_calib.c
View File

@ -62,23 +62,27 @@
#include "systemlib/err.h" #include "systemlib/err.h"
#include "drivers/drv_pwm_output.h" #include "drivers/drv_pwm_output.h"
#include <uORB/topics/actuator_controls.h>
static void usage(const char *reason); static void usage(const char *reason);
__EXPORT int esc_calib_main(int argc, char *argv[]); __EXPORT int esc_calib_main(int argc, char *argv[]);
#define MAX_CHANNELS 14
static void static void
usage(const char *reason) usage(const char *reason)
{ {
if (reason != NULL) if (reason != NULL)
warnx("%s", reason); 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 int
@ -86,157 +90,247 @@ esc_calib_main(int argc, char *argv[])
{ {
char *dev = PWM_OUTPUT_DEVICE_PATH; char *dev = PWM_OUTPUT_DEVICE_PATH;
char *ep; char *ep;
bool channels_selected[MAX_CHANNELS] = {false};
int ch; int ch;
int ret; int ret;
char c; 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; struct pollfd fds;
fds.fd = 0; /* stdin */ fds.fd = 0; /* stdin */
fds.events = POLLIN; fds.events = POLLIN;
if (argc < 2) if (argc < 2) {
usage(NULL); 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) { switch (ch) {
case 'd': case 'd':
dev = optarg; dev = optarg;
argc-=2; arg_consumed += 2;
break; 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: default:
usage(NULL); usage(NULL);
} }
} }
if(argc < 2) { if (set_mask == 0)
usage("no channels provided"); usage("no channels chosen");
}
while (--argc) { /* make sure no other source is publishing control values now */
const char *arg = argv[argc]; struct actuator_controls_s actuators;
unsigned channel_number = strtol(arg, &ep, 0); int act_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
if (*ep == '\0') { /* clear changed flag */
if (channel_number > MAX_CHANNELS || channel_number <= 0) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, act_sub, &actuators);
err(1, "invalid channel number: %d", channel_number);
} else {
channels_selected[channel_number-1] = true;
} /* 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" printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
"\n" "\n"
"Make sure\n" "Make sure\n"
"\t - that the ESCs are not powered\n" "\t - that the ESCs are not powered\n"
"\t - that safety is off (two short blinks)\n" "\t - that safety is off (two short blinks)\n"
"\t - that the controllers are stopped\n" "\t - that the controllers are stopped\n"
"\n" "\n"
"Do you want to start calibration now: y or n?\n"); "Do you want to start calibration now: y or n?\n");
/* wait for user input */ /* wait for user input */
while (1) { while (1) {
ret = poll(&fds, 1, 0); ret = poll(&fds, 1, 0);
if (ret > 0) { if (ret > 0) {
read(0, &c, 1); read(0, &c, 1);
if (c == 'y' || c == 'Y') { if (c == 'y' || c == 'Y') {
break; break;
} else if (c == 0x03 || c == 0x63 || c == 'q') { } else if (c == 0x03 || c == 0x63 || c == 'q') {
printf("ESC calibration exited\n"); printf("ESC calibration exited\n");
exit(0); exit(0);
} else if (c == 'n' || c == 'N') { } else if (c == 'n' || c == 'N') {
printf("ESC calibration aborted\n"); printf("ESC calibration aborted\n");
exit(0); exit(0);
} else { } else {
printf("Unknown input, ESC calibration aborted\n"); printf("Unknown input, ESC calibration aborted\n");
exit(0); exit(0);
} }
} }
/* rate limit to ~ 20 Hz */ /* rate limit to ~ 20 Hz */
usleep(50000); usleep(50000);
} }
/* open for ioctl only */ /* open for ioctl only */
int fd = open(dev, 0); int fd = open(dev, 0);
if (fd < 0) if (fd < 0)
err(1, "can't open %s", dev); err(1, "can't open %s", dev);
/* Wait for user confirmation */ /* get number of channels available on the device */
printf("\nHigh PWM set\n" ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels);
"\n" if (ret != OK)
"Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n" err(1, "PWM_SERVO_GET_COUNT");
"\n");
/* 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); fflush(stdout);
while (1) { 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) 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); ret = poll(&fds, 1, 0);
if (ret > 0) { if (ret > 0) {
read(0, &c, 1); read(0, &c, 1);
if (c == 13) { if (c == 13) {
break; break;
} else if (c == 0x03 || c == 0x63 || c == 'q') { } else if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("ESC calibration exited"); warnx("ESC calibration exited");
exit(0); exit(0);
} }
} }
/* rate limit to ~ 20 Hz */ /* rate limit to ~ 20 Hz */
usleep(50000); usleep(50000);
} }
/* we don't need any more user input */ printf("Low PWM set: %d\n"
"\n"
"Hit ENTER when finished\n"
printf("Low PWM set, hit ENTER when finished\n" "\n", pwm_low);
"\n");
while (1) { while (1) {
/* Then set low PWM */ /* set disarmed PWM */
for (unsigned i = 0; i<MAX_CHANNELS; i++) { for (unsigned i = 0; i < max_channels; i++) {
if(channels_selected[i]) { if (set_mask & 1<<i) {
ret = ioctl(fd, PWM_SERVO_SET(i), 900); ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low);
if (ret != OK) 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); ret = poll(&fds, 1, 0);
if (ret > 0) { if (ret > 0) {
read(0, &c, 1); read(0, &c, 1);
if (c == 13) { if (c == 13) {
break; break;
} else if (c == 0x03 || c == 0x63 || c == 'q') { } else if (c == 0x03 || c == 0x63 || c == 'q') {
printf("ESC calibration exited\n"); printf("ESC calibration exited\n");
exit(0); exit(0);
} }
} }
/* rate limit to ~ 20 Hz */ /* rate limit to ~ 20 Hz */
usleep(50000); 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"); printf("ESC calibration finished\n");
exit(0); exit(0);

496
src/systemcmds/pwm/pwm.c
View File

@ -45,6 +45,7 @@
#include <stdbool.h> #include <stdbool.h>
#include <unistd.h> #include <unistd.h>
#include <fcntl.h> #include <fcntl.h>
#include <poll.h>
#include <sys/mount.h> #include <sys/mount.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
#include <sys/stat.h> #include <sys/stat.h>
@ -71,16 +72,36 @@ usage(const char *reason)
warnx("%s", reason); warnx("%s", reason);
errx(1, errx(1,
"usage:\n" "usage:\n"
"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [-m chanmask ] [arm|disarm] [<channel_value> ...]\n" "pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info ...\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"
"\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; unsigned alt_rate = 0;
uint32_t alt_channel_groups = 0; uint32_t alt_channel_groups = 0;
bool alt_channels_set = false; bool alt_channels_set = false;
bool print_info = false; bool print_verbose = false;
int ch; int ch;
int ret; int ret;
char *ep; char *ep;
uint32_t set_mask = 0;
unsigned group; 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); 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) { 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': 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); group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32)) if ((*ep != '\0') || (group >= 32))
usage("bad channel_group value"); usage("bad channel_group value");
alt_channel_groups |= (1 << group); alt_channel_groups |= (1 << group);
alt_channels_set = true; alt_channels_set = true;
break; warnx("alt channels set, group: %d", group);
case 'd':
dev = optarg;
break;
case 'u':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
break; break;
case 'm': case 'm':
set_mask = strtol(optarg, &ep, 0); /* Read in mask directly */
set_mask = strtoul(optarg, &ep, 0);
if (*ep != '\0') if (*ep != '\0')
usage("bad set_mask value"); usage("bad set_mask value");
break; break;
case 'v': case 'a':
print_info = true; 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; break;
default: 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 */ /* open for ioctl only */
int fd = open(dev, 0); int fd = open(dev, 0);
if (fd < 0) if (fd < 0)
err(1, "can't open %s", dev); err(1, "can't open %s", dev);
/* change alternate PWM rate */ /* get the number of servo channels */
if (alt_rate > 0) { unsigned servo_count;
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate); 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) if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)"); 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);
/* directly supplied channel mask */
if (set_mask != -1) {
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, set_mask);
if (ret != OK) if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE"); err(1, "PWM_SERVO_ARM");
}
/* assign alternate rate to channel groups */ if (print_verbose)
if (alt_channels_set) { warnx("Outputs armed");
uint32_t mask = 0; exit(0);
for (unsigned group = 0; group < 32; group++) { } else if (!strcmp(argv[1], "disarm")) {
if ((1 << group) & alt_channel_groups) { /* disarm, but do not revoke the SET_ARM_OK flag */
uint32_t group_mask; 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 (print_verbose)
if (ret != OK) warnx("Outputs disarmed");
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group); 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 */ } else if (!strcmp(argv[1], "info")) {
unsigned nchannels = 0;
unsigned channel[8] = {0}; printf("device: %s\n", dev);
while (argc--) {
const char *arg = argv[0]; uint32_t info_default_rate;
argv++; uint32_t info_alt_rate;
if (!strcmp(arg, "arm")) { uint32_t info_alt_rate_mask;
/* tell IO that its ok to disable its safety with the switch */
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); ret = ioctl(fd, PWM_SERVO_GET_DEFAULT_UPDATE_RATE, (unsigned long)&info_default_rate);
if (ret != OK) if (ret != OK) {
err(1, "PWM_SERVO_SET_ARM_OK"); err(1, "PWM_SERVO_GET_DEFAULT_UPDATE_RATE");
/* 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;
} }
if (!strcmp(arg, "disarm")) {
/* disarm, but do not revoke the SET_ARM_OK flag */ ret = ioctl(fd, PWM_SERVO_GET_UPDATE_RATE, (unsigned long)&info_alt_rate);
ret = ioctl(fd, PWM_SERVO_DISARM, 0); if (ret != OK) {
if (ret != OK) err(1, "PWM_SERVO_GET_UPDATE_RATE");
err(1, "PWM_SERVO_DISARM");
continue;
} }
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; ret = ioctl(fd, PWM_SERVO_GET_SELECT_UPDATE_RATE, (unsigned long)&info_alt_rate_mask);
nchannels++; if (ret != OK) {
err(1, "PWM_SERVO_GET_SELECT_UPDATE_RATE");
continue;
} }
usage("unrecognized option");
}
/* print verbose info */ struct pwm_output_values failsafe_pwm;
if (print_info) { struct pwm_output_values disarmed_pwm;
/* get the number of servo channels */ struct pwm_output_values min_pwm;
unsigned count; struct pwm_output_values max_pwm;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
if (ret != OK) ret = ioctl(fd, PWM_SERVO_GET_FAILSAFE_PWM, (unsigned long)&failsafe_pwm);
err(1, "PWM_SERVO_GET_COUNT"); 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 */ /* print current servo values */
for (unsigned i = 0; i < count; i++) { for (unsigned i = 0; i < servo_count; i++) {
servo_position_t spos; servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos); ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) { 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 { } else {
printf("%u: ERROR\n", i); printf("%u: ERROR\n", i);
} }
} }
/* print rate groups */ /* print rate groups */
for (unsigned i = 0; i < count; i++) { for (unsigned i = 0; i < servo_count; i++) {
uint32_t group_mask; uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&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); printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++) for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j)) if (group_mask & (1 << j))
printf(" %u", j); printf(" %u", j+1);
printf("\n"); 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);
}