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

systemlib mixer: Add public method for multi_rotor saturation status

This commit is contained in:
Paul Riseborough 2016-11-24 16:03:03 +11:00 committed by Lorenz Meier
parent eb67686b11
commit 8b55c8ec55
5 changed files with 49 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -169,6 +169,12 @@ NullMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
return 0;
}
uint16_t
NullMixer::get_saturation_status()
{
return 0;
}
void
NullMixer::groups_required(uint32_t &groups)
{

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

@ -176,6 +176,13 @@ public:
*/
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg) = 0;
/**
* Get the saturation status.
*
* @return Integer bitmask containing saturation_status from multirotor_motor_limits.msg .
*/
virtual uint16_t get_saturation_status(void) = 0;
/**
* Analyses the mix configuration and updates a bitmask of groups
* that are required.
@ -261,6 +268,7 @@ public:
~MixerGroup();
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
/**
@ -394,6 +402,7 @@ public:
static NullMixer *from_text(const char *buf, unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
virtual void set_offset(float trim) {};
unsigned set_trim(float trim)
@ -465,6 +474,7 @@ public:
uint16_t max);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
/**
@ -571,6 +581,7 @@ public:
unsigned &buflen);
virtual unsigned mix(float *outputs, unsigned space, uint16_t *status_reg);
virtual uint16_t get_saturation_status(void);
virtual void groups_required(uint32_t &groups);
/**
@ -602,19 +613,19 @@ private:
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;
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);

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

@ -141,6 +141,13 @@ MixerGroup::set_trims(int16_t *values, unsigned n)
return index;
}
uint16_t
MixerGroup::get_saturation_status()
{
Mixer *mixer = _first;
return mixer->get_saturation_status();
}
unsigned
MixerGroup::count()
{

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

@ -312,6 +312,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
if (min_out < 0.0f) {
_saturation_status.flags.motor_neg = true;
}
if (max_out > 1.0f) {
_saturation_status.flags.motor_pos = true;
}
@ -334,6 +335,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
}
} else if (out > 1.0f) {
// allow to reduce thrust to get some yaw response
float thrust_reduction = fminf(0.15f, out - 1.0f);
@ -506,3 +508,7 @@ MultirotorMixer::groups_required(uint32_t &groups)
groups |= (1 << 0);
}
uint16_t MultirotorMixer::get_saturation_status()
{
return _saturation_status.value;
}

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

@ -308,6 +308,12 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
return 1;
}
uint16_t
SimpleMixer::get_saturation_status()
{
return 0;
}
void
SimpleMixer::groups_required(uint32_t &groups)
{