forked from Archive/PX4-Autopilot
pwm_out multi-instance support
PX4 uses banks of 8 outputs as a logical structure. Boards that have more outputs than 8 get multiple instances. This is an arbitrary choice that helps with overall structure and enables the mixing of different device classes (like FMU, IO or UAVCAN).
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
c1c0a62be5
commit
e7722b11eb
|
|
@ -32,7 +32,7 @@ fi
|
|||
# USE_IO is set to 'no' for all boards w/o px4io driver or SYS_USE_IO disabled
|
||||
if [ $USE_IO = no ]
|
||||
then
|
||||
set MIXER_AUX none
|
||||
set AUX_MODE none
|
||||
fi
|
||||
|
||||
#
|
||||
|
|
@ -147,6 +147,7 @@ then
|
|||
if mixer load ${OUTPUT_DEV} ${MIXER_FILE}
|
||||
then
|
||||
echo "INFO [init] Mixer: ${MIXER_FILE} on ${OUTPUT_DEV}"
|
||||
|
||||
else
|
||||
echo "ERROR [init] Failed loading mixer: ${MIXER_FILE}"
|
||||
tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
|
||||
|
|
@ -199,6 +200,7 @@ then
|
|||
echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}"
|
||||
fi
|
||||
else
|
||||
echo "INFO [init] setting PWM_AUX_OUT none"
|
||||
set PWM_AUX_OUT none
|
||||
fi
|
||||
else
|
||||
|
|
@ -223,6 +225,36 @@ then
|
|||
fi
|
||||
fi
|
||||
|
||||
if [ $MIXER_AUX != none -a $AUX_MODE = none -a -e $OUTPUT_AUX_DEV ]
|
||||
then
|
||||
#
|
||||
# Load aux mixer.
|
||||
#
|
||||
if [ -f ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix ]
|
||||
then
|
||||
set MIXER_AUX_FILE ${SDCARD_MIXERS_PATH}/${MIXER_AUX}.aux.mix
|
||||
else
|
||||
|
||||
if [ -f /etc/mixers/${MIXER_AUX}.aux.mix ]
|
||||
then
|
||||
set MIXER_AUX_FILE /etc/mixers/${MIXER_AUX}.aux.mix
|
||||
fi
|
||||
fi
|
||||
|
||||
if mixer load ${OUTPUT_AUX_DEV} ${MIXER_AUX_FILE}
|
||||
then
|
||||
echo "INFO [init] Mixer: ${MIXER_AUX_FILE} on ${OUTPUT_AUX_DEV}"
|
||||
|
||||
# Set PWM_AUX output frequency.
|
||||
if [ $PWM_AUX_RATE != none ]
|
||||
then
|
||||
pwm rate -c ${PWM_AUX_OUT} -r ${PWM_AUX_RATE} -d ${OUTPUT_AUX_DEV}
|
||||
fi
|
||||
else
|
||||
echo "ERROR [init] Failed loading mixer: ${MIXER_AUX_FILE}"
|
||||
fi
|
||||
fi
|
||||
|
||||
if [ $OUTPUT_MODE = pwm_out -o $OUTPUT_MODE = io ]
|
||||
then
|
||||
if [ $PWM_OUT != none ]
|
||||
|
|
|
|||
|
|
@ -33,21 +33,27 @@
|
|||
|
||||
#include "PWMOut.hpp"
|
||||
|
||||
PWMOut::PWMOut() :
|
||||
CDev(PX4FMU_DEVICE_PATH),
|
||||
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default),
|
||||
pthread_mutex_t pwm_out_module_mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
static px4::atomic<PWMOut *> _objects[PWM_OUT_MAX_INSTANCES] {};
|
||||
|
||||
static bool is_running() { return (_objects[0].load() != nullptr) || (_objects[1].load() != nullptr); }
|
||||
|
||||
PWMOut::PWMOut(int instance, uint8_t output_base) :
|
||||
CDev((instance == 0) ? PX4FMU_DEVICE_PATH : PX4FMU_DEVICE_PATH"1"),
|
||||
OutputModuleInterface((instance == 0) ? MODULE_NAME"0" : MODULE_NAME"1", px4::wq_configurations::hp_default),
|
||||
_instance(instance),
|
||||
_output_base(output_base),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
|
||||
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": interval"))
|
||||
{
|
||||
_mixing_output.setAllMinValues(PWM_DEFAULT_MIN);
|
||||
_mixing_output.setAllMaxValues(PWM_DEFAULT_MAX);
|
||||
|
||||
}
|
||||
|
||||
PWMOut::~PWMOut()
|
||||
{
|
||||
/* make sure servos are off */
|
||||
up_pwm_servo_deinit();
|
||||
up_pwm_servo_deinit(); // TODO: review for multi
|
||||
|
||||
/* clean up the alternate device node */
|
||||
unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance);
|
||||
|
|
@ -106,9 +112,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0x1;
|
||||
_pwm_mask = 0b0000'0000'0000'0001 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 1;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
break;
|
||||
|
||||
|
|
@ -129,9 +136,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0x3;
|
||||
_pwm_mask = 0b0000'0000'0000'0011 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 2;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -152,9 +160,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0x7;
|
||||
_pwm_mask = 0b0000'0000'0000'0111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 3;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -175,9 +184,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0xf;
|
||||
_pwm_mask = 0b0000'0000'0000'1111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 4;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -192,9 +202,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 400;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0x0f;
|
||||
_pwm_mask = 0b0000'0000'0000'1111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 4;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -216,9 +227,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0x1f;
|
||||
_pwm_mask = 0b0000'0000'0001'1111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 5;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -232,9 +244,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0x3f;
|
||||
_pwm_mask = 0b0000'0000'0011'1111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 6;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -248,9 +261,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0xff;
|
||||
_pwm_mask = 0b0000'0000'1111'1111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 8;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -264,9 +278,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0xFFF;
|
||||
_pwm_mask = 0b0000'1111'1111'1111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 12;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -280,9 +295,10 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_default_rate = 50;
|
||||
_pwm_alt_rate = 50;
|
||||
_pwm_alt_rate_channels = 0;
|
||||
_pwm_mask = 0x3fff;
|
||||
_pwm_mask = 0b0011'1111'1111'1111 << _output_base;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 14;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
break;
|
||||
|
|
@ -297,11 +313,12 @@ int PWMOut::set_mode(Mode mode)
|
|||
_pwm_mask = 0x0;
|
||||
_pwm_initialized = false;
|
||||
_num_outputs = 0;
|
||||
_mixing_output.setMaxNumOutputs(_num_outputs);
|
||||
update_params();
|
||||
|
||||
if (old_mask != _pwm_mask) {
|
||||
/* disable servo outputs - no need to set rates */
|
||||
up_pwm_servo_deinit();
|
||||
up_pwm_servo_deinit(); // TODO: review for multi
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -342,7 +359,7 @@ int PWMOut::set_mode(Mode mode)
|
|||
*/
|
||||
int PWMOut::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
|
||||
{
|
||||
PX4_DEBUG("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
|
||||
PX4_DEBUG("pwm_out%u set_pwm_rate %x %u %u", _instance, rate_map, default_rate, alt_rate);
|
||||
|
||||
for (unsigned pass = 0; pass < 2; pass++) {
|
||||
|
||||
|
|
@ -448,31 +465,34 @@ void PWMOut::update_current_rate()
|
|||
// max interval 0.5 - 100 ms (10 - 2000Hz)
|
||||
const int update_interval_in_us = math::constrain(1000000 / max_rate, 500, 100000);
|
||||
|
||||
PX4_INFO("instance: %d, MAX RATE: %d, default: %d, alt: %d", _instance, max_rate, _pwm_default_rate, _pwm_alt_rate);
|
||||
|
||||
_current_update_rate = max_rate;
|
||||
_mixing_output.setMaxTopicUpdateRate(update_interval_in_us);
|
||||
}
|
||||
|
||||
int PWMOut::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
PWMOut *instance = new PWMOut();
|
||||
for (int instance = 0; instance < PWM_OUT_MAX_INSTANCES; instance++) {
|
||||
uint8_t base = instance * 8; // TODO: configurable
|
||||
PWMOut *dev = new PWMOut(instance, base);
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
if (dev) {
|
||||
_objects[instance].store(dev);
|
||||
|
||||
if (instance->init() == PX4_OK) {
|
||||
return PX4_OK;
|
||||
if (dev->init() != PX4_OK) {
|
||||
PX4_ERR("%d - init failed", instance);
|
||||
delete dev;
|
||||
_objects[instance].store(nullptr);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void PWMOut::capture_trampoline(void *context, uint32_t chan_index,
|
||||
|
|
@ -496,7 +516,7 @@ void PWMOut::update_pwm_out_state(bool on)
|
|||
_pwm_initialized = true;
|
||||
}
|
||||
|
||||
up_pwm_servo_arm(on);
|
||||
up_pwm_servo_arm(on); // TODO REVIEW for multi
|
||||
}
|
||||
|
||||
bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
||||
|
|
@ -509,7 +529,7 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
|||
/* output to the servos */
|
||||
if (_pwm_initialized) {
|
||||
for (size_t i = 0; i < math::min(_num_outputs, num_outputs); i++) {
|
||||
up_pwm_servo_set(i, outputs[i]);
|
||||
up_pwm_servo_set(_output_base + i, outputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -517,7 +537,7 @@ bool PWMOut::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
|
|||
* the oneshots with updated values.
|
||||
*/
|
||||
if (num_control_groups_updated > 0) {
|
||||
up_pwm_update();
|
||||
up_pwm_update(); // TODO: review for multi
|
||||
}
|
||||
|
||||
return true;
|
||||
|
|
@ -529,7 +549,7 @@ void PWMOut::Run()
|
|||
ScheduleClear();
|
||||
_mixing_output.unregister();
|
||||
|
||||
exit_and_cleanup();
|
||||
//exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -711,10 +731,10 @@ void PWMOut::update_params()
|
|||
uint16_t &reverse_pwm_mask = _mixing_output.reverseOutputMask();
|
||||
|
||||
if (pwm_rev >= 1) {
|
||||
reverse_pwm_mask = reverse_pwm_mask | (2 << i);
|
||||
reverse_pwm_mask = reverse_pwm_mask | (1 << i);
|
||||
|
||||
} else {
|
||||
reverse_pwm_mask = reverse_pwm_mask & ~(2 << i);
|
||||
reverse_pwm_mask = reverse_pwm_mask & ~(1 << i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -777,7 +797,7 @@ int PWMOut::ioctl(file *filp, int cmd, unsigned long arg)
|
|||
break;
|
||||
|
||||
default:
|
||||
PX4_DEBUG("not in a PWM mode");
|
||||
PX4_DEBUG("pwm_out%u, not in a PWM mode", _instance);
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -793,7 +813,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
{
|
||||
int ret = OK;
|
||||
|
||||
PX4_DEBUG("ioctl cmd: %d, arg: %ld", cmd, arg);
|
||||
PX4_DEBUG("pwm_out%u: ioctl cmd: %d, arg: %ld", _instance, cmd, arg);
|
||||
|
||||
lock();
|
||||
|
||||
|
|
@ -1101,7 +1121,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
case PWM_SERVO_SET(1):
|
||||
case PWM_SERVO_SET(0):
|
||||
if (arg <= 2100) {
|
||||
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
|
||||
up_pwm_servo_set(cmd - PWM_SERVO_SET(0 + _output_base), arg);
|
||||
|
||||
} else {
|
||||
ret = -EINVAL;
|
||||
|
|
@ -1173,7 +1193,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
/* FALLTHROUGH */
|
||||
case PWM_SERVO_GET(1):
|
||||
case PWM_SERVO_GET(0):
|
||||
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
|
||||
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0 + _output_base));
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_RATEGROUP(0):
|
||||
|
|
@ -1198,7 +1218,7 @@ int PWMOut::pwm_ioctl(file *filp, int cmd, unsigned long arg)
|
|||
case PWM_SERVO_GET_RATEGROUP(12):
|
||||
case PWM_SERVO_GET_RATEGROUP(13):
|
||||
#endif
|
||||
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
|
||||
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0 + _output_base));
|
||||
break;
|
||||
|
||||
case PWM_SERVO_GET_COUNT:
|
||||
|
|
@ -1552,9 +1572,8 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
|
|||
return -1;
|
||||
}
|
||||
|
||||
PWMOut::Mode servo_mode;
|
||||
|
||||
servo_mode = PWMOut::MODE_NONE;
|
||||
PWMOut::Mode pwm_mode0 = PWMOut::MODE_NONE;
|
||||
PWMOut::Mode pwm_mode1 = PWMOut::MODE_NONE;
|
||||
|
||||
switch (new_mode) {
|
||||
case PORT_FULL_GPIO:
|
||||
|
|
@ -1565,56 +1584,64 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
|
|||
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 4
|
||||
/* select 4-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_4PWM;
|
||||
pwm_mode0 = PWMOut::MODE_4PWM;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 5
|
||||
servo_mode = PWMOut::MODE_5PWM;
|
||||
pwm_mode0 = PWMOut::MODE_5PWM;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 6
|
||||
servo_mode = PWMOut::MODE_6PWM;
|
||||
pwm_mode0 = PWMOut::MODE_6PWM;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 8
|
||||
servo_mode = PWMOut::MODE_8PWM;
|
||||
pwm_mode0 = PWMOut::MODE_8PWM;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 12
|
||||
servo_mode = PWMOut::MODE_12PWM;
|
||||
//pwm_mode0 = PWMOut::MODE_12PWM;
|
||||
pwm_mode0 = PWMOut::MODE_8PWM;
|
||||
pwm_mode1 = PWMOut::MODE_4PWM;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM == 14
|
||||
servo_mode = PWMOut::MODE_14PWM;
|
||||
//pwm_mode0 = PWMOut::MODE_14PWM;
|
||||
pwm_mode0 = PWMOut::MODE_8PWM;
|
||||
pwm_mode1 = PWMOut::MODE_6PWM;
|
||||
#endif
|
||||
break;
|
||||
|
||||
case PORT_PWM1:
|
||||
/* select 2-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_1PWM;
|
||||
pwm_mode0 = PWMOut::MODE_1PWM;
|
||||
break;
|
||||
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 14
|
||||
|
||||
case PORT_PWM14:
|
||||
/* select 14-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_14PWM;
|
||||
//pwm_mode0 = PWMOut::MODE_14PWM;
|
||||
pwm_mode0 = PWMOut::MODE_8PWM;
|
||||
pwm_mode1 = PWMOut::MODE_6PWM;
|
||||
break;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 12
|
||||
|
||||
case PORT_PWM12:
|
||||
/* select 12-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_12PWM;
|
||||
//pwm_mode0 = PWMOut::MODE_12PWM;
|
||||
pwm_mode0 = PWMOut::MODE_8PWM;
|
||||
pwm_mode1 = PWMOut::MODE_4PWM;
|
||||
break;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 8
|
||||
|
||||
case PORT_PWM8:
|
||||
/* select 8-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_8PWM;
|
||||
pwm_mode0 = PWMOut::MODE_8PWM;
|
||||
break;
|
||||
#endif
|
||||
#if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
|
||||
|
||||
case PORT_PWM6:
|
||||
/* select 6-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_6PWM;
|
||||
pwm_mode0 = PWMOut::MODE_6PWM;
|
||||
break;
|
||||
#endif
|
||||
|
||||
|
|
@ -1622,7 +1649,7 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
|
|||
|
||||
case PORT_PWM5:
|
||||
/* select 5-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_5PWM;
|
||||
pwm_mode0 = PWMOut::MODE_5PWM;
|
||||
break;
|
||||
|
||||
|
||||
|
|
@ -1630,7 +1657,7 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
|
|||
|
||||
case PORT_PWM5CAP1:
|
||||
/* select 5-pin PWM mode 1 capture */
|
||||
servo_mode = PWMOut::MODE_5PWM1CAP;
|
||||
pwm_mode0 = PWMOut::MODE_5PWM1CAP;
|
||||
break;
|
||||
|
||||
# endif
|
||||
|
|
@ -1640,7 +1667,7 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
|
|||
|
||||
case PORT_PWM4:
|
||||
/* select 4-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_4PWM;
|
||||
pwm_mode0 = PWMOut::MODE_4PWM;
|
||||
break;
|
||||
|
||||
|
||||
|
|
@ -1648,39 +1675,39 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
|
|||
|
||||
case PORT_PWM4CAP1:
|
||||
/* select 4-pin PWM mode 1 capture */
|
||||
servo_mode = PWMOut::MODE_4PWM1CAP;
|
||||
pwm_mode0 = PWMOut::MODE_4PWM1CAP;
|
||||
break;
|
||||
|
||||
case PORT_PWM4CAP2:
|
||||
/* select 4-pin PWM mode 2 capture */
|
||||
servo_mode = PWMOut::MODE_4PWM2CAP;
|
||||
pwm_mode0 = PWMOut::MODE_4PWM2CAP;
|
||||
break;
|
||||
|
||||
# endif
|
||||
|
||||
case PORT_PWM3:
|
||||
/* select 3-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_3PWM;
|
||||
pwm_mode0 = PWMOut::MODE_3PWM;
|
||||
break;
|
||||
|
||||
# if defined(BOARD_HAS_CAPTURE)
|
||||
|
||||
case PORT_PWM3CAP1:
|
||||
/* select 3-pin PWM mode 1 capture */
|
||||
servo_mode = PWMOut::MODE_3PWM1CAP;
|
||||
pwm_mode0 = PWMOut::MODE_3PWM1CAP;
|
||||
break;
|
||||
# endif
|
||||
|
||||
case PORT_PWM2:
|
||||
/* select 2-pin PWM mode */
|
||||
servo_mode = PWMOut::MODE_2PWM;
|
||||
pwm_mode0 = PWMOut::MODE_2PWM;
|
||||
break;
|
||||
|
||||
# if defined(BOARD_HAS_CAPTURE)
|
||||
|
||||
case PORT_PWM2CAP2:
|
||||
/* select 2-pin PWM mode 2 capture */
|
||||
servo_mode = PWMOut::MODE_2PWM2CAP;
|
||||
pwm_mode0 = PWMOut::MODE_2PWM2CAP;
|
||||
break;
|
||||
|
||||
# endif
|
||||
|
|
@ -1690,11 +1717,16 @@ int PWMOut::fmu_new_mode(PortMode new_mode)
|
|||
return -1;
|
||||
}
|
||||
|
||||
PWMOut *object = get_instance();
|
||||
PWMOut *pwm0 = _objects[0].load(); // TODO: get_instance();
|
||||
|
||||
if (servo_mode != object->get_mode()) {
|
||||
/* (re)set the PWM output mode */
|
||||
object->set_mode(servo_mode);
|
||||
if (pwm0 && pwm_mode0 != pwm0->get_mode()) {
|
||||
pwm0->set_mode(pwm_mode0);
|
||||
}
|
||||
|
||||
PWMOut *pwm1 = _objects[1].load(); // TODO: get_instance();
|
||||
|
||||
if (pwm1 && pwm_mode1 != pwm1->get_mode()) {
|
||||
pwm1->set_mode(pwm_mode1);
|
||||
}
|
||||
|
||||
return OK;
|
||||
|
|
@ -1768,7 +1800,7 @@ int PWMOut::test()
|
|||
} else {
|
||||
input_capture_config_t conf = capture_conf[i].chan;
|
||||
conf.callback = &PWMOut::capture_trampoline;
|
||||
conf.context = PWMOut::get_instance();
|
||||
conf.context = _objects[0].load(); // TODO PWMOut::get_instance();
|
||||
|
||||
if (::ioctl(fd, INPUT_CAP_SET_CALLBACK, (unsigned long)&conf) == 0) {
|
||||
capture_conf[i].valid = true;
|
||||
|
|
@ -2056,7 +2088,17 @@ int PWMOut::custom_command(int argc, char *argv[])
|
|||
|
||||
int PWMOut::print_status()
|
||||
{
|
||||
PX4_INFO("Max update rate: %i Hz", _current_update_rate);
|
||||
if (_class_instance == CLASS_DEVICE_PRIMARY) {
|
||||
PX4_INFO("%d - PWM_MAIN 0x%04X", _instance, _pwm_mask);
|
||||
|
||||
} else if (_class_instance == CLASS_DEVICE_SECONDARY) {
|
||||
PX4_INFO("%d - PWM_AUX 0x%04X", _instance, _pwm_mask);
|
||||
|
||||
} else if (_class_instance == CLASS_DEVICE_TERTIARY) {
|
||||
PX4_INFO("%d - PWM_EXTRA 0x%04X", _instance, _pwm_mask);
|
||||
}
|
||||
|
||||
PX4_INFO("%d - Max update rate: %i Hz", _instance, _current_update_rate);
|
||||
|
||||
const char *mode_str = nullptr;
|
||||
|
||||
|
|
@ -2102,7 +2144,7 @@ int PWMOut::print_status()
|
|||
}
|
||||
|
||||
if (mode_str) {
|
||||
PX4_INFO("PWM Mode: %s", mode_str);
|
||||
PX4_INFO("%d - PWM Mode: %s", _instance, mode_str);
|
||||
}
|
||||
|
||||
perf_print_counter(_cycle_perf);
|
||||
|
|
@ -2197,5 +2239,89 @@ mixer files.
|
|||
|
||||
extern "C" __EXPORT int pwm_out_main(int argc, char *argv[])
|
||||
{
|
||||
return PWMOut::main(argc, argv);
|
||||
if (argc <= 1 || strcmp(argv[1], "-h") == 0) {
|
||||
return PWMOut::print_usage();
|
||||
}
|
||||
|
||||
if (strcmp(argv[1], "start") == 0) {
|
||||
int ret = 0;
|
||||
PWMOut::lock_module();
|
||||
|
||||
ret = PWMOut::task_spawn(argc - 1, argv + 1);
|
||||
|
||||
if (ret < 0) {
|
||||
PX4_ERR("start failed (%i)", ret);
|
||||
}
|
||||
|
||||
PWMOut::unlock_module();
|
||||
return ret;
|
||||
|
||||
} else if (strcmp(argv[1], "status") == 0) {
|
||||
if (PWMOut::trylock_module()) {
|
||||
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
|
||||
if (_objects[i].load()) {
|
||||
PX4_INFO_RAW("\n");
|
||||
_objects[i].load()->print_status();
|
||||
}
|
||||
}
|
||||
|
||||
PWMOut::unlock_module();
|
||||
|
||||
} else {
|
||||
PX4_WARN("module locked, try again later");
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
} else if (strcmp(argv[1], "stop") == 0) {
|
||||
PWMOut::lock_module();
|
||||
|
||||
if (argc > 2) {
|
||||
int instance = atoi(argv[2]);
|
||||
|
||||
if (instance >= 0 && instance < PWM_OUT_MAX_INSTANCES) {
|
||||
PX4_INFO("stopping instance %d", instance);
|
||||
PWMOut *inst = _objects[instance].load();
|
||||
|
||||
if (inst) {
|
||||
inst->request_stop();
|
||||
px4_usleep(20000); // 20 ms
|
||||
delete inst;
|
||||
_objects[instance].store(nullptr);
|
||||
}
|
||||
} else {
|
||||
PX4_ERR("invalid instance %d", instance);
|
||||
}
|
||||
|
||||
} else {
|
||||
// otherwise stop everything
|
||||
bool was_running = false;
|
||||
|
||||
for (int i = 0; i < PWM_OUT_MAX_INSTANCES; i++) {
|
||||
PWMOut *inst = _objects[i].load();
|
||||
|
||||
if (inst) {
|
||||
PX4_INFO("stopping pwm_out instance %d", i);
|
||||
was_running = true;
|
||||
inst->request_stop();
|
||||
px4_usleep(20000); // 20 ms
|
||||
delete inst;
|
||||
_objects[i].store(nullptr);
|
||||
}
|
||||
}
|
||||
|
||||
if (!was_running) {
|
||||
PX4_WARN("not running");
|
||||
}
|
||||
}
|
||||
|
||||
PWMOut::unlock_module();
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
PWMOut::lock_module(); // Lock here, as the method could access _object.
|
||||
int ret = PWMOut::custom_command(argc - 1, argv + 1);
|
||||
PWMOut::unlock_module();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2021 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
|
||||
|
|
@ -93,7 +93,10 @@ enum PortMode {
|
|||
// TODO: keep in sync with drivers/camera_capture
|
||||
#define PX4FMU_DEVICE_PATH "/dev/px4fmu"
|
||||
|
||||
class PWMOut : public cdev::CDev, public ModuleBase<PWMOut>, public OutputModuleInterface
|
||||
static constexpr int PWM_OUT_MAX_INSTANCES{(DIRECT_PWM_OUTPUT_CHANNELS > 8) ? 2 : 1};
|
||||
extern pthread_mutex_t pwm_out_module_mutex;
|
||||
|
||||
class PWMOut : public cdev::CDev, public OutputModuleInterface
|
||||
{
|
||||
public:
|
||||
enum Mode {
|
||||
|
|
@ -116,7 +119,10 @@ public:
|
|||
MODE_5CAP,
|
||||
MODE_6CAP,
|
||||
};
|
||||
PWMOut();
|
||||
|
||||
PWMOut() = delete;
|
||||
explicit PWMOut(int instance = 0, uint8_t output_base = 0);
|
||||
|
||||
virtual ~PWMOut();
|
||||
|
||||
/** @see ModuleBase */
|
||||
|
|
@ -131,7 +137,14 @@ public:
|
|||
void Run() override;
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
int print_status();
|
||||
|
||||
bool should_exit() const { return _task_should_exit.load(); }
|
||||
void request_stop() { _task_should_exit.store(true); }
|
||||
|
||||
static void lock_module() { pthread_mutex_lock(&pwm_out_module_mutex); }
|
||||
static bool trylock_module() { return (pthread_mutex_trylock(&pwm_out_module_mutex) == 0); }
|
||||
static void unlock_module() { pthread_mutex_unlock(&pwm_out_module_mutex); }
|
||||
|
||||
/** change the FMU mode of the running module */
|
||||
static int fmu_new_mode(PortMode new_mode);
|
||||
|
|
@ -158,6 +171,11 @@ private:
|
|||
static constexpr int FMU_MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
|
||||
static_assert(FMU_MAX_ACTUATORS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails");
|
||||
|
||||
px4::atomic_bool _task_should_exit{false};
|
||||
|
||||
const int _instance;
|
||||
const uint32_t _output_base;
|
||||
|
||||
MixingOutput _mixing_output{FMU_MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, true};
|
||||
|
||||
Mode _mode{MODE_NONE};
|
||||
|
|
|
|||
|
|
@ -178,6 +178,8 @@ public:
|
|||
|
||||
void setIgnoreLockdown(bool ignore_lockdown) { _ignore_lockdown = ignore_lockdown; }
|
||||
|
||||
void setMaxNumOutputs(uint8_t max_num_outputs) { _max_num_outputs = max_num_outputs; }
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
|
||||
|
|
@ -261,7 +263,7 @@ private:
|
|||
|
||||
bool _wq_switched{false};
|
||||
uint8_t _driver_instance{0}; ///< for boards that supports multiple outputs (e.g. PX4IO + FMU)
|
||||
const uint8_t _max_num_outputs;
|
||||
uint8_t _max_num_outputs;
|
||||
|
||||
struct MotorTest {
|
||||
uORB::Subscription test_motor_sub{ORB_ID(test_motor)};
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ px4_add_module(
|
|||
MAIN pwm
|
||||
COMPILE_FLAGS
|
||||
-Wno-array-bounds
|
||||
-O0
|
||||
SRCS
|
||||
pwm.cpp
|
||||
DEPENDS
|
||||
|
|
|
|||
Loading…
Reference in New Issue