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
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

15
ROMFS/px4fmu_common/init.d/11_dji_f450
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

15
ROMFS/px4fmu_common/init.d/15_tbs_discovery
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

15
ROMFS/px4fmu_common/init.d/16_3dr_iris
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

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
#
pwm -u 400 -m 0xff
pwm rate -c 1234 -r 400
#
# 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
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

30
ROMFS/px4fmu_common/init.d/rc.custom_io_esc
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

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
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#

14
ROMFS/px4fmu_common/init.d/rcS
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

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
/**
* 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)

4
src/drivers/hil/hil.cpp
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;

2
src/drivers/mkblctrl/mkblctrl.cpp
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;

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
* 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");

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

@ -3,4 +3,5 @@
#
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);
/**
* 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'");
}

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[]);
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);
@ -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 */
@ -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);

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.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 */

145
src/modules/px4iofirmware/mixer.cpp
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]);
}
}

1
src/modules/px4iofirmware/module.mk
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

4
src/modules/px4iofirmware/protocol.h
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.

6
src/modules/px4iofirmware/px4io.c
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
*/
@ -171,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) {

9
src/modules/px4iofirmware/px4io.h
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.
*/

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.
*/
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:

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

@ -449,6 +449,7 @@ public:
HEX_PLUS, /**< hex in + configuration */
OCTA_X,
OCTA_PLUS,
OCTA_COX,
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 },
};
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);

13
src/modules/systemlib/mixer/multi_tables
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]]}

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 {
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 */
};
/**

2
src/systemcmds/config/config.c
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);

216
src/systemcmds/esc_calib/esc_calib.c
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);

496
src/systemcmds/pwm/pwm.c
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);
}