aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-12-29 16:01:24 -0800
committerpx4dev <px4@purgatory.org>2012-12-29 16:01:24 -0800
commitd81edb09cf29dd36d50ce7a7bcf55631fecc470f (patch)
tree63cb30938794a69981782ff666b9e181c2b286da /apps
parentf9520ee39d0e14bc67cce809375fb69de9a7f977 (diff)
downloadpx4-firmware-d81edb09cf29dd36d50ce7a7bcf55631fecc470f.tar.gz
px4-firmware-d81edb09cf29dd36d50ce7a7bcf55631fecc470f.tar.bz2
px4-firmware-d81edb09cf29dd36d50ce7a7bcf55631fecc470f.zip
whitespace/formatting
Diffstat (limited to 'apps')
-rw-r--r--apps/px4io/comms.c14
-rw-r--r--apps/px4io/controls.c4
-rw-r--r--apps/px4io/dsm.c40
-rw-r--r--apps/px4io/mixer.cpp13
-rw-r--r--apps/px4io/protocol.h4
-rw-r--r--apps/px4io/px4io.c2
-rw-r--r--apps/px4io/px4io.h13
-rw-r--r--apps/px4io/safety.c22
-rw-r--r--apps/px4io/sbus.c50
-rw-r--r--apps/systemcmds/mixer/mixer.c2
-rw-r--r--apps/systemlib/mixer/mixer_group.cpp32
-rw-r--r--apps/systemlib/mixer/mixer_multirotor.cpp10
12 files changed, 129 insertions, 77 deletions
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
index bc2142a4b..b815eda3c 100644
--- a/apps/px4io/comms.c
+++ b/apps/px4io/comms.c
@@ -110,6 +110,7 @@ comms_main(void)
if (fds.revents & POLLIN) {
char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf));
+
for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]);
}
@@ -123,7 +124,8 @@ comms_main(void)
/* should we send a report to the FMU? */
now = hrt_absolute_time();
delta = now - last_report_time;
- if ((delta > FMU_MIN_REPORT_INTERVAL) &&
+
+ if ((delta > FMU_MIN_REPORT_INTERVAL) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
system_state.fmu_report_due = false;
@@ -132,6 +134,7 @@ comms_main(void)
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i];
+
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
@@ -172,7 +175,7 @@ comms_handle_command(const void *buffer, size_t length)
system_state.fmu_channel_data[i] = cmd->servo_command[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
- if(system_state.arm_ok && !cmd->arm_ok) {
+ if (system_state.arm_ok && !cmd->arm_ok) {
system_state.armed = false;
}
@@ -185,7 +188,7 @@ comms_handle_command(const void *buffer, size_t length)
// if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false;
- /* XXX do relay changes here */
+ /* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
system_state.relays[i] = cmd->relay_state[i];
@@ -204,14 +207,17 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
case F2I_MAGIC:
comms_handle_command(buffer, length);
break;
+
case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length);
break;
+
case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length);
break;
+
default:
- frame_bad++;
+ frame_bad++;
break;
}
}
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 3b3782918..43e811ab0 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -90,6 +90,7 @@ controls_main(void)
if (fds[0].revents & POLLIN)
locked |= dsm_input();
+
if (fds[1].revents & POLLIN)
locked |= sbus_input();
@@ -139,6 +140,7 @@ ppm_input(void)
/* PPM data exists, copy it */
system_state.rc_channels = ppm_decoded_channels;
+
for (unsigned i = 0; i < ppm_decoded_channels; i++)
system_state.rc_channel_data[i] = ppm_buffer[i];
@@ -150,5 +152,5 @@ ppm_input(void)
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
- }
+ }
}
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 2611f3a03..560ef47d9 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -47,7 +47,7 @@
#include <termios.h>
#include <systemlib/ppm_decode.h>
-
+
#include <drivers/drv_hrt.h>
#define DEBUG
@@ -97,6 +97,7 @@ dsm_init(const char *device)
dsm_guess_format(true);
debug("DSM: ready");
+
} else {
debug("DSM: open failed");
}
@@ -118,7 +119,7 @@ dsm_input(void)
* frame transmission time is ~1.4ms.
*
* We expect to only be called when bytes arrive for processing,
- * and if an interval of more than 5ms passes between calls,
+ * and if an interval of more than 5ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
@@ -126,6 +127,7 @@ dsm_input(void)
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
+
if ((now - last_rx_time) > 5000) {
if (partial_frame_count > 0) {
dsm_frame_drops++;
@@ -142,6 +144,7 @@ dsm_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
+
last_rx_time = now;
/*
@@ -153,7 +156,7 @@ dsm_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < DSM_FRAME_SIZE)
- goto out;
+ goto out;
/*
* Great, it looks like we might have a frame. Go ahead and
@@ -164,7 +167,7 @@ dsm_input(void)
out:
/*
- * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
+ * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
}
@@ -212,6 +215,7 @@ dsm_guess_format(bool reset)
/* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
cs10 |= (1 << channel);
+
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
cs11 |= (1 << channel);
@@ -222,7 +226,7 @@ dsm_guess_format(bool reset)
if (samples++ < 5)
return;
- /*
+ /*
* Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise.
*
@@ -233,7 +237,7 @@ dsm_guess_format(bool reset)
* See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue.
*/
- static uint32_t masks[] = {
+ static uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */
@@ -247,14 +251,17 @@ dsm_guess_format(bool reset)
if (cs10 == masks[i])
votes10++;
+
if (cs11 == masks[i])
votes11++;
}
+
if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11;
debug("DSM: detected 11-bit format");
return;
}
+
if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10;
debug("DSM: detected 10-bit format");
@@ -270,13 +277,13 @@ static void
dsm_decode(hrt_abstime frame_time)
{
-/*
- debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
- frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
- frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
-*/
/*
- * If we have lost signal for at least a second, reset the
+ debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+ frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
+ frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+ */
+ /*
+ * If we have lost signal for at least a second, reset the
* format guessing heuristic.
*/
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
@@ -292,7 +299,7 @@ dsm_decode(hrt_abstime frame_time)
}
/*
- * The encoding of the first two bytes is uncertain, so we're
+ * The encoding of the first two bytes is uncertain, so we're
* going to ignore them for now.
*
* Each channel is a 16-bit unsigned value containing either a 10-
@@ -322,9 +329,10 @@ dsm_decode(hrt_abstime frame_time)
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11)
value /= 2;
+
value += 998;
- /*
+ /*
* Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have.
*
@@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time)
case 0:
channel = 2;
break;
+
case 1:
channel = 0;
break;
+
case 2:
channel = 1;
+
default:
break;
}
+
system_state.rc_channel_data[channel] = value;
}
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 9b3d65dba..0fd4ac717 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -69,9 +69,9 @@ static uint16_t *control_values;
static int control_count;
static int mixer_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &control);
+ uint8_t control_group,
+ uint8_t control_index,
+ float &control);
static MixerGroup mixer_group(mixer_callback, 0);
@@ -96,6 +96,7 @@ mixer_tick(void)
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_use_fmu = false;
}
+
} else {
fmu_input_drops = 0;
system_state.fmu_data_received = false;
@@ -127,6 +128,7 @@ mixer_tick(void)
if (i < mixed) {
/* scale to servo output */
system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
+
} else {
/* set to zero to inhibit PWM pulse output */
system_state.servos[i] = 0;
@@ -144,6 +146,7 @@ mixer_tick(void)
* Decide whether the servos should be armed right now.
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
+
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
up_pwm_servo_arm(true);
@@ -180,6 +183,7 @@ mixer_handle_text(const void *buffer, size_t length)
static unsigned mixer_text_length = 0;
px4io_mixdata *msg = (px4io_mixdata *)buffer;
+
if (length < sizeof(px4io_mixdata))
return;
@@ -189,8 +193,10 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_RESET:
mixer_group.reset();
mixer_text_length = 0;
+
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
+
/* check for overflow - this is really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
return;
@@ -207,6 +213,7 @@ mixer_handle_text(const void *buffer, size_t length)
/* copy any leftover text to the base of the buffer for re-use */
if (mixer_text_length > 0)
memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length);
+
break;
}
} \ No newline at end of file
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 080d33001..979f2f8cb 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -60,7 +60,7 @@ struct px4io_command {
};
/**
- * Periodic report from IO to FMU
+ * Periodic report from IO to FMU
*/
struct px4io_report {
uint16_t i2f_magic;
@@ -72,7 +72,7 @@ struct px4io_report {
};
/**
- * As-needed config message from FMU to IO
+ * As-needed config message from FMU to IO
*/
struct px4io_config {
uint16_t f2i_config_magic;
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 230906662..74362617d 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -88,7 +88,7 @@ int user_start(int argc, char *argv[])
up_pwm_servo_init(0xff);
/* start the flight control signal handler */
- task_create("FCon",
+ task_create("FCon",
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)controls_main,
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index f127df4a3..ad10961b4 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -31,11 +31,11 @@
*
****************************************************************************/
- /**
- * @file px4io.h
- *
- * General defines and structures for the PX4IO module firmware.
- */
+/**
+ * @file px4io.h
+ *
+ * General defines and structures for the PX4IO module firmware.
+ */
#include <nuttx/config.h>
@@ -66,8 +66,7 @@
/*
* System state structure.
*/
-struct sys_state_s
-{
+struct sys_state_s {
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 60d20905a..93596ca75 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -31,9 +31,9 @@
*
****************************************************************************/
- /**
- * @file Safety button logic.
- */
+/**
+ * @file Safety button logic.
+ */
#include <nuttx/config.h>
#include <stdio.h>
@@ -95,13 +95,13 @@ safety_init(void)
static void
safety_check_button(void *arg)
{
- /*
+ /*
* Debounce the safety button, change state if it has been held for long enough.
*
*/
safety_button_pressed = BUTTON_SAFETY;
- if(safety_button_pressed) {
+ if (safety_button_pressed) {
//printf("Pressed, Arm counter: %d, Disarm counter: %d\n", arm_counter, disarm_counter);
}
@@ -109,34 +109,42 @@ safety_check_button(void *arg)
if (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
+
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */
system_state.armed = true;
counter++;
system_state.fmu_report_due = true;
}
- /* Disarm quickly */
+
+ /* Disarm quickly */
+
} else if (safety_button_pressed && system_state.armed) {
if (counter < DISARM_COUNTER_THRESHOLD) {
counter++;
+
} else if (counter == DISARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
system_state.armed = false;
counter++;
system_state.fmu_report_due = true;
}
+
} else {
counter = 0;
}
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
+
if (system_state.armed) {
if (system_state.arm_ok) {
pattern = LED_PATTERN_IO_FMU_ARMED;
+
} else {
pattern = LED_PATTERN_IO_ARMED;
}
+
} else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED;
}
@@ -167,8 +175,10 @@ failsafe_blink(void *arg)
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) {
failsafe = !failsafe;
+
} else {
failsafe = false;
}
+
LED_AMBER(failsafe);
} \ No newline at end of file
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index a8f628a84..9679f657b 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -88,6 +88,7 @@ sbus_init(const char *device)
last_rx_time = hrt_absolute_time();
debug("Sbus: ready");
+
} else {
debug("Sbus: open failed");
}
@@ -109,7 +110,7 @@ sbus_input(void)
* frame transmission time is ~2ms.
*
* We expect to only be called when bytes arrive for processing,
- * and if an interval of more than 3ms passes between calls,
+ * and if an interval of more than 3ms passes between calls,
* the first byte we read will be the first byte of a frame.
*
* In the case where byte(s) are dropped from a frame, this also
@@ -117,6 +118,7 @@ sbus_input(void)
* if we didn't drop bytes...
*/
now = hrt_absolute_time();
+
if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) {
sbus_frame_drops++;
@@ -133,6 +135,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
+
last_rx_time = now;
/*
@@ -144,7 +147,7 @@ sbus_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
- goto out;
+ goto out;
/*
* Great, it looks like we might have a frame. Go ahead and
@@ -155,7 +158,7 @@ sbus_input(void)
out:
/*
- * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
+ * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
}
@@ -177,23 +180,23 @@ struct sbus_bit_pick {
uint8_t mask;
uint8_t lshift;
};
-static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
-/* 0 */ { { 0, 0, 0xff, 0},{ 1, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
-/* 1 */ { { 1, 3, 0x1f, 0},{ 2, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
-/* 2 */ { { 2, 6, 0x03, 0},{ 3, 0, 0xff, 2},{ 4, 0, 0x01, 10} },
-/* 3 */ { { 4, 1, 0x7f, 0},{ 5, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
-/* 4 */ { { 5, 4, 0x0f, 0},{ 6, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
-/* 5 */ { { 6, 7, 0x01, 0},{ 7, 0, 0xff, 1},{ 8, 0, 0x03, 9} },
-/* 6 */ { { 8, 2, 0x3f, 0},{ 9, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
-/* 7 */ { { 9, 5, 0x07, 0},{10, 0, 0xff, 3},{ 0, 0, 0x00, 0} },
-/* 8 */ { {11, 0, 0xff, 0},{12, 0, 0x07, 8},{ 0, 0, 0x00, 0} },
-/* 9 */ { {12, 3, 0x1f, 0},{13, 0, 0x3f, 5},{ 0, 0, 0x00, 0} },
-/* 10 */ { {13, 6, 0x03, 0},{14, 0, 0xff, 2},{15, 0, 0x01, 10} },
-/* 11 */ { {15, 1, 0x7f, 0},{16, 0, 0x0f, 7},{ 0, 0, 0x00, 0} },
-/* 12 */ { {16, 4, 0x0f, 0},{17, 0, 0x7f, 4},{ 0, 0, 0x00, 0} },
-/* 13 */ { {17, 7, 0x01, 0},{18, 0, 0xff, 1},{19, 0, 0x03, 9} },
-/* 14 */ { {19, 2, 0x3f, 0},{20, 0, 0x1f, 6},{ 0, 0, 0x00, 0} },
-/* 15 */ { {20, 5, 0x07, 0},{21, 0, 0xff, 3},{ 0, 0, 0x00, 0} }
+static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
+ /* 0 */ { { 0, 0, 0xff, 0}, { 1, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
+ /* 1 */ { { 1, 3, 0x1f, 0}, { 2, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
+ /* 2 */ { { 2, 6, 0x03, 0}, { 3, 0, 0xff, 2}, { 4, 0, 0x01, 10} },
+ /* 3 */ { { 4, 1, 0x7f, 0}, { 5, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
+ /* 4 */ { { 5, 4, 0x0f, 0}, { 6, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
+ /* 5 */ { { 6, 7, 0x01, 0}, { 7, 0, 0xff, 1}, { 8, 0, 0x03, 9} },
+ /* 6 */ { { 8, 2, 0x3f, 0}, { 9, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
+ /* 7 */ { { 9, 5, 0x07, 0}, {10, 0, 0xff, 3}, { 0, 0, 0x00, 0} },
+ /* 8 */ { {11, 0, 0xff, 0}, {12, 0, 0x07, 8}, { 0, 0, 0x00, 0} },
+ /* 9 */ { {12, 3, 0x1f, 0}, {13, 0, 0x3f, 5}, { 0, 0, 0x00, 0} },
+ /* 10 */ { {13, 6, 0x03, 0}, {14, 0, 0xff, 2}, {15, 0, 0x01, 10} },
+ /* 11 */ { {15, 1, 0x7f, 0}, {16, 0, 0x0f, 7}, { 0, 0, 0x00, 0} },
+ /* 12 */ { {16, 4, 0x0f, 0}, {17, 0, 0x7f, 4}, { 0, 0, 0x00, 0} },
+ /* 13 */ { {17, 7, 0x01, 0}, {18, 0, 0xff, 1}, {19, 0, 0x03, 9} },
+ /* 14 */ { {19, 2, 0x3f, 0}, {20, 0, 0x1f, 6}, { 0, 0, 0x00, 0} },
+ /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
static void
@@ -213,8 +216,8 @@ sbus_decode(hrt_abstime frame_time)
/* we have received something we think is a frame */
last_frame_time = frame_time;
- unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
- SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
+ unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
+ SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
/* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) {
@@ -232,6 +235,7 @@ sbus_decode(hrt_abstime frame_time)
value |= piece;
}
}
+
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998;
}
@@ -245,7 +249,7 @@ sbus_decode(hrt_abstime frame_time)
system_state.rc_channels = chancount;
/* and note that we have received data from the R/C controller */
- system_state.rc_channels_timestamp = frame_time;
+ system_state.rc_channels_timestamp = frame_time;
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
index 8d73bfcc4..e2f7b5bd5 100644
--- a/apps/systemcmds/mixer/mixer.c
+++ b/apps/systemcmds/mixer/mixer.c
@@ -117,6 +117,8 @@ load(const char *devname, const char *fname)
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
continue;
+ /* XXX an optimisation here would be to strip extra whitespace */
+
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
break;
diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp
index 6dfe4fbef..b98531c4d 100644
--- a/apps/systemlib/mixer/mixer_group.cpp
+++ b/apps/systemlib/mixer/mixer_group.cpp
@@ -91,7 +91,7 @@ MixerGroup::reset()
mixer = _first;
_first = mixer->_next;
delete mixer;
- }
+ }
}
unsigned
@@ -132,26 +132,32 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
/* use the next character as a hint to decide which mixer class to construct */
switch (*p) {
- case 'Z':
- m = NullMixer::from_text(p, buflen);
- break;
- case 'M':
- m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen);
- break;
- case 'R':
- m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen);
- break;
- default:
- /* it's probably junk or whitespace */
- break;
+ case 'Z':
+ m = NullMixer::from_text(p, buflen);
+ break;
+
+ case 'M':
+ m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen);
+ break;
+
+ case 'R':
+ m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen);
+ break;
+
+ default:
+ /* it's probably junk or whitespace */
+ break;
}
+
if (m != nullptr) {
add_mixer(m);
ret = 0;
+
} else {
/* skip whitespace or junk in the buffer */
buflen--;
}
}
+
return ret;
}
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index 233025b21..3dd6dfcf7 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -166,10 +166,12 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
debug("multirotor parse failed on '%s'", buf);
return nullptr;
}
+
if (used > (int)buflen) {
debug("multirotor spec used %d of %u", used, buflen);
return nullptr;
}
+
buflen -= used;
if (!strcmp(geomname, "4+")) {
@@ -226,10 +228,12 @@ MultirotorMixer::mix(float *outputs, unsigned space)
/* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) {
output_factor = 0.0f;
- /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+ /* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+
} else if (thrust < startpoint_full_control && thrust > min_thrust) {
- output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust);
- /* and then stay at full control */
+ output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
+ /* and then stay at full control */
+
} else {
output_factor = max_thrust;
}