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

multirotor_motor_limits only publish for MC

This commit is contained in:
Daniel Agar 2017-09-09 12:25:11 -04:00 committed by Nuno Marques
parent 9cacd3f994
commit 26f00609ac
23 changed files with 192 additions and 352 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
msg/multirotor_motor_limits.msg
View File

@ -1,11 +1,13 @@
uint16 saturation_status # Integer bit mask indicating which axes in the control mixer are saturated
# 0 - True if any motor is saturated at the upper limit
# 1 - True if any motor is saturated at the lower limit
# 2 - True if a positive roll increment will increase motor saturation
# 3 - True if negative roll increment will increase motor saturation
# 4 - True if positive pitch increment will increase motor saturation
# 5 - True if negative pitch increment will increase motor saturation
# 6 - True if positive yaw increment will increase motor saturation
# 7 - True if negative yaw increment will increase motor saturation
# 8 - True if positive thrust increment will increase motor saturation
# 9 - True if negative thrust increment will increase motor saturation
# 0 - True if the saturation status is valid
# 1 - True if any motor is saturated at the upper limit
# 2 - True if any motor is saturated at the lower limit
# 3 - True if a positive roll increment will increase motor saturation
# 4 - True if negative roll increment will increase motor saturation
# 5 - True if positive pitch increment will increase motor saturation
# 6 - True if negative pitch increment will increase motor saturation
# 7 - True if positive yaw increment will increase motor saturation
# 8 - True if negative yaw increment will increase motor saturation
# 9 - True if positive thrust increment will increase motor saturation
# 10 - True if negative thrust increment will increase motor saturation

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

@ -299,9 +299,7 @@ void task_main(int argc, char *argv[])
if (_mixer_group != nullptr) {
_outputs.timestamp = hrt_absolute_time();
/* do mixing */
_outputs.noutputs = _mixer_group->mix(_outputs.output,
actuator_outputs_s::NUM_ACTUATOR_OUTPUTS,
NULL);
_outputs.noutputs = _mixer_group->mix(_outputs.output, actuator_outputs_s::NUM_ACTUATOR_OUTPUTS);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; i < _outputs.NUM_ACTUATOR_OUTPUTS; i++) {

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

@ -539,7 +539,7 @@ MK::task_main()
if (_mixers != nullptr) {
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs, NULL);
outputs.noutputs = _mixers->mix(&outputs.output[0], _num_outputs);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */

3
src/drivers/pwm_out_rc_in/pwm_out_rc_in.cpp
View File

@ -55,7 +55,6 @@
#include <v1.0/mavlink_types.h>
#include <v1.0/common/mavlink.h>
/*
* This driver is supposed to run on Snapdragon. It sends actuator_controls (PWM)
* to a Pixhawk/Pixfalcon/Pixracer over UART (mavlink) and receives RC input.
@ -415,7 +414,7 @@ void task_main(int argc, char *argv[])
_outputs.timestamp = _controls.timestamp;
/* do mixing */
_outputs.noutputs = _mixer->mix(_outputs.output, 0 /* not used */, NULL);
_outputs.noutputs = _mixer->mix(_outputs.output, 0);
/* disable unused ports by setting their output to NaN */
for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]); i++) {

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

@ -474,7 +474,7 @@ PWMSim::task_main()
}
/* do mixing */
num_outputs = _mixers->mix(&outputs.output[0], num_outputs, nullptr);
num_outputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.noutputs = num_outputs;
outputs.timestamp = hrt_absolute_time();

285
src/drivers/px4fmu/fmu.cpp
View File

@ -34,80 +34,58 @@
/**
* @file fmu.cpp
*
* Driver/configurator for the PX4 FMU multi-purpose port on v1 and v2 boards.
* Driver/configurator for the PX4 FMU
*/
#include <px4_config.h>
#include <px4_log.h>
#include <px4_module.h>
#include <px4_getopt.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <float.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <nuttx/arch.h>
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_input_capture.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <cfloat>
#include <board_config.h>
#include <systemlib/px4_macros.h>
#include <systemlib/systemlib.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <systemlib/board_serial.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_gpio.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_input_capture.h>
#include <lib/rc/sbus.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_rc_input.h>
#include <lib/rc/dsm.h>
#include <lib/rc/sbus.h>
#include <lib/rc/st24.h>
#include <lib/rc/sumd.h>
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_log.h>
#include <px4_module.h>
#include <systemlib/board_serial.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/pwm_limit/pwm_limit.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/adc_report.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#ifdef HRT_PPM_CHANNEL
# include <systemlib/ppm_decode.h>
#endif
#include <systemlib/circuit_breaker.h>
#define SCHEDULE_INTERVAL 2000 /**< The schedule interval in usec (500 Hz) */
#define NAN_VALUE (0.0f/0.0f) /**< NaN value for throttle lock mode */
#define BUTTON_SAFETY px4_arch_gpioread(GPIO_BTN_SAFETY)
#define CYCLE_COUNT 10 /* safety switch must be held for 1 second to activate */
static constexpr uint8_t CYCLE_COUNT = 10; /* safety switch must be held for 1 second to activate */
static constexpr uint8_t MAX_ACTUATORS = DIRECT_PWM_OUTPUT_CHANNELS;
/*
* Define the various LED flash sequences for each system state.
*/
#define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */
#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
#define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */
#define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */
#define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */
@ -127,10 +105,10 @@ enum PortMode {
PORT_CAPTURE,
};
#if !defined(BOARD_HAS_PWM)
# error "board_config.h needs to define BOARD_HAS_PWM"
#endif
class PX4FMU : public device::CDev, public ModuleBase<PX4FMU>
{
public:
@ -305,9 +283,8 @@ private:
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
void update_pwm_rev_mask();
void publish_pwm_outputs(uint16_t *values, size_t numvalues);
void update_pwm_out_state(bool on);
void pwm_output_set(unsigned i, unsigned value);
void update_params();
struct GPIOConfig {
@ -550,7 +527,7 @@ PX4FMU::safety_check_button(void)
* Debounce the safety button, change state if it has been held for long enough.
*
*/
bool safety_button_pressed = BUTTON_SAFETY;
bool safety_button_pressed = px4_arch_gpioread(GPIO_BTN_SAFETY);
/*
* Keep pressed for a while to arm.
@ -951,26 +928,6 @@ PX4FMU::update_pwm_trims()
}
}
void
PX4FMU::publish_pwm_outputs(uint16_t *values, size_t numvalues)
{
actuator_outputs_s outputs = {};
outputs.noutputs = numvalues;
outputs.timestamp = hrt_absolute_time();
for (size_t i = 0; i < _max_actuators; ++i) {
outputs.output[i] = i < numvalues ? (float)values[i] : 0;
}
if (_outputs_pub == nullptr) {
int instance = _class_instance;
_outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &instance, ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs);
}
}
int
PX4FMU::task_spawn(int argc, char *argv[])
{
@ -1006,7 +963,6 @@ PX4FMU::task_spawn(int argc, char *argv[])
if (!run_as_task) {
/* schedule a cycle to start things */
int ret = work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, nullptr, 0);
if (ret < 0) {
@ -1161,14 +1117,6 @@ void PX4FMU::rc_io_invert(bool invert)
}
#endif
void
PX4FMU::pwm_output_set(unsigned i, unsigned value)
{
if (_pwm_initialized) {
up_pwm_servo_set(i, value);
}
}
void
PX4FMU::update_pwm_out_state(bool on)
{
@ -1248,7 +1196,7 @@ PX4FMU::cycle()
/* wait for an update */
unsigned n_updates = 0;
int ret = ::poll(_poll_fds, _poll_fds_num, poll_timeout);
int ret = px4_poll(_poll_fds, _poll_fds_num, poll_timeout);
/* this would be bad... */
if (ret < 0) {
@ -1261,81 +1209,59 @@ PX4FMU::cycle()
} else {
perf_begin(_ctl_latency);
/* get controls for required topics */
unsigned poll_id = 0;
if (_mixers != nullptr) {
/* get controls for required topics */
unsigned poll_id = 0;
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
if (_poll_fds[poll_id].revents & POLLIN) {
if (i == 0) {
n_updates++;
}
for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
if (_control_subs[i] > 0) {
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
#if defined(DEBUG_BUILD)
static int main_out_latency = 0;
static int sum_latency = 0;
static uint64_t last_cycle_time = 0;
if (i == 0) {
uint64_t now = hrt_absolute_time();
uint64_t latency = now - _controls[i].timestamp;
if (latency > main_out_latency) { main_out_latency = latency; }
sum_latency += latency;
if ((now - last_cycle_time) >= 1000000) {
last_cycle_time = now;
PX4_DEBUG("pwm max latency: %d, avg: %5.3f", main_out_latency, (double)(sum_latency / 100.0));
main_out_latency = latency;
sum_latency = 0;
if (_poll_fds[poll_id].revents & POLLIN) {
if (i == 0) {
n_updates++;
}
orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
}
#endif
poll_id++;
}
poll_id++;
}
/* During ESC calibration, we overwrite the throttle value. */
if (i == 0 && _armed.in_esc_calibration_mode) {
/* During ESC calibration, we overwrite the throttle value. */
if (i == 0 && _armed.in_esc_calibration_mode) {
/* Set all controls to 0 */
memset(&_controls[i], 0, sizeof(_controls[i]));
/* Set all controls to 0 */
memset(&_controls[i], 0, sizeof(_controls[i]));
/* except thrust to maximum. */
_controls[i].control[actuator_controls_s::INDEX_THROTTLE] = 1.0f;
/* except thrust to maximum. */
_controls[i].control[3] = 1.0f;
/* Switch off the PWM limit ramp for the calibration. */
_pwm_limit.state = PWM_LIMIT_STATE_ON;
/* Switch off the PWM limit ramp for the calibration. */
_pwm_limit.state = PWM_LIMIT_STATE_ON;
}
}
}
} // poll_fds
/* run the mixers on every cycle */
{
/* can we mix? */
if (_mixers != nullptr) {
hrt_abstime now = hrt_absolute_time();
float dt = (now - _time_last_mix) / 1e6f;
_time_last_mix = now;
if (dt < 0.0001f) {
dt = 0.0001f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
if (_mot_t_max > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1]
float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
hrt_abstime now = hrt_absolute_time();
float dt = (now - _time_last_mix) / 1e6f;
_time_last_mix = now;
if (dt < 0.0001f) {
dt = 0.0001f;
} else if (dt > 0.02f) {
dt = 0.02f;
}
// maximum value the outputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator outputs are in the range [-1,1]
const float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
_mixers->set_max_delta_out_once(delta_out_max);
}
@ -1345,37 +1271,14 @@ PX4FMU::cycle()
/* do mixing */
float outputs[_max_actuators];
size_t mixed_num_outputs = _mixers->mix(outputs, _num_outputs, NULL);
/* publish mixer status */
multirotor_motor_limits_s multirotor_motor_limits = {};
multirotor_motor_limits.saturation_status = _mixers->get_saturation_status();
if (_to_mixer_status == nullptr) {
/* publish limits */
int instance = _class_instance;
_to_mixer_status = orb_advertise_multi(ORB_ID(multirotor_motor_limits), &multirotor_motor_limits, &instance,
ORB_PRIO_DEFAULT);
} else {
orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &multirotor_motor_limits);
}
/* disable unused ports by setting their output to NaN */
for (size_t i = 0; i < sizeof(outputs) / sizeof(outputs[0]); i++) {
if (i >= mixed_num_outputs) {
outputs[i] = NAN_VALUE;
}
}
uint16_t pwm_limited[_max_actuators];
const unsigned mixed_num_outputs = _mixers->mix(outputs, _num_outputs);
/* the PWM limit call takes care of out of band errors, NaN and constrains */
uint16_t pwm_limited[MAX_ACTUATORS];
pwm_limit_calc(_throttle_armed, arm_nothrottle(), mixed_num_outputs, _reverse_pwm_mask,
_disarmed_pwm, _min_pwm, _max_pwm, outputs, pwm_limited, &_pwm_limit);
/* overwrite outputs in case of force_failsafe with _failsafe_pwm PWM values */
if (_armed.force_failsafe) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
@ -1391,19 +1294,43 @@ PX4FMU::cycle()
}
/* output to the servos */
for (size_t i = 0; i < mixed_num_outputs; i++) {
pwm_output_set(i, pwm_limited[i]);
if (_pwm_initialized) {
for (size_t i = 0; i < mixed_num_outputs; i++) {
up_pwm_servo_set(i, pwm_limited[i]);
}
}
/* Trigger all timer's channels in Oneshot mode to fire
* the oneshots with updated values.
*/
if (n_updates > 0) {
up_pwm_update();
}
publish_pwm_outputs(pwm_limited, mixed_num_outputs);
actuator_outputs_s actuator_outputs = {};
actuator_outputs.timestamp = hrt_absolute_time();
actuator_outputs.noutputs = mixed_num_outputs;
// zero unused outputs
for (size_t i = 0; i < mixed_num_outputs; ++i) {
actuator_outputs.output[i] = pwm_limited[i];
}
orb_publish_auto(ORB_ID(actuator_outputs), &_outputs_pub, &actuator_outputs, &_class_instance, ORB_PRIO_DEFAULT);
/* publish mixer status */
MultirotorMixer::saturation_status saturation_status;
saturation_status.value = _mixers->get_saturation_status();
if (saturation_status.flags.valid) {
multirotor_motor_limits_s motor_limits;
motor_limits.timestamp = hrt_absolute_time();
motor_limits.saturation_status = saturation_status.value;
orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &_class_instance,
ORB_PRIO_DEFAULT);
}
perf_end(_ctl_latency);
}
}
@ -1729,8 +1656,7 @@ PX4FMU::cycle()
// we have a new PPM frame. Publish it.
rc_updated = true;
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp,
false, false, 0);
fill_rc_in(ppm_decoded_channels, ppm_buffer, _cycle_timestamp, false, false, 0);
_rc_scan_locked = true;
_rc_in.rc_ppm_frame_length = ppm_frame_length;
_rc_in.timestamp_last_signal = ppm_last_valid_decode;
@ -1790,10 +1716,8 @@ PX4FMU::cycle()
exit_and_cleanup();
} else {
/* schedule next cycle */
work_queue(HPWORK, &_work, (worker_t)&PX4FMU::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
// USEC2TICK(SCHEDULE_INTERVAL - main_out_latency));
}
break;
@ -1863,7 +1787,7 @@ PX4FMU::control_callback(uintptr_t handle,
control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE_ALTERNATE) &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
/* set the throttle to an invalid value */
input = NAN_VALUE;
input = NAN;
}
}
@ -2508,8 +2432,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)_controls, mixinfo);
SimpleMixer *mixer = new SimpleMixer(control_callback, (uintptr_t)_controls, mixinfo);
if (mixer->check()) {
delete mixer;
@ -2517,9 +2440,9 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
ret = -EINVAL;
} else {
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)_controls);
if (_mixers == nullptr) {
_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
}
_mixers->add_mixer(mixer);
_mixers->groups_required(_groups_required);

1
src/drivers/px4io/CMakeLists.txt
View File

@ -40,7 +40,6 @@ px4_add_module(
px4io_uploader.cpp
px4io_serial.cpp
px4io_i2c.cpp
px4io_params.c
DEPENDS
platforms__common
)

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

@ -1928,12 +1928,6 @@ PX4IO::io_publish_raw_rc()
int
PX4IO::io_publish_pwm_outputs()
{
/* data we are going to fetch */
actuator_outputs_s outputs = {};
multirotor_motor_limits_s motor_limits;
outputs.timestamp = hrt_absolute_time();
/* get servo values from IO */
uint16_t ctl[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
@ -1942,38 +1936,33 @@ PX4IO::io_publish_pwm_outputs()
return ret;
}
actuator_outputs_s outputs = {};
outputs.timestamp = hrt_absolute_time();
outputs.noutputs = _max_actuators;
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++) {
outputs.output[i] = ctl[i];
}
outputs.noutputs = _max_actuators;
/* lazily advertise on first publication */
if (_to_outputs == nullptr) {
int instance;
_to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs),
&outputs, &instance, ORB_PRIO_MAX);
} else {
orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
}
int instance;
orb_publish_auto(ORB_ID(actuator_outputs), &_to_outputs, &outputs, &instance, ORB_PRIO_DEFAULT);
/* get mixer status flags from IO */
uint16_t mixer_status;
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &mixer_status, sizeof(mixer_status) / sizeof(uint16_t));
motor_limits.saturation_status = mixer_status;
MultirotorMixer::saturation_status saturation_status;
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_MIXER, &saturation_status.value, 1);
if (ret != OK) {
return ret;
}
/* publish mixer status */
if (_to_mixer_status == nullptr) {
_to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits);
if (saturation_status.flags.valid) {
multirotor_motor_limits_s motor_limits;
motor_limits.timestamp = hrt_absolute_time();
motor_limits.saturation_status = saturation_status.value;
} else {
orb_publish(ORB_ID(multirotor_motor_limits), _to_mixer_status, &motor_limits);
orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT);
}
return OK;

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

@ -402,9 +402,7 @@ void task_main(int argc, char *argv[])
_outputs.timestamp = hrt_absolute_time();
/* do mixing for virtual control group */
_outputs.noutputs = _mixer->mix(_outputs.output,
_outputs.NUM_ACTUATOR_OUTPUTS,
NULL);
_outputs.noutputs = _mixer->mix(_outputs.output, _outputs.NUM_ACTUATOR_OUTPUTS);
//set max, min and disarmed pwm
const uint16_t reverse_mask = 0;

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

@ -655,7 +655,7 @@ TAP_ESC::cycle()
if (_is_armed && _mixers != nullptr) {
/* do mixing */
num_outputs = _mixers->mix(&_outputs.output[0], num_outputs, NULL);
num_outputs = _mixers->mix(&_outputs.output[0], num_outputs);
_outputs.noutputs = num_outputs;
_outputs.timestamp = hrt_absolute_time();

27
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -64,6 +64,7 @@
#include <px4_tasks.h>
#include <systemlib/circuit_breaker.h>
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <systemlib/param/param.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
@ -161,27 +162,12 @@ private:
struct actuator_controls_s _actuators; /**< actuator controls */
struct actuator_armed_s _armed; /**< actuator arming status */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct multirotor_motor_limits_s _motor_limits; /**< motor limits */
struct mc_att_ctrl_status_s _controller_status; /**< controller status */
struct battery_status_s _battery_status; /**< battery status */
struct sensor_gyro_s _sensor_gyro; /**< gyro data before thermal correctons and ekf bias estimates are applied */
struct sensor_correction_s _sensor_correction; /**< sensor thermal corrections */
union {
struct {
uint16_t motor_pos : 1; // 0 - true when any motor has saturated in the positive direction
uint16_t motor_neg : 1; // 1 - true when any motor has saturated in the negative direction
uint16_t roll_pos : 1; // 2 - true when a positive roll demand change will increase saturation
uint16_t roll_neg : 1; // 3 - true when a negative roll demand change will increase saturation
uint16_t pitch_pos : 1; // 4 - true when a positive pitch demand change will increase saturation
uint16_t pitch_neg : 1; // 5 - true when a negative pitch demand change will increase saturation
uint16_t yaw_pos : 1; // 6 - true when a positive yaw demand change will increase saturation
uint16_t yaw_neg : 1; // 7 - true when a negative yaw demand change will increase saturation
uint16_t thrust_pos : 1; // 8 - true when a positive thrust demand change will increase saturation
uint16_t thrust_neg : 1; // 9 - true when a negative thrust demand change will increase saturation
} flags;
uint16_t value;
} _saturation_status;
MultirotorMixer::saturation_status _saturation_status{};
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _controller_latency_perf;
@ -415,13 +401,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_actuators{},
_armed{},
_vehicle_status{},
_motor_limits{},
_controller_status{},
_battery_status{},
_sensor_gyro{},
_sensor_correction{},
_saturation_status{},
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_controller_latency_perf(perf_alloc_once(PC_ELAPSED, "ctrl_latency")),
@ -784,8 +767,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll()
orb_check(_motor_limits_sub, &updated);
if (updated) {
orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &_motor_limits);
_saturation_status.value = _motor_limits.saturation_status;
multirotor_motor_limits_s motor_limits = {};
orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits);
_saturation_status.value = motor_limits.saturation_status;
}
}

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

@ -91,11 +91,7 @@ enum mixer_source {
static volatile mixer_source source;
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &control);
static int mixer_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &control);
static int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits);
static MixerGroup mixer_group(mixer_callback, 0);
@ -107,10 +103,9 @@ int mixer_mix_threadsafe(float *outputs, volatile uint16_t *limits)
return 0;
}
uint16_t mixer_limits = 0;
in_mixer = true;
int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &mixer_limits);
*limits = mixer_limits;
int mixcount = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
*limits = mixer_group.get_saturation_status();
in_mixer = false;
return mixcount;
@ -600,7 +595,6 @@ mixer_set_failsafe()
}
/* mix */
mixed = mixer_mix_threadsafe(&outputs[0], &r_mixer_limits);
/* scale to PWM and update the servo outputs as required */

2
src/modules/systemlib/mixer/mixer.cpp
View File

@ -184,7 +184,7 @@ NullMixer::NullMixer() :
}
unsigned
NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
NullMixer::mix(float *outputs, unsigned space)
{
if (space > 0) {
*outputs = 0.0f;

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

@ -131,8 +131,6 @@
#include <px4_config.h>
#include "drivers/drv_mixer.h"
#include <uORB/topics/multirotor_motor_limits.h>
#include "mixer_load.h"
/**
@ -174,12 +172,12 @@ public:
* @param space The number of available entries in the output array;
* @return The number of entries in the output array that were populated.
*/
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg) = 0;
virtual unsigned mix(float *outputs, unsigned space) = 0;
/**
* Get the saturation status.
*
* @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg .
* @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg.
*/
virtual uint16_t get_saturation_status(void) = 0;
@ -284,7 +282,7 @@ public:
MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
~MixerGroup();
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual unsigned mix(float *outputs, unsigned space);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
@ -425,7 +423,7 @@ public:
*/
static NullMixer *from_text(const char *buf, unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual unsigned mix(float *outputs, unsigned space);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
virtual void set_offset(float trim) {}
@ -490,14 +488,10 @@ public:
* @return A new SimpleMixer instance, or nullptr if one could not be
* allocated.
*/
static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
unsigned input,
uint16_t min,
uint16_t mid,
uint16_t max);
static SimpleMixer *pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
uint16_t mid, uint16_t max);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual unsigned mix(float *outputs, unsigned space);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
@ -599,12 +593,10 @@ public:
* @return A new MultirotorMixer instance, or nullptr
* if the text format is bad.
*/
static MultirotorMixer *from_text(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
const char *buf,
static MultirotorMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual unsigned mix(float *outputs, unsigned space);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
@ -618,7 +610,7 @@ public:
* @param[in] delta_out_max Maximum delta output.
*
*/
virtual void set_max_delta_out_once(float delta_out_max) {_delta_out_max = delta_out_max;}
virtual void set_max_delta_out_once(float delta_out_max) { _delta_out_max = delta_out_max; }
unsigned set_trim(float trim)
{
@ -632,6 +624,23 @@ public:
*/
virtual void set_thrust_factor(float val) {_thrust_factor = val;}
union saturation_status {
struct {
uint16_t valid : 1; // 0 - true when the saturation status is used
uint16_t motor_pos : 1; // 1 - true when any motor has saturated in the positive direction
uint16_t motor_neg : 1; // 2 - true when any motor has saturated in the negative direction
uint16_t roll_pos : 1; // 3 - true when a positive roll demand change will increase saturation
uint16_t roll_neg : 1; // 4 - true when a negative roll demand change will increase saturation
uint16_t pitch_pos : 1; // 5 - true when a positive pitch demand change will increase saturation
uint16_t pitch_neg : 1; // 6 - true when a negative pitch demand change will increase saturation
uint16_t yaw_pos : 1; // 7 - true when a positive yaw demand change will increase saturation
uint16_t yaw_neg : 1; // 8 - true when a negative yaw demand change will increase saturation
uint16_t thrust_pos : 1; // 9 - true when a positive thrust demand change will increase saturation
uint16_t thrust_neg : 1; //10 - true when a negative thrust demand change will increase saturation
} flags;
uint16_t value;
};
private:
float _roll_scale;
float _pitch_scale;
@ -640,27 +649,8 @@ private:
float _delta_out_max;
float _thrust_factor;
orb_advert_t _limits_pub;
multirotor_motor_limits_s _limits;
union {
struct {
uint16_t motor_pos : 1; // 0 - true when any motor has saturated in the positive direction
uint16_t motor_neg : 1; // 1 - true when any motor has saturated in the negative direction
uint16_t roll_pos : 1; // 2 - true when a positive roll demand change will increase saturation
uint16_t roll_neg : 1; // 3 - true when a negative roll demand change will increase saturation
uint16_t pitch_pos : 1; // 4 - true when a positive pitch demand change will increase saturation
uint16_t pitch_neg : 1; // 5 - true when a negative pitch demand change will increase saturation
uint16_t yaw_pos : 1; // 6 - true when a positive yaw demand change will increase saturation
uint16_t yaw_neg : 1; // 7 - true when a negative yaw demand change will increase saturation
uint16_t thrust_pos : 1; // 8 - true when a positive thrust demand change will increase saturation
uint16_t thrust_neg : 1; // 9 - true when a negative thrust demand change will increase saturation
} flags;
uint16_t value;
} _saturation_status;
void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);
saturation_status _saturation_status;
unsigned _rotor_count;
const Rotor *_rotors;
@ -729,15 +719,14 @@ public:
* @return A new HelicopterMixer instance, or nullptr
* if the text format is bad.
*/
static HelicopterMixer *from_text(Mixer::ControlCallback control_cb,
uintptr_t cb_handle,
const char *buf,
static HelicopterMixer *from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, const char *buf,
unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual unsigned mix(float *outputs, unsigned space);
virtual void groups_required(uint32_t &groups);
virtual uint16_t get_saturation_status(void) { return 0; }
unsigned set_trim(float trim)
{
return 4;

4
src/modules/systemlib/mixer/mixer_group.cpp
View File

@ -103,13 +103,13 @@ MixerGroup::reset()
}
unsigned
MixerGroup::mix(float *outputs, unsigned space, uint16_t *status_reg)
MixerGroup::mix(float *outputs, unsigned space)
{
Mixer *mixer = _first;
unsigned index = 0;
while ((mixer != nullptr) && (index < space)) {
index += mixer->mix(outputs + index, space - index, status_reg);
index += mixer->mix(outputs + index, space - index);
mixer = mixer->_next;
}

2
src/modules/systemlib/mixer/mixer_helicopter.cpp
View File

@ -234,7 +234,7 @@ HelicopterMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
}
unsigned
HelicopterMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
HelicopterMixer::mix(float *outputs, unsigned space)
{
/* Find index to use for curves */
float thrust_cmd = get_control(0, 3);

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

@ -83,7 +83,6 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
_idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */
_delta_out_max(0.0f),
_thrust_factor(0.0f),
_limits_pub(),
_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
_outputs_prev(new float[_rotor_count])
@ -178,13 +177,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} else if (!strcmp(geomname, "6a")) {
geometry = MultirotorGeometry::DODECA_BOTTOM_COX;
#if 0
} else if (!strcmp(geomname, "8cw")) {
geometry = MultirotorGeometry::OCTA_COX_WIDE;
#endif
} else if (!strcmp(geomname, "2-")) {
geometry = MultirotorGeometry::TWIN_ENGINE;
@ -209,7 +201,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
MultirotorMixer::mix(float *outputs, unsigned space)
{
/* Summary of mixing strategy:
1) mix roll, pitch and thrust without yaw.
@ -404,22 +396,16 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
// update the saturation status report
update_saturation_status(i, clipping_high, clipping_low);
}
// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
_delta_out_max = 0.0f;
// Notify saturation status
if (status_reg != nullptr) {
(*status_reg) = _saturation_status.value;
}
return _rotor_count;
}
/*
* This function update the control saturation status report using hte following inputs:
* This function update the control saturation status report using the following inputs:
*
* index: 0 based index identifying the motor that is saturating
* clipping_high: true if the motor demand is being limited in the positive direction
@ -438,7 +424,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
} else if (_rotors[index].roll_scale < 0.0f) {
// A negative change in roll will increase saturation
_saturation_status.flags.roll_neg = true;
}
// check if the pitch input is saturating
@ -449,7 +434,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
} else if (_rotors[index].pitch_scale < 0.0f) {
// A negative change in pitch will increase saturation
_saturation_status.flags.pitch_neg = true;
}
// check if the yaw input is saturating
@ -460,7 +444,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
} else if (_rotors[index].yaw_scale < 0.0f) {
// A negative change in yaw will increase saturation
_saturation_status.flags.yaw_neg = true;
}
// A positive change in thrust will increase saturation
@ -479,7 +462,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
} else if (_rotors[index].roll_scale < 0.0f) {
// A positive change in roll will increase saturation
_saturation_status.flags.roll_pos = true;
}
// check if the pitch input is saturating
@ -490,7 +472,6 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
} else if (_rotors[index].pitch_scale < 0.0f) {
// A positive change in pitch will increase saturation
_saturation_status.flags.pitch_pos = true;
}
// check if the yaw input is saturating
@ -501,13 +482,13 @@ MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bo
} else if (_rotors[index].yaw_scale < 0.0f) {
// A positive change in yaw will increase saturation
_saturation_status.flags.yaw_pos = true;
}
// A negative change in thrust will increase saturation
_saturation_status.flags.thrust_neg = true;
}
_saturation_status.flags.valid = true;
}
void

2
src/modules/systemlib/mixer/mixer_simple.cpp
View File

@ -286,7 +286,7 @@ out:
}
unsigned
SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;

13
src/modules/systemlib/system_params.c
View File

@ -77,19 +77,6 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
*/
PARAM_DEFINE_INT32(SYS_HITL, 0);
/**
* Set usage of IO board
*
* Can be used to use a standard startup script but with a FMU only set-up. Set to 0 to force the FMU only set-up.
*
* @boolean
* @min 0
* @max 1
* @reboot_required true
* @group System
*/
PARAM_DEFINE_INT32(SYS_USE_IO, 1);
/**
* Set restart type
*

2
src/modules/uavcan/uavcan_main.cpp
View File

@ -909,7 +909,7 @@ int UavcanNode::run()
unsigned num_outputs_max = 8;
// Do mixing
_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max, NULL);
_outputs.noutputs = _mixers->mix(&_outputs.output[0], num_outputs_max);
new_output = true;
}

4
src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp
View File

@ -377,9 +377,7 @@ void task_main(int argc, char *argv[])
if (_mixers != nullptr) {
/* do mixing */
_outputs.noutputs = _mixers->mix(_outputs.output,
4,
NULL);
_outputs.noutputs = _mixers->mix(_outputs.output, 4);
}
// Set last throttle for battery calculations

4
src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
View File

@ -337,9 +337,7 @@ void task_main(int argc, char *argv[])
int16_t motor_rpms[UART_ESC_MAX_MOTORS];
if (_armed.armed) {
_outputs.noutputs = mixer->mix(&_outputs.output[0],
actuator_controls_s::NUM_ACTUATOR_CONTROLS,
NULL);
_outputs.noutputs = mixer->mix(&_outputs.output[0], actuator_controls_s::NUM_ACTUATOR_CONTROLS);
// Make sure we support only up to UART_ESC_MAX_MOTORS motors
if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {

10
src/systemcmds/tests/test_mixer.cpp
View File

@ -395,7 +395,7 @@ bool MixerTest::mixerTest()
/* mix */
should_prearm = true;
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@ -436,7 +436,7 @@ bool MixerTest::mixerTest()
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US + 2 * sleep_quantum_us) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@ -480,7 +480,7 @@ bool MixerTest::mixerTest()
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@ -508,7 +508,7 @@ bool MixerTest::mixerTest()
while (hrt_elapsed_time(&starttime) < 600000) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
@ -545,7 +545,7 @@ bool MixerTest::mixerTest()
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max, nullptr);
mixed = mixer_group.mix(&outputs[0], output_max);
pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min,
r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);