forked from Archive/PX4-Autopilot
Mixer library: Fix code style
This commit is contained in:
Lorenz Meier
parent
fd63ba7b89
commit
5f586fc354
|
|
@ -35,6 +35,8 @@
|
|||
* @file mixer.cpp
|
||||
*
|
||||
* Control channel input/output mixer and failsafe.
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
|
@ -64,6 +66,7 @@ extern "C" {
|
|||
/* current servo arm/disarm state */
|
||||
static bool mixer_servos_armed = false;
|
||||
static bool should_arm = false;
|
||||
static bool should_arm_nothrottle = false;
|
||||
static bool should_always_enable_pwm = false;
|
||||
static volatile bool in_mixer = false;
|
||||
|
||||
|
|
@ -172,6 +175,11 @@ mixer_tick(void)
|
|||
)
|
||||
);
|
||||
|
||||
should_arm_nothrottle = (
|
||||
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
|
||||
/* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
|
||||
/* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK));
|
||||
|
||||
should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
|
||||
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
|
||||
|
|
@ -237,24 +245,25 @@ mixer_tick(void)
|
|||
/* the pwm limit call takes care of out of band errors */
|
||||
pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
|
||||
/* clamp unused outputs to zero */
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servos[i] = 0;
|
||||
outputs[i] = 0;
|
||||
}
|
||||
|
||||
/* store normalized outputs */
|
||||
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
/* set arming */
|
||||
bool needs_to_arm = (should_arm || should_always_enable_pwm);
|
||||
bool needs_to_arm = (should_arm || should_arm_nothrottle || should_always_enable_pwm);
|
||||
|
||||
/* check any conditions that prevent arming */
|
||||
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
|
||||
needs_to_arm = false;
|
||||
}
|
||||
if (!should_arm && !should_always_enable_pwm) {
|
||||
needs_to_arm = false;
|
||||
}
|
||||
|
||||
if (needs_to_arm && !mixer_servos_armed) {
|
||||
/* need to arm, but not armed */
|
||||
|
|
@ -308,8 +317,9 @@ mixer_callback(uintptr_t handle,
|
|||
uint8_t control_index,
|
||||
float &control)
|
||||
{
|
||||
if (control_group >= PX4IO_CONTROL_GROUPS)
|
||||
if (control_group >= PX4IO_CONTROL_GROUPS) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
switch (source) {
|
||||
case MIX_FMU:
|
||||
|
|
@ -359,8 +369,8 @@ mixer_callback(uintptr_t handle,
|
|||
}
|
||||
}
|
||||
|
||||
/* motor spinup phase - lock throttle to zero */
|
||||
if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
|
||||
/* motor spinup phase or only safety off, but not armed - lock throttle to zero */
|
||||
if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
|
||||
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
|
||||
control_index == actuator_controls_s::INDEX_THROTTLE) {
|
||||
/* limit the throttle output to zero during motor spinup,
|
||||
|
|
@ -458,8 +468,9 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||
isr_debug(2, "used %u", mixer_text_length - resid);
|
||||
|
||||
/* copy any leftover text to the base of the buffer for re-use */
|
||||
if (resid > 0)
|
||||
if (resid > 0) {
|
||||
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
|
||||
}
|
||||
|
||||
mixer_text_length = resid;
|
||||
|
||||
|
|
@ -482,8 +493,9 @@ mixer_set_failsafe()
|
|||
*/
|
||||
|
||||
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* set failsafe defaults to the values for all inputs = 0 */
|
||||
float outputs[PX4IO_SERVO_COUNT];
|
||||
|
|
@ -501,7 +513,8 @@ mixer_set_failsafe()
|
|||
}
|
||||
|
||||
/* disable the rest of the outputs */
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
|
||||
for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) {
|
||||
r_page_servo_failsafe[i] = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -98,20 +98,25 @@ Mixer::scale(const mixer_scaler_s &scaler, float input)
|
|||
int
|
||||
Mixer::scale_check(struct mixer_scaler_s &scaler)
|
||||
{
|
||||
if (scaler.offset > 1.001f)
|
||||
if (scaler.offset > 1.001f) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (scaler.offset < -1.001f)
|
||||
if (scaler.offset < -1.001f) {
|
||||
return 2;
|
||||
}
|
||||
|
||||
if (scaler.min_output > scaler.max_output)
|
||||
if (scaler.min_output > scaler.max_output) {
|
||||
return 3;
|
||||
}
|
||||
|
||||
if (scaler.min_output < -1.001f)
|
||||
if (scaler.min_output < -1.001f) {
|
||||
return 4;
|
||||
}
|
||||
|
||||
if (scaler.max_output > 1.001f)
|
||||
if (scaler.max_output > 1.001f) {
|
||||
return 5;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -120,11 +125,14 @@ const char *
|
|||
Mixer::findtag(const char *buf, unsigned &buflen, char tag)
|
||||
{
|
||||
while (buflen >= 2) {
|
||||
if ((buf[0] == tag) && (buf[1] == ':'))
|
||||
if ((buf[0] == tag) && (buf[1] == ':')) {
|
||||
return buf;
|
||||
}
|
||||
|
||||
buf++;
|
||||
buflen--;
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
|
|
@ -174,13 +182,15 @@ NullMixer::from_text(const char *buf, unsigned &buflen)
|
|||
|
||||
/* enforce that the mixer ends with space or a new line */
|
||||
for (int i = buflen - 1; i >= 0; i--) {
|
||||
if (buf[i] == '\0')
|
||||
if (buf[i] == '\0') {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* require a space or newline at the end of the buffer, fail on printable chars */
|
||||
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
|
||||
/* found a line ending or space, so no split symbols / numbers. good. */
|
||||
break;
|
||||
|
||||
} else {
|
||||
return nm;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -222,7 +222,7 @@ protected:
|
|||
* @param buflen length of the buffer.
|
||||
* @param tag character to search for.
|
||||
*/
|
||||
static const char * findtag(const char *buf, unsigned &buflen, char tag);
|
||||
static const char *findtag(const char *buf, unsigned &buflen, char tag);
|
||||
|
||||
/**
|
||||
* Skip a line
|
||||
|
|
@ -231,13 +231,13 @@ protected:
|
|||
* @param buflen length of the buffer.
|
||||
* @return 0 / OK if a line could be skipped, 1 else
|
||||
*/
|
||||
static const char * skipline(const char *buf, unsigned &buflen);
|
||||
static const char *skipline(const char *buf, unsigned &buflen);
|
||||
|
||||
private:
|
||||
|
||||
/* do not allow to copy due to prt data members */
|
||||
Mixer(const Mixer&);
|
||||
Mixer& operator=(const Mixer&);
|
||||
Mixer(const Mixer &);
|
||||
Mixer &operator=(const Mixer &);
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -316,8 +316,8 @@ private:
|
|||
Mixer *_first; /**< linked list of mixers */
|
||||
|
||||
/* do not allow to copy due to pointer data members */
|
||||
MixerGroup(const MixerGroup&);
|
||||
MixerGroup operator=(const MixerGroup&);
|
||||
MixerGroup(const MixerGroup &);
|
||||
MixerGroup operator=(const MixerGroup &);
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -437,8 +437,8 @@ private:
|
|||
uint8_t &control_index);
|
||||
|
||||
/* do not allow to copy due to ptr data members */
|
||||
SimpleMixer(const SimpleMixer&);
|
||||
SimpleMixer operator=(const SimpleMixer&);
|
||||
SimpleMixer(const SimpleMixer &);
|
||||
SimpleMixer operator=(const SimpleMixer &);
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -449,13 +449,13 @@ private:
|
|||
typedef unsigned int MultirotorGeometryUnderlyingType;
|
||||
enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
|
||||
|
||||
/**
|
||||
* Multi-rotor mixer for pre-defined vehicle geometries.
|
||||
*
|
||||
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
|
||||
* a set of outputs based on the configured geometry.
|
||||
*/
|
||||
class __EXPORT MultirotorMixer : public Mixer
|
||||
/**
|
||||
* Multi-rotor mixer for pre-defined vehicle geometries.
|
||||
*
|
||||
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
|
||||
* a set of outputs based on the configured geometry.
|
||||
*/
|
||||
class __EXPORT MultirotorMixer : public Mixer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
|
|
@ -531,8 +531,8 @@ private:
|
|||
const Rotor *_rotors;
|
||||
|
||||
/* do not allow to copy due to ptr data members */
|
||||
MultirotorMixer(const MultirotorMixer&);
|
||||
MultirotorMixer operator=(const MultirotorMixer&);
|
||||
MultirotorMixer(const MultirotorMixer &);
|
||||
MultirotorMixer operator=(const MultirotorMixer &);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -52,6 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
|
|||
|
||||
/* open the mixer definition file */
|
||||
fp = fopen(fname, "r");
|
||||
|
||||
if (fp == NULL) {
|
||||
warnx("file not found");
|
||||
return -1;
|
||||
|
|
@ -59,29 +60,38 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
|
|||
|
||||
/* read valid lines from the file into a buffer */
|
||||
buf[0] = '\0';
|
||||
|
||||
for (;;) {
|
||||
|
||||
/* get a line, bail on error/EOF */
|
||||
line[0] = '\0';
|
||||
if (fgets(line, sizeof(line), fp) == NULL)
|
||||
|
||||
if (fgets(line, sizeof(line), fp) == NULL) {
|
||||
break;
|
||||
}
|
||||
|
||||
/* if the line doesn't look like a mixer definition line, skip it */
|
||||
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
|
||||
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* compact whitespace in the buffer */
|
||||
char *t, *f;
|
||||
|
||||
for (f = line; *f != '\0'; f++) {
|
||||
/* scan for space characters */
|
||||
if (*f == ' ') {
|
||||
/* look for additional spaces */
|
||||
t = f + 1;
|
||||
while (*t == ' ')
|
||||
|
||||
while (*t == ' ') {
|
||||
t++;
|
||||
}
|
||||
|
||||
if (*t == '\0') {
|
||||
/* strip trailing whitespace */
|
||||
*f = '\0';
|
||||
|
||||
} else if (t > (f + 1)) {
|
||||
memmove(f + 1, t, strlen(t) + 1);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2015 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
|
||||
|
|
@ -109,15 +109,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
|
|||
|
||||
/* enforce that the mixer ends with space or a new line */
|
||||
for (int i = buflen - 1; i >= 0; i--) {
|
||||
if (buf[i] == '\0')
|
||||
if (buf[i] == '\0') {
|
||||
continue;
|
||||
}
|
||||
|
||||
/* require a space or newline at the end of the buffer, fail on printable chars */
|
||||
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
|
||||
/* found a line ending or space, so no split symbols / numbers. good. */
|
||||
break;
|
||||
|
||||
} else {
|
||||
debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
|
||||
debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen - 1, buf[i]);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
|
|
@ -134,6 +136,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
|
|||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return nullptr;
|
||||
|
|
@ -170,7 +173,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
|
|||
|
||||
} else if (!strcmp(geomname, "8x")) {
|
||||
geometry = MultirotorGeometry::OCTA_X;
|
||||
|
||||
|
||||
} else if (!strcmp(geomname, "8c")) {
|
||||
geometry = MultirotorGeometry::OCTA_COX;
|
||||
|
||||
|
|
@ -222,6 +225,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
|
|||
if (status_reg != NULL) {
|
||||
(*status_reg) = 0;
|
||||
}
|
||||
|
||||
// thrust boost parameters
|
||||
float thrust_increase_factor = 1.5f;
|
||||
float thrust_decrease_factor = 0.6f;
|
||||
|
|
@ -238,6 +242,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
|
|||
if (out < min_out) {
|
||||
min_out = out;
|
||||
}
|
||||
|
||||
if (out > max_out) {
|
||||
max_out = out;
|
||||
}
|
||||
|
|
@ -248,87 +253,94 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
|
|||
float boost = 0.0f; // value added to demanded thrust (can also be negative)
|
||||
float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch
|
||||
|
||||
if(min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
|
||||
if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
|
||||
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
|
||||
if(max_thrust_diff >= -min_out) {
|
||||
|
||||
if (max_thrust_diff >= -min_out) {
|
||||
boost = -min_out;
|
||||
}
|
||||
else {
|
||||
|
||||
} else {
|
||||
boost = max_thrust_diff;
|
||||
roll_pitch_scale = (thrust + boost)/(thrust - min_out);
|
||||
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
|
||||
}
|
||||
}
|
||||
else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
|
||||
float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
|
||||
if(max_thrust_diff >= max_out - 1.0f) {
|
||||
|
||||
} else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
|
||||
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
|
||||
|
||||
if (max_thrust_diff >= max_out - 1.0f) {
|
||||
boost = -(max_out - 1.0f);
|
||||
|
||||
} else {
|
||||
boost = -max_thrust_diff;
|
||||
roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
|
||||
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
|
||||
}
|
||||
}
|
||||
else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
|
||||
|
||||
} else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
|
||||
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
|
||||
boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff);
|
||||
roll_pitch_scale = (thrust + boost)/(thrust - min_out);
|
||||
}
|
||||
else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f ) {
|
||||
float max_thrust_diff = thrust - thrust_decrease_factor*thrust;
|
||||
boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f);
|
||||
roll_pitch_scale = (1 - (thrust + boost))/(max_out - thrust);
|
||||
}
|
||||
else if (min_out < 0.0f && max_out > 1.0f) {
|
||||
boost = constrain(-(max_out - 1.0f + min_out)/2.0f, thrust_decrease_factor*thrust - thrust, thrust_increase_factor*thrust - thrust);
|
||||
roll_pitch_scale = (thrust + boost)/(thrust - min_out);
|
||||
boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff);
|
||||
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
|
||||
|
||||
} else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) {
|
||||
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
|
||||
boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f);
|
||||
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
|
||||
|
||||
} else if (min_out < 0.0f && max_out > 1.0f) {
|
||||
boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust,
|
||||
thrust_increase_factor * thrust - thrust);
|
||||
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
|
||||
}
|
||||
|
||||
// notify if saturation has occurred
|
||||
if(min_out < 0.0f) {
|
||||
if(status_reg != NULL) {
|
||||
if (min_out < 0.0f) {
|
||||
if (status_reg != NULL) {
|
||||
(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
|
||||
}
|
||||
}
|
||||
if(max_out > 0.0f) {
|
||||
if(status_reg != NULL) {
|
||||
|
||||
if (max_out > 0.0f) {
|
||||
if (status_reg != NULL) {
|
||||
(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
|
||||
}
|
||||
}
|
||||
|
||||
// mix again but now with thrust boost, scale roll/pitch and also add yaw
|
||||
for(unsigned i = 0; i < _rotor_count; i++) {
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
float out = (roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
|
||||
yaw * _rotors[i].yaw_scale +
|
||||
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
|
||||
yaw * _rotors[i].yaw_scale +
|
||||
thrust + boost;
|
||||
|
||||
out *= _rotors[i].out_scale;
|
||||
|
||||
// scale yaw if it violates limits. inform about yaw limit reached
|
||||
if(out < 0.0f) {
|
||||
if (out < 0.0f) {
|
||||
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
|
||||
roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale;
|
||||
if(status_reg != NULL) {
|
||||
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
|
||||
|
||||
if (status_reg != NULL) {
|
||||
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
|
||||
}
|
||||
}
|
||||
else if(out > 1.0f) {
|
||||
|
||||
} else if (out > 1.0f) {
|
||||
// allow to reduce thrust to get some yaw response
|
||||
float thrust_reduction = fminf(0.15f, out - 1.0f);
|
||||
thrust -= thrust_reduction;
|
||||
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
|
||||
roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale;
|
||||
if(status_reg != NULL) {
|
||||
roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
|
||||
|
||||
if (status_reg != NULL) {
|
||||
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* last mix, add yaw and scale outputs to range idle_speed...1 */
|
||||
/* add yaw and scale outputs to range idle_speed...1 */
|
||||
for (unsigned i = 0; i < _rotor_count; i++) {
|
||||
outputs[i] = (roll * _rotors[i].roll_scale +
|
||||
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
|
||||
yaw * _rotors[i].yaw_scale +
|
||||
thrust + boost;
|
||||
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
|
||||
yaw * _rotors[i].yaw_scale +
|
||||
thrust + boost;
|
||||
|
||||
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2012-2015 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
|
||||
|
|
@ -67,8 +67,9 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb,
|
|||
|
||||
SimpleMixer::~SimpleMixer()
|
||||
{
|
||||
if (_info != nullptr)
|
||||
if (_info != nullptr) {
|
||||
free(_info);
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -77,8 +78,9 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
|
|||
int ret;
|
||||
int s[5];
|
||||
int n = -1;
|
||||
|
||||
|
||||
buf = findtag(buf, buflen, 'O');
|
||||
|
||||
if ((buf == nullptr) || (buflen < 12)) {
|
||||
debug("output parser failed finding tag, ret: '%s'", buf);
|
||||
return -1;
|
||||
|
|
@ -91,6 +93,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
|
|||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return -1;
|
||||
|
|
@ -106,12 +109,14 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
|
|||
}
|
||||
|
||||
int
|
||||
SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group, uint8_t &control_index)
|
||||
SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler, uint8_t &control_group,
|
||||
uint8_t &control_index)
|
||||
{
|
||||
unsigned u[2];
|
||||
int s[5];
|
||||
|
||||
buf = findtag(buf, buflen, 'S');
|
||||
|
||||
if ((buf == nullptr) || (buflen < 16)) {
|
||||
debug("control parser failed finding tag, ret: '%s'", buf);
|
||||
return -1;
|
||||
|
|
@ -124,6 +129,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
|
|||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
return -1;
|
||||
|
|
@ -156,6 +162,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
|
|||
}
|
||||
|
||||
buf = skipline(buf, buflen);
|
||||
|
||||
if (buf == nullptr) {
|
||||
debug("no line ending, line is incomplete");
|
||||
goto out;
|
||||
|
|
@ -198,14 +205,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
|
|||
|
||||
out:
|
||||
|
||||
if (mixinfo != nullptr)
|
||||
if (mixinfo != nullptr) {
|
||||
free(mixinfo);
|
||||
}
|
||||
|
||||
return sm;
|
||||
}
|
||||
|
||||
SimpleMixer *
|
||||
SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
|
||||
SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, unsigned input, uint16_t min,
|
||||
uint16_t mid, uint16_t max)
|
||||
{
|
||||
SimpleMixer *sm = nullptr;
|
||||
mixer_simple_s *mixinfo = nullptr;
|
||||
|
|
@ -258,8 +267,9 @@ SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, u
|
|||
|
||||
out:
|
||||
|
||||
if (mixinfo != nullptr)
|
||||
if (mixinfo != nullptr) {
|
||||
free(mixinfo);
|
||||
}
|
||||
|
||||
return sm;
|
||||
}
|
||||
|
|
@ -269,11 +279,13 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
|
|||
{
|
||||
float sum = 0.0f;
|
||||
|
||||
if (_info == nullptr)
|
||||
if (_info == nullptr) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (space < 1)
|
||||
if (space < 1) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (unsigned i = 0; i < _info->control_count; i++) {
|
||||
float input;
|
||||
|
|
@ -293,8 +305,9 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
|
|||
void
|
||||
SimpleMixer::groups_required(uint32_t &groups)
|
||||
{
|
||||
for (unsigned i = 0; i < _info->control_count; i++)
|
||||
for (unsigned i = 0; i < _info->control_count; i++) {
|
||||
groups |= 1 << _info->controls[i].control_group;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
|
|
@ -305,14 +318,16 @@ SimpleMixer::check()
|
|||
|
||||
/* sanity that presumes that a mixer includes a control no more than once */
|
||||
/* max of 32 groups due to groups_required API */
|
||||
if (_info->control_count > 32)
|
||||
if (_info->control_count > 32) {
|
||||
return -2;
|
||||
}
|
||||
|
||||
/* validate the output scaler */
|
||||
ret = scale_check(_info->output_scaler);
|
||||
|
||||
if (ret != 0)
|
||||
if (ret != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* validate input scalers */
|
||||
for (unsigned i = 0; i < _info->control_count; i++) {
|
||||
|
|
@ -328,8 +343,9 @@ SimpleMixer::check()
|
|||
/* validate the scaler */
|
||||
ret = scale_check(_info->controls[i].scaler);
|
||||
|
||||
if (ret != 0)
|
||||
if (ret != 0) {
|
||||
return (10 * i + ret);
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
Loading…
Reference in New Issue