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

fixed publication of mixer limit flags

This commit is contained in:
Roman Bapst 2015-04-10 17:34:38 +02:00 committed by tumbili
parent 2d985dbed3
commit 555e96a37a
16 changed files with 69 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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

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

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

@ -664,7 +664,7 @@ PX4FMU::task_main()
}
/* do mixing */
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.timestamp = hrt_absolute_time();
/* iterate actuators */

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

@ -88,6 +88,7 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <debug.h>
@ -288,6 +289,7 @@ private:
orb_advert_t _to_battery; ///< battery status / voltage
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
orb_advert_t _to_mixer_status; ///< mixer status flags
actuator_outputs_s _outputs; ///< mixed outputs
servorail_status_s _servorail_status; ///< servorail status
@ -513,6 +515,7 @@ PX4IO::PX4IO(device::Device *interface) :
_to_battery(0),
_to_servorail(0),
_to_safety(0),
_to_mixer_status(0),
_outputs{},
_servorail_status{},
_primary_pwm_device(false),
@ -1687,6 +1690,8 @@ 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 */
@ -1718,6 +1723,21 @@ PX4IO::io_publish_pwm_outputs()
orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs);
}
/* 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));
memcpy(&motor_limits,&mixer_status,sizeof(motor_limits));
if (ret != OK)
return ret;
/* publish mixer status */
if(_to_mixer_status == 0) {
_to_mixer_status = orb_advertise(ORB_ID(multirotor_motor_limits), &motor_limits);
} else {
orb_publish(ORB_ID(multirotor_motor_limits),_to_mixer_status, &motor_limits);
}
return OK;
}

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

@ -230,7 +230,7 @@ mixer_tick(void)
/* poor mans mutex */
in_mixer = true;
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
in_mixer = false;
/* the pwm limit call takes care of out of band errors */
@ -453,7 +453,7 @@ mixer_set_failsafe()
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {

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

@ -133,6 +133,11 @@
#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */
#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */
#define PX4IO_P_STATUS_MIXER 9 /* mixer actuator limit flags */
#define PX4IO_P_STATUS_MIXER_LOWER_LIMIT (1 << 0) /**< at least one actuator output has reached lower limit */
#define PX4IO_P_STATUS_MIXER_UPPER_LIMIT (1 << 1) /**< at least one actuator output has reached upper limit */
#define PX4IO_P_STATUS_MIXER_YAW_LIMIT (1 << 2) /**< yaw control is limited because it causes output clipping */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */

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

@ -98,8 +98,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS]
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
#define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE])
#define r_mixer_limits r_page_status[PX4IO_P_STATUS_MIXER]
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]

1
src/modules/px4iofirmware/registers.c
View File

@ -90,6 +90,7 @@ uint16_t r_page_status[] = {
[PX4IO_P_STATUS_VSERVO] = 0,
[PX4IO_P_STATUS_VRSSI] = 0,
[PX4IO_P_STATUS_PRSSI] = 0,
[PX4IO_P_STATUS_MIXER] = 0,
};
/**

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

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

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

@ -174,7 +174,7 @@ 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) = 0;
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg) = 0;
/**
* Analyses the mix configuration and updates a bitmask of groups
@ -250,7 +250,7 @@ public:
MixerGroup(ControlCallback control_cb, uintptr_t cb_handle);
~MixerGroup();
virtual unsigned mix(float *outputs, unsigned space);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual void groups_required(uint32_t &groups);
/**
@ -346,7 +346,7 @@ public:
*/
static NullMixer *from_text(const char *buf, unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual void groups_required(uint32_t &groups);
};
@ -411,7 +411,7 @@ public:
uint16_t mid,
uint16_t max);
virtual unsigned mix(float *outputs, unsigned space);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual void groups_required(uint32_t &groups);
/**
@ -515,7 +515,7 @@ public:
const char *buf,
unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual void groups_required(uint32_t &groups);
private:

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

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

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

@ -36,10 +36,7 @@
*
* Multi-rotor mixers.
*/
#include <uORB/uORB.h>
#include <uORB/topics/multirotor_motor_limits.h>
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
@ -53,6 +50,8 @@
#include <unistd.h>
#include <math.h>
#include <px4iofirmware/protocol.h>
#include "mixer.h"
// This file is generated by the multi_tables script which is invoked during the build process
@ -199,7 +198,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
}
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
{
float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
@ -210,10 +209,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float min_out = 0.0f;
float max_out = 0.0f;
_limits.roll_pitch = false;
_limits.yaw = false;
_limits.throttle_upper = false;
_limits.throttle_lower = false;
if (status_reg != NULL) {
(*status_reg) = 0;
}
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
for (unsigned i = 0; i < _rotor_count; i++) {
@ -226,7 +224,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* limit yaw if it causes outputs clipping */
if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
yaw = -out / _rotors[i].yaw_scale;
_limits.yaw = true;
if(status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
}
}
/* calculate min and max output values */
@ -257,7 +257,10 @@ MultirotorMixer::mix(float *outputs, unsigned space)
outputs[i] = out;
}
_limits.roll_pitch = true;
if(status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
}
} else {
/* roll/pitch mixed without lower side limiting, add yaw control */
@ -270,7 +273,10 @@ MultirotorMixer::mix(float *outputs, unsigned space)
float scale_out;
if (max_out > 1.0f) {
scale_out = 1.0f / max_out;
_limits.throttle_upper = true;
if(status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
}
} else {
scale_out = 1.0f;
@ -278,20 +284,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* scale outputs to range _idle_speed..1, and do final limiting */
for (unsigned i = 0; i < _rotor_count; i++) {
if (outputs[i] < _idle_speed) {
_limits.throttle_lower = true;
}
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
}
#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
/* publish/advertise motor limits if running on FMU */
if (_limits_pub > 0) {
orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits);
} else {
_limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits);
}
#endif
return _rotor_count;
}

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

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

9
src/modules/uORB/topics/multirotor_motor_limits.h
View File

@ -52,11 +52,10 @@
* Motor limits
*/
struct multirotor_motor_limits_s {
uint8_t roll_pitch : 1; // roll/pitch limit reached
uint8_t yaw : 1; // yaw limit reached
uint8_t throttle_lower : 1; // lower throttle limit reached
uint8_t throttle_upper : 1; // upper throttle limit reached
uint8_t reserved : 4;
uint8_t lower_limit : 1; // at least one actuator command has saturated on the lower limit
uint8_t upper_limit : 1; // at least one actuator command has saturated on the upper limit
uint8_t yaw : 1; // yaw limit reached
uint8_t reserved : 5; // reserved
};
/**

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

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

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

@ -202,7 +202,7 @@ int test_mixer(int argc, char *argv[])
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
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);
@ -248,7 +248,7 @@ int test_mixer(int argc, char *argv[])
}
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
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);
@ -275,7 +275,7 @@ int test_mixer(int argc, char *argv[])
while (hrt_elapsed_time(&starttime) < 600000) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
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);
@ -311,7 +311,7 @@ int test_mixer(int argc, char *argv[])
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
/* mix */
mixed = mixer_group.mix(&outputs[0], output_max);
mixed = mixer_group.mix(&outputs[0], output_max, NULL);
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);