zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

whitespace/formatting

sbg
px4dev 12 years ago
parent
f9520ee39d
commit
d81edb09cf
12 changed files with 129 additions and 77 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 10
      apps/px4io/comms.c
  2. 2
      apps/px4io/controls.c
  3. 24
      apps/px4io/dsm.c
  4. 13
      apps/px4io/mixer.cpp
  5. 13
      apps/px4io/px4io.h
  6. 20
      apps/px4io/safety.c
  7. 40
      apps/px4io/sbus.c
  8. 2
      apps/systemcmds/mixer/mixer.c
  9. 30
      apps/systemlib/mixer/mixer_group.cpp
  10. 10
      apps/systemlib/mixer/mixer_multirotor.cpp

10
apps/px4io/comms.c
Unescape View File

@ -110,6 +110,7 @@ comms_main(void)
if (fds.revents & POLLIN) { if (fds.revents & POLLIN) {
char buf[32]; char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf)); ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++) for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]); hx_stream_rx(stream, buf[i]);
} }
@ -123,6 +124,7 @@ comms_main(void)
/* should we send a report to the FMU? */ /* should we send a report to the FMU? */
now = hrt_absolute_time(); now = hrt_absolute_time();
delta = now - last_report_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 || (delta > FMU_MAX_REPORT_INTERVAL))) {
@ -132,6 +134,7 @@ comms_main(void)
/* populate the report */ /* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++) for (unsigned i = 0; i < system_state.rc_channels; i++)
report.rc_channel[i] = system_state.rc_channel_data[i]; report.rc_channel[i] = system_state.rc_channel_data[i];
report.channel_count = system_state.rc_channels; report.channel_count = system_state.rc_channels;
report.armed = system_state.armed; 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]; 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 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; system_state.armed = false;
} }
@ -204,14 +207,17 @@ comms_handle_frame(void *arg, const void *buffer, size_t length)
case F2I_MAGIC: case F2I_MAGIC:
comms_handle_command(buffer, length); comms_handle_command(buffer, length);
break; break;
case F2I_CONFIG_MAGIC: case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length); comms_handle_config(buffer, length);
break; break;
case F2I_MIXER_MAGIC: case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length); mixer_handle_text(buffer, length);
break; break;
default: default:
frame_bad++; frame_bad++;
break; break;
} }
} }

2
apps/px4io/controls.c
Unescape View File

@ -90,6 +90,7 @@ controls_main(void)
if (fds[0].revents & POLLIN) if (fds[0].revents & POLLIN)
locked |= dsm_input(); locked |= dsm_input();
if (fds[1].revents & POLLIN) if (fds[1].revents & POLLIN)
locked |= sbus_input(); locked |= sbus_input();
@ -139,6 +140,7 @@ ppm_input(void)
/* PPM data exists, copy it */ /* PPM data exists, copy it */
system_state.rc_channels = ppm_decoded_channels; system_state.rc_channels = ppm_decoded_channels;
for (unsigned i = 0; i < ppm_decoded_channels; i++) for (unsigned i = 0; i < ppm_decoded_channels; i++)
system_state.rc_channel_data[i] = ppm_buffer[i]; system_state.rc_channel_data[i] = ppm_buffer[i];

24
apps/px4io/dsm.c
Unescape View File

@ -97,6 +97,7 @@ dsm_init(const char *device)
dsm_guess_format(true); dsm_guess_format(true);
debug("DSM: ready"); debug("DSM: ready");
} else { } else {
debug("DSM: open failed"); debug("DSM: open failed");
} }
@ -126,6 +127,7 @@ dsm_input(void)
* if we didn't drop bytes... * if we didn't drop bytes...
*/ */
now = hrt_absolute_time(); now = hrt_absolute_time();
if ((now - last_rx_time) > 5000) { if ((now - last_rx_time) > 5000) {
if (partial_frame_count > 0) { if (partial_frame_count > 0) {
dsm_frame_drops++; dsm_frame_drops++;
@ -142,6 +144,7 @@ dsm_input(void)
/* if the read failed for any reason, just give up here */ /* if the read failed for any reason, just give up here */
if (ret < 1) if (ret < 1)
goto out; goto out;
last_rx_time = now; last_rx_time = now;
/* /*
@ -153,7 +156,7 @@ dsm_input(void)
* If we don't have a full frame, return * If we don't have a full frame, return
*/ */
if (partial_frame_count < DSM_FRAME_SIZE) if (partial_frame_count < DSM_FRAME_SIZE)
goto out; goto out;
/* /*
* Great, it looks like we might have a frame. Go ahead and * Great, it looks like we might have a frame. Go ahead and
@ -212,6 +215,7 @@ dsm_guess_format(bool reset)
/* if the channel decodes, remember the assigned number */ /* if the channel decodes, remember the assigned number */
if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31))
cs10 |= (1 << channel); cs10 |= (1 << channel);
if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
cs11 |= (1 << channel); cs11 |= (1 << channel);
@ -247,14 +251,17 @@ dsm_guess_format(bool reset)
if (cs10 == masks[i]) if (cs10 == masks[i])
votes10++; votes10++;
if (cs11 == masks[i]) if (cs11 == masks[i])
votes11++; votes11++;
} }
if ((votes11 == 1) && (votes10 == 0)) { if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11; channel_shift = 11;
debug("DSM: detected 11-bit format"); debug("DSM: detected 11-bit format");
return; return;
} }
if ((votes10 == 1) && (votes11 == 0)) { if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10; channel_shift = 10;
debug("DSM: detected 10-bit format"); debug("DSM: detected 10-bit format");
@ -270,11 +277,11 @@ static void
dsm_decode(hrt_abstime frame_time) 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", 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[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]); 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 * If we have lost signal for at least a second, reset the
* format guessing heuristic. * format guessing heuristic.
@ -322,6 +329,7 @@ dsm_decode(hrt_abstime frame_time)
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11) if (channel_shift == 11)
value /= 2; value /= 2;
value += 998; value += 998;
/* /*
@ -335,14 +343,18 @@ dsm_decode(hrt_abstime frame_time)
case 0: case 0:
channel = 2; channel = 2;
break; break;
case 1: case 1:
channel = 0; channel = 0;
break; break;
case 2: case 2:
channel = 1; channel = 1;
default: default:
break; break;
} }
system_state.rc_channel_data[channel] = value; system_state.rc_channel_data[channel] = value;
} }

13
apps/px4io/mixer.cpp
Unescape View File

@ -69,9 +69,9 @@ static uint16_t *control_values;
static int control_count; static int control_count;
static int mixer_callback(uintptr_t handle, static int mixer_callback(uintptr_t handle,
uint8_t control_group, uint8_t control_group,
uint8_t control_index, uint8_t control_index,
float &control); float &control);
static MixerGroup mixer_group(mixer_callback, 0); static MixerGroup mixer_group(mixer_callback, 0);
@ -96,6 +96,7 @@ mixer_tick(void)
if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) { if (fmu_input_drops >= FMU_INPUT_DROP_LIMIT) {
system_state.mixer_use_fmu = false; system_state.mixer_use_fmu = false;
} }
} else { } else {
fmu_input_drops = 0; fmu_input_drops = 0;
system_state.fmu_data_received = false; system_state.fmu_data_received = false;
@ -127,6 +128,7 @@ mixer_tick(void)
if (i < mixed) { if (i < mixed) {
/* scale to servo output */ /* scale to servo output */
system_state.servos[i] = (outputs[i] * 500.0f) + 1500; system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
} else { } else {
/* set to zero to inhibit PWM pulse output */ /* set to zero to inhibit PWM pulse output */
system_state.servos[i] = 0; system_state.servos[i] = 0;
@ -144,6 +146,7 @@ mixer_tick(void)
* Decide whether the servos should be armed right now. * Decide whether the servos should be armed right now.
*/ */
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0); should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
if (should_arm && !mixer_servos_armed) { if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */ /* need to arm, but not armed */
up_pwm_servo_arm(true); up_pwm_servo_arm(true);
@ -180,6 +183,7 @@ mixer_handle_text(const void *buffer, size_t length)
static unsigned mixer_text_length = 0; static unsigned mixer_text_length = 0;
px4io_mixdata *msg = (px4io_mixdata *)buffer; px4io_mixdata *msg = (px4io_mixdata *)buffer;
if (length < sizeof(px4io_mixdata)) if (length < sizeof(px4io_mixdata))
return; return;
@ -189,8 +193,10 @@ mixer_handle_text(const void *buffer, size_t length)
case F2I_MIXER_ACTION_RESET: case F2I_MIXER_ACTION_RESET:
mixer_group.reset(); mixer_group.reset();
mixer_text_length = 0; mixer_text_length = 0;
/* FALLTHROUGH */ /* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND: case F2I_MIXER_ACTION_APPEND:
/* check for overflow - this is really fatal */ /* check for overflow - this is really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
return; 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 */ /* copy any leftover text to the base of the buffer for re-use */
if (mixer_text_length > 0) if (mixer_text_length > 0)
memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length); memcpy(&mixer_text[0], end - mixer_text_length, mixer_text_length);
break; break;
} }
} }

13
apps/px4io/px4io.h
Unescape View File

@ -31,11 +31,11 @@
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file px4io.h * @file px4io.h
* *
* General defines and structures for the PX4IO module firmware. * General defines and structures for the PX4IO module firmware.
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
@ -66,8 +66,7 @@
/* /*
* System state structure. * System state structure.
*/ */
struct sys_state_s struct sys_state_s {
{
bool armed; /* IO armed */ bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */ bool arm_ok; /* FMU says OK to arm */

20
apps/px4io/safety.c
Unescape View File

@ -31,9 +31,9 @@
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file Safety button logic. * @file Safety button logic.
*/ */
#include <nuttx/config.h> #include <nuttx/config.h>
#include <stdio.h> #include <stdio.h>
@ -101,7 +101,7 @@ safety_check_button(void *arg)
*/ */
safety_button_pressed = BUTTON_SAFETY; 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); //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 (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) { if (counter < ARM_COUNTER_THRESHOLD) {
counter++; counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) { } else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */ /* change to armed state and notify the FMU */
system_state.armed = true; system_state.armed = true;
counter++; counter++;
system_state.fmu_report_due = true; system_state.fmu_report_due = true;
} }
/* Disarm quickly */
/* Disarm quickly */
} else if (safety_button_pressed && system_state.armed) { } else if (safety_button_pressed && system_state.armed) {
if (counter < DISARM_COUNTER_THRESHOLD) { if (counter < DISARM_COUNTER_THRESHOLD) {
counter++; counter++;
} else if (counter == DISARM_COUNTER_THRESHOLD) { } else if (counter == DISARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */ /* change to disarmed state and notify the FMU */
system_state.armed = false; system_state.armed = false;
counter++; counter++;
system_state.fmu_report_due = true; system_state.fmu_report_due = true;
} }
} else { } else {
counter = 0; counter = 0;
} }
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE; uint16_t pattern = LED_PATTERN_SAFE;
if (system_state.armed) { if (system_state.armed) {
if (system_state.arm_ok) { if (system_state.arm_ok) {
pattern = LED_PATTERN_IO_FMU_ARMED; pattern = LED_PATTERN_IO_FMU_ARMED;
} else { } else {
pattern = LED_PATTERN_IO_ARMED; pattern = LED_PATTERN_IO_ARMED;
} }
} else if (system_state.arm_ok) { } else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED; pattern = LED_PATTERN_FMU_ARMED;
} }
@ -167,8 +175,10 @@ failsafe_blink(void *arg)
/* blink the failsafe LED if we don't have FMU input */ /* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_use_fmu) { if (!system_state.mixer_use_fmu) {
failsafe = !failsafe; failsafe = !failsafe;
} else { } else {
failsafe = false; failsafe = false;
} }
LED_AMBER(failsafe); LED_AMBER(failsafe);
} }

40
apps/px4io/sbus.c
Unescape View File

@ -88,6 +88,7 @@ sbus_init(const char *device)
last_rx_time = hrt_absolute_time(); last_rx_time = hrt_absolute_time();
debug("Sbus: ready"); debug("Sbus: ready");
} else { } else {
debug("Sbus: open failed"); debug("Sbus: open failed");
} }
@ -117,6 +118,7 @@ sbus_input(void)
* if we didn't drop bytes... * if we didn't drop bytes...
*/ */
now = hrt_absolute_time(); now = hrt_absolute_time();
if ((now - last_rx_time) > 3000) { if ((now - last_rx_time) > 3000) {
if (partial_frame_count > 0) { if (partial_frame_count > 0) {
sbus_frame_drops++; sbus_frame_drops++;
@ -133,6 +135,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */ /* if the read failed for any reason, just give up here */
if (ret < 1) if (ret < 1)
goto out; goto out;
last_rx_time = now; last_rx_time = now;
/* /*
@ -144,7 +147,7 @@ sbus_input(void)
* If we don't have a full frame, return * If we don't have a full frame, return
*/ */
if (partial_frame_count < SBUS_FRAME_SIZE) if (partial_frame_count < SBUS_FRAME_SIZE)
goto out; goto out;
/* /*
* Great, it looks like we might have a frame. Go ahead and * Great, it looks like we might have a frame. Go ahead and
@ -178,22 +181,22 @@ struct sbus_bit_pick {
uint8_t lshift; uint8_t lshift;
}; };
static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} }, /* 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} } /* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
}; };
static void static void
@ -214,7 +217,7 @@ sbus_decode(hrt_abstime frame_time)
last_frame_time = frame_time; last_frame_time = frame_time;
unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ? unsigned chancount = (PX4IO_INPUT_CHANNELS > SBUS_INPUT_CHANNELS) ?
SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS; SBUS_INPUT_CHANNELS : PX4IO_INPUT_CHANNELS;
/* use the decoder matrix to extract channel data */ /* use the decoder matrix to extract channel data */
for (unsigned channel = 0; channel < chancount; channel++) { for (unsigned channel = 0; channel < chancount; channel++) {
@ -232,6 +235,7 @@ sbus_decode(hrt_abstime frame_time)
value |= piece; value |= piece;
} }
} }
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */ /* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998; system_state.rc_channel_data[channel] = (value / 2) + 998;
} }

2
apps/systemcmds/mixer/mixer.c
Unescape View File

@ -117,6 +117,8 @@ load(const char *devname, const char *fname)
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
continue; continue;
/* XXX an optimisation here would be to strip extra whitespace */
/* if the line is too long to fit in the buffer, bail */ /* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
break; break;

30
apps/systemlib/mixer/mixer_group.cpp
Unescape View File

@ -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 */ /* use the next character as a hint to decide which mixer class to construct */
switch (*p) { switch (*p) {
case 'Z': case 'Z':
m = NullMixer::from_text(p, buflen); m = NullMixer::from_text(p, buflen);
break; break;
case 'M':
m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen); case 'M':
break; m = SimpleMixer::from_text(_control_cb, _cb_handle, p, buflen);
case 'R': break;
m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen);
break; case 'R':
default: m = MultirotorMixer::from_text(_control_cb, _cb_handle, p, buflen);
/* it's probably junk or whitespace */ break;
break;
default:
/* it's probably junk or whitespace */
break;
} }
if (m != nullptr) { if (m != nullptr) {
add_mixer(m); add_mixer(m);
ret = 0; ret = 0;
} else { } else {
/* skip whitespace or junk in the buffer */ /* skip whitespace or junk in the buffer */
buflen--; buflen--;
} }
} }
return ret; return ret;
} }

10
apps/systemlib/mixer/mixer_multirotor.cpp
Unescape View File

@ -166,10 +166,12 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
debug("multirotor parse failed on '%s'", buf); debug("multirotor parse failed on '%s'", buf);
return nullptr; return nullptr;
} }
if (used > (int)buflen) { if (used > (int)buflen) {
debug("multirotor spec used %d of %u", used, buflen); debug("multirotor spec used %d of %u", used, buflen);
return nullptr; return nullptr;
} }
buflen -= used; buflen -= used;
if (!strcmp(geomname, "4+")) { 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 */ /* keep roll, pitch and yaw control to 0 below min thrust */
if (thrust <= min_thrust) { if (thrust <= min_thrust) {
output_factor = 0.0f; 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) { } else if (thrust < startpoint_full_control && thrust > min_thrust) {
output_factor = (thrust/max_thrust)/(startpoint_full_control-min_thrust); output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
/* and then stay at full control */ /* and then stay at full control */
} else { } else {
output_factor = max_thrust; output_factor = max_thrust;
} }

Write Preview
Loading…
Cancel
Save