fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 15:08:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_motor_limits only publish for MC

Browse Source
This commit is contained in:
Daniel Agar
2017-09-19 02:25:30 +01:00
committed by Nuno Marques
parent 9cacd3f994
commit 26f00609ac
23 changed files with 192 additions and 352 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/systemlib/mixer/mixer.cpp
+1 -1
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;
src/modules/systemlib/mixer/mixer.h
+31 -42
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;
src/modules/systemlib/mixer/mixer_group.cpp
+2 -2
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;
}
src/modules/systemlib/mixer/mixer_helicopter.cpp
+1 -1
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);
src/modules/systemlib/mixer/mixer_multirotor.cpp
+4 -23
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
src/modules/systemlib/mixer/mixer_simple.cpp
+1 -1
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;
src/modules/systemlib/system_params.c
-13
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
*