Mixer library: Fix code style

This commit is contained in:
Lorenz Meier 2015-07-04 09:51:10 +02:00
parent fd63ba7b89
commit 5f586fc354
6 changed files with 157 additions and 96 deletions

View File

@ -35,6 +35,8 @@
* @file mixer.cpp * @file mixer.cpp
* *
* Control channel input/output mixer and failsafe. * Control channel input/output mixer and failsafe.
*
* @author Lorenz Meier <lorenz@px4.io>
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -64,6 +66,7 @@ extern "C" {
/* current servo arm/disarm state */ /* current servo arm/disarm state */
static bool mixer_servos_armed = false; static bool mixer_servos_armed = false;
static bool should_arm = false; static bool should_arm = false;
static bool should_arm_nothrottle = false;
static bool should_always_enable_pwm = false; static bool should_always_enable_pwm = false;
static volatile bool in_mixer = 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) 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_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_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 */ /* 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); 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; r_page_servos[i] = 0;
outputs[i] = 0;
}
/* store normalized outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
} }
} }
/* set arming */ /* 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 */ /* check any conditions that prevent arming */
if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) {
needs_to_arm = false; needs_to_arm = false;
} }
if (!should_arm && !should_always_enable_pwm) {
needs_to_arm = false;
}
if (needs_to_arm && !mixer_servos_armed) { if (needs_to_arm && !mixer_servos_armed) {
/* need to arm, but not armed */ /* need to arm, but not armed */
@ -308,8 +317,9 @@ mixer_callback(uintptr_t handle,
uint8_t control_index, uint8_t control_index,
float &control) float &control)
{ {
if (control_group >= PX4IO_CONTROL_GROUPS) if (control_group >= PX4IO_CONTROL_GROUPS) {
return -1; return -1;
}
switch (source) { switch (source) {
case MIX_FMU: case MIX_FMU:
@ -359,8 +369,8 @@ mixer_callback(uintptr_t handle,
} }
} }
/* motor spinup phase - lock throttle to zero */ /* motor spinup phase or only safety off, but not armed - lock throttle to zero */
if (pwm_limit.state == PWM_LIMIT_STATE_RAMP) { if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) { control_index == actuator_controls_s::INDEX_THROTTLE) {
/* limit the throttle output to zero during motor spinup, /* 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); isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */ /* 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); memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
}
mixer_text_length = resid; mixer_text_length = resid;
@ -482,8 +493,9 @@ mixer_set_failsafe()
*/ */
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || 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; return;
}
/* set failsafe defaults to the values for all inputs = 0 */ /* set failsafe defaults to the values for all inputs = 0 */
float outputs[PX4IO_SERVO_COUNT]; float outputs[PX4IO_SERVO_COUNT];
@ -501,7 +513,8 @@ mixer_set_failsafe()
} }
/* disable the rest of the outputs */ /* 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; r_page_servo_failsafe[i] = 0;
}
} }

View File

@ -98,20 +98,25 @@ Mixer::scale(const mixer_scaler_s &scaler, float input)
int int
Mixer::scale_check(struct mixer_scaler_s &scaler) Mixer::scale_check(struct mixer_scaler_s &scaler)
{ {
if (scaler.offset > 1.001f) if (scaler.offset > 1.001f) {
return 1; return 1;
}
if (scaler.offset < -1.001f) if (scaler.offset < -1.001f) {
return 2; return 2;
}
if (scaler.min_output > scaler.max_output) if (scaler.min_output > scaler.max_output) {
return 3; return 3;
}
if (scaler.min_output < -1.001f) if (scaler.min_output < -1.001f) {
return 4; return 4;
}
if (scaler.max_output > 1.001f) if (scaler.max_output > 1.001f) {
return 5; return 5;
}
return 0; return 0;
} }
@ -120,11 +125,14 @@ const char *
Mixer::findtag(const char *buf, unsigned &buflen, char tag) Mixer::findtag(const char *buf, unsigned &buflen, char tag)
{ {
while (buflen >= 2) { while (buflen >= 2) {
if ((buf[0] == tag) && (buf[1] == ':')) if ((buf[0] == tag) && (buf[1] == ':')) {
return buf; return buf;
}
buf++; buf++;
buflen--; buflen--;
} }
return nullptr; 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 */ /* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) { for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0') if (buf[i] == '\0') {
continue; continue;
}
/* require a space or newline at the end of the buffer, fail on printable chars */ /* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending or space, so no split symbols / numbers. good. */ /* found a line ending or space, so no split symbols / numbers. good. */
break; break;
} else { } else {
return nm; return nm;
} }

View File

@ -222,7 +222,7 @@ protected:
* @param buflen length of the buffer. * @param buflen length of the buffer.
* @param tag character to search for. * @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 * Skip a line
@ -231,13 +231,13 @@ protected:
* @param buflen length of the buffer. * @param buflen length of the buffer.
* @return 0 / OK if a line could be skipped, 1 else * @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: private:
/* do not allow to copy due to prt data members */ /* do not allow to copy due to prt data members */
Mixer(const Mixer&); Mixer(const Mixer &);
Mixer& operator=(const Mixer&); Mixer &operator=(const Mixer &);
}; };
/** /**
@ -316,8 +316,8 @@ private:
Mixer *_first; /**< linked list of mixers */ Mixer *_first; /**< linked list of mixers */
/* do not allow to copy due to pointer data members */ /* do not allow to copy due to pointer data members */
MixerGroup(const MixerGroup&); MixerGroup(const MixerGroup &);
MixerGroup operator=(const MixerGroup&); MixerGroup operator=(const MixerGroup &);
}; };
/** /**
@ -437,8 +437,8 @@ private:
uint8_t &control_index); uint8_t &control_index);
/* do not allow to copy due to ptr data members */ /* do not allow to copy due to ptr data members */
SimpleMixer(const SimpleMixer&); SimpleMixer(const SimpleMixer &);
SimpleMixer operator=(const SimpleMixer&); SimpleMixer operator=(const SimpleMixer &);
}; };
/** /**
@ -449,13 +449,13 @@ private:
typedef unsigned int MultirotorGeometryUnderlyingType; typedef unsigned int MultirotorGeometryUnderlyingType;
enum class MultirotorGeometry : MultirotorGeometryUnderlyingType; enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
/** /**
* Multi-rotor mixer for pre-defined vehicle geometries. * Multi-rotor mixer for pre-defined vehicle geometries.
* *
* Collects four inputs (roll, pitch, yaw, thrust) and mixes them to * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
* a set of outputs based on the configured geometry. * a set of outputs based on the configured geometry.
*/ */
class __EXPORT MultirotorMixer : public Mixer class __EXPORT MultirotorMixer : public Mixer
{ {
public: public:
/** /**
@ -531,8 +531,8 @@ private:
const Rotor *_rotors; const Rotor *_rotors;
/* do not allow to copy due to ptr data members */ /* do not allow to copy due to ptr data members */
MultirotorMixer(const MultirotorMixer&); MultirotorMixer(const MultirotorMixer &);
MultirotorMixer operator=(const MultirotorMixer&); MultirotorMixer operator=(const MultirotorMixer &);
}; };
#endif #endif

View File

@ -52,6 +52,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
/* open the mixer definition file */ /* open the mixer definition file */
fp = fopen(fname, "r"); fp = fopen(fname, "r");
if (fp == NULL) { if (fp == NULL) {
warnx("file not found"); warnx("file not found");
return -1; 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 */ /* read valid lines from the file into a buffer */
buf[0] = '\0'; buf[0] = '\0';
for (;;) { for (;;) {
/* get a line, bail on error/EOF */ /* get a line, bail on error/EOF */
line[0] = '\0'; line[0] = '\0';
if (fgets(line, sizeof(line), fp) == NULL)
if (fgets(line, sizeof(line), fp) == NULL) {
break; break;
}
/* if the line doesn't look like a mixer definition line, skip it */ /* 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; continue;
}
/* compact whitespace in the buffer */ /* compact whitespace in the buffer */
char *t, *f; char *t, *f;
for (f = line; *f != '\0'; f++) { for (f = line; *f != '\0'; f++) {
/* scan for space characters */ /* scan for space characters */
if (*f == ' ') { if (*f == ' ') {
/* look for additional spaces */ /* look for additional spaces */
t = f + 1; t = f + 1;
while (*t == ' ')
while (*t == ' ') {
t++; t++;
}
if (*t == '\0') { if (*t == '\0') {
/* strip trailing whitespace */ /* strip trailing whitespace */
*f = '\0'; *f = '\0';
} else if (t > (f + 1)) { } else if (t > (f + 1)) {
memmove(f + 1, t, strlen(t) + 1); memmove(f + 1, t, strlen(t) + 1);
} }

View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * 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 */ /* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) { for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0') if (buf[i] == '\0') {
continue; continue;
}
/* require a space or newline at the end of the buffer, fail on printable chars */ /* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') { if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending or space, so no split symbols / numbers. good. */ /* found a line ending or space, so no split symbols / numbers. good. */
break; break;
} else { } 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; return nullptr;
} }
@ -134,6 +136,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} }
buf = skipline(buf, buflen); buf = skipline(buf, buflen);
if (buf == nullptr) { if (buf == nullptr) {
debug("no line ending, line is incomplete"); debug("no line ending, line is incomplete");
return nullptr; return nullptr;
@ -170,7 +173,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
} else if (!strcmp(geomname, "8x")) { } else if (!strcmp(geomname, "8x")) {
geometry = MultirotorGeometry::OCTA_X; geometry = MultirotorGeometry::OCTA_X;
} else if (!strcmp(geomname, "8c")) { } else if (!strcmp(geomname, "8c")) {
geometry = MultirotorGeometry::OCTA_COX; geometry = MultirotorGeometry::OCTA_COX;
@ -222,6 +225,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
if (status_reg != NULL) { if (status_reg != NULL) {
(*status_reg) = 0; (*status_reg) = 0;
} }
// thrust boost parameters // thrust boost parameters
float thrust_increase_factor = 1.5f; float thrust_increase_factor = 1.5f;
float thrust_decrease_factor = 0.6f; float thrust_decrease_factor = 0.6f;
@ -238,6 +242,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
if (out < min_out) { if (out < min_out) {
min_out = out; min_out = out;
} }
if (out > max_out) { if (out > max_out) {
max_out = 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 boost = 0.0f; // value added to demanded thrust (can also be negative)
float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch 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; float max_thrust_diff = thrust * thrust_increase_factor - thrust;
if(max_thrust_diff >= -min_out) {
if (max_thrust_diff >= -min_out) {
boost = -min_out; boost = -min_out;
}
else { } else {
boost = max_thrust_diff; 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) { } 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; float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
if(max_thrust_diff >= max_out - 1.0f) {
if (max_thrust_diff >= max_out - 1.0f) {
boost = -(max_out - 1.0f); boost = -(max_out - 1.0f);
} else { } else {
boost = -max_thrust_diff; 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; float max_thrust_diff = thrust * thrust_increase_factor - thrust;
boost = constrain(-min_out - (1.0f - max_out)/2.0f,0.0f, max_thrust_diff); boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, 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 ) { } 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; float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
boost = constrain(-(max_out - 1.0f - min_out)/2.0f, -max_thrust_diff, 0.0f); boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f);
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) { } 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); boost = constrain(-(max_out - 1.0f + min_out) / 2.0f, thrust_decrease_factor * thrust - thrust,
roll_pitch_scale = (thrust + boost)/(thrust - min_out); thrust_increase_factor * thrust - thrust);
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
} }
// notify if saturation has occurred // notify if saturation has occurred
if(min_out < 0.0f) { if (min_out < 0.0f) {
if(status_reg != NULL) { if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT; (*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; (*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
} }
} }
// mix again but now with thrust boost, scale roll/pitch and also add yaw // 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 + float out = (roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale) * roll_pitch_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
yaw * _rotors[i].yaw_scale + yaw * _rotors[i].yaw_scale +
thrust + boost; thrust + boost;
out *= _rotors[i].out_scale; out *= _rotors[i].out_scale;
// scale yaw if it violates limits. inform about yaw limit reached // 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) * yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost)/_rotors[i].yaw_scale; roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
if(status_reg != NULL) {
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; (*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 // allow to reduce thrust to get some yaw response
float thrust_reduction = fminf(0.15f, out - 1.0f); float thrust_reduction = fminf(0.15f, out - 1.0f);
thrust -= thrust_reduction; thrust -= thrust_reduction;
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale; roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
if(status_reg != NULL) {
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; (*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++) { for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = (roll * _rotors[i].roll_scale + outputs[i] = (roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale) * roll_pitch_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
yaw * _rotors[i].yaw_scale + yaw * _rotors[i].yaw_scale +
thrust + boost; thrust + boost;
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f); outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
} }

View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -67,8 +67,9 @@ SimpleMixer::SimpleMixer(ControlCallback control_cb,
SimpleMixer::~SimpleMixer() SimpleMixer::~SimpleMixer()
{ {
if (_info != nullptr) if (_info != nullptr) {
free(_info); free(_info);
}
} }
int int
@ -77,8 +78,9 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
int ret; int ret;
int s[5]; int s[5];
int n = -1; int n = -1;
buf = findtag(buf, buflen, 'O'); buf = findtag(buf, buflen, 'O');
if ((buf == nullptr) || (buflen < 12)) { if ((buf == nullptr) || (buflen < 12)) {
debug("output parser failed finding tag, ret: '%s'", buf); debug("output parser failed finding tag, ret: '%s'", buf);
return -1; return -1;
@ -91,6 +93,7 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
} }
buf = skipline(buf, buflen); buf = skipline(buf, buflen);
if (buf == nullptr) { if (buf == nullptr) {
debug("no line ending, line is incomplete"); debug("no line ending, line is incomplete");
return -1; return -1;
@ -106,12 +109,14 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
} }
int 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]; unsigned u[2];
int s[5]; int s[5];
buf = findtag(buf, buflen, 'S'); buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16)) { if ((buf == nullptr) || (buflen < 16)) {
debug("control parser failed finding tag, ret: '%s'", buf); debug("control parser failed finding tag, ret: '%s'", buf);
return -1; return -1;
@ -124,6 +129,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
} }
buf = skipline(buf, buflen); buf = skipline(buf, buflen);
if (buf == nullptr) { if (buf == nullptr) {
debug("no line ending, line is incomplete"); debug("no line ending, line is incomplete");
return -1; return -1;
@ -156,6 +162,7 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
} }
buf = skipline(buf, buflen); buf = skipline(buf, buflen);
if (buf == nullptr) { if (buf == nullptr) {
debug("no line ending, line is incomplete"); debug("no line ending, line is incomplete");
goto out; goto out;
@ -198,14 +205,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
out: out:
if (mixinfo != nullptr) if (mixinfo != nullptr) {
free(mixinfo); free(mixinfo);
}
return sm; return sm;
} }
SimpleMixer * 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; SimpleMixer *sm = nullptr;
mixer_simple_s *mixinfo = nullptr; mixer_simple_s *mixinfo = nullptr;
@ -258,8 +267,9 @@ SimpleMixer::pwm_input(Mixer::ControlCallback control_cb, uintptr_t cb_handle, u
out: out:
if (mixinfo != nullptr) if (mixinfo != nullptr) {
free(mixinfo); free(mixinfo);
}
return sm; return sm;
} }
@ -269,11 +279,13 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
{ {
float sum = 0.0f; float sum = 0.0f;
if (_info == nullptr) if (_info == nullptr) {
return 0; return 0;
}
if (space < 1) if (space < 1) {
return 0; return 0;
}
for (unsigned i = 0; i < _info->control_count; i++) { for (unsigned i = 0; i < _info->control_count; i++) {
float input; float input;
@ -293,8 +305,9 @@ SimpleMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
void void
SimpleMixer::groups_required(uint32_t &groups) 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; groups |= 1 << _info->controls[i].control_group;
}
} }
int int
@ -305,14 +318,16 @@ SimpleMixer::check()
/* sanity that presumes that a mixer includes a control no more than once */ /* sanity that presumes that a mixer includes a control no more than once */
/* max of 32 groups due to groups_required API */ /* max of 32 groups due to groups_required API */
if (_info->control_count > 32) if (_info->control_count > 32) {
return -2; return -2;
}
/* validate the output scaler */ /* validate the output scaler */
ret = scale_check(_info->output_scaler); ret = scale_check(_info->output_scaler);
if (ret != 0) if (ret != 0) {
return ret; return ret;
}
/* validate input scalers */ /* validate input scalers */
for (unsigned i = 0; i < _info->control_count; i++) { for (unsigned i = 0; i < _info->control_count; i++) {
@ -328,8 +343,9 @@ SimpleMixer::check()
/* validate the scaler */ /* validate the scaler */
ret = scale_check(_info->controls[i].scaler); ret = scale_check(_info->controls[i].scaler);
if (ret != 0) if (ret != 0) {
return (10 * i + ret); return (10 * i + ret);
}
} }
return 0; return 0;