whitespace/formatting

This commit is contained in:
px4dev
2012-12-29 16:01:24 -08:00
parent f9520ee39d
commit d81edb09cf
12 changed files with 129 additions and 77 deletions
+10 -4
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,7 +124,8 @@ 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))) {
system_state.fmu_report_due = false; system_state.fmu_report_due = false;
@@ -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;
} }
@@ -185,7 +188,7 @@ comms_handle_command(const void *buffer, size_t length)
// if (!system_state.arm_ok && system_state.armed) // if (!system_state.arm_ok && system_state.armed)
// system_state.armed = false; // system_state.armed = false;
/* XXX do relay changes here */ /* XXX do relay changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
system_state.relays[i] = cmd->relay_state[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: 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;
} }
} }
+3 -1
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];
@@ -150,5 +152,5 @@ ppm_input(void)
/* trigger an immediate report to the FMU */ /* trigger an immediate report to the FMU */
system_state.fmu_report_due = true; system_state.fmu_report_due = true;
} }
} }
+26 -14
View File
@@ -47,7 +47,7 @@
#include <termios.h> #include <termios.h>
#include <systemlib/ppm_decode.h> #include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#define DEBUG #define DEBUG
@@ -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");
} }
@@ -118,7 +119,7 @@ dsm_input(void)
* frame transmission time is ~1.4ms. * frame transmission time is ~1.4ms.
* *
* We expect to only be called when bytes arrive for processing, * 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. * 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 * 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... * 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
@@ -164,7 +167,7 @@ dsm_input(void)
out: 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; return (now - last_frame_time) < 200000;
} }
@@ -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);
@@ -222,7 +226,7 @@ dsm_guess_format(bool reset)
if (samples++ < 5) if (samples++ < 5)
return; return;
/* /*
* Iterate the set of sensible sniffed channel sets and see whether * Iterate the set of sensible sniffed channel sets and see whether
* decoding in 10 or 11-bit mode has yielded anything we recognise. * 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 * See e.g. http://git.openpilot.org/cru/OPReview-116 for a discussion
* of this issue. * of this issue.
*/ */
static uint32_t masks[] = { static uint32_t masks[] = {
0x3f, /* 6 channels (DX6) */ 0x3f, /* 6 channels (DX6) */
0x7f, /* 7 channels (DX7) */ 0x7f, /* 7 channels (DX7) */
0xff, /* 8 channels (DX8) */ 0xff, /* 8 channels (DX8) */
@@ -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,13 +277,13 @@ 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",
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. * format guessing heuristic.
*/ */
if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0)) 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. * going to ignore them for now.
* *
* Each channel is a 16-bit unsigned value containing either a 10- * 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 */ /* 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;
/* /*
* Store the decoded channel into the R/C input buffer, taking into * Store the decoded channel into the R/C input buffer, taking into
* account the different ideas about channel assignement that we have. * account the different ideas about channel assignement that we have.
* *
@@ -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;
} }
+10 -3
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;
} }
} }
+2 -2
View File
@@ -60,7 +60,7 @@ struct px4io_command {
}; };
/** /**
* Periodic report from IO to FMU * Periodic report from IO to FMU
*/ */
struct px4io_report { struct px4io_report {
uint16_t i2f_magic; 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 { struct px4io_config {
uint16_t f2i_config_magic; uint16_t f2i_config_magic;
+1 -1
View File
@@ -88,7 +88,7 @@ int user_start(int argc, char *argv[])
up_pwm_servo_init(0xff); up_pwm_servo_init(0xff);
/* start the flight control signal handler */ /* start the flight control signal handler */
task_create("FCon", task_create("FCon",
SCHED_PRIORITY_DEFAULT, SCHED_PRIORITY_DEFAULT,
1024, 1024,
(main_t)controls_main, (main_t)controls_main,
+6 -7
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 */
+16 -6
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>
@@ -95,13 +95,13 @@ safety_init(void)
static void static void
safety_check_button(void *arg) safety_check_button(void *arg)
{ {
/* /*
* Debounce the safety button, change state if it has been held for long enough. * Debounce the safety button, change state if it has been held for long enough.
* *
*/ */
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);
} }
+27 -23
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");
} }
@@ -109,7 +110,7 @@ sbus_input(void)
* frame transmission time is ~2ms. * frame transmission time is ~2ms.
* *
* We expect to only be called when bytes arrive for processing, * 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. * 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 * 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... * 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
@@ -155,7 +158,7 @@ sbus_input(void)
out: 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; return (now - last_frame_time) < 200000;
} }
@@ -177,23 +180,23 @@ struct sbus_bit_pick {
uint8_t mask; uint8_t mask;
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
@@ -213,8 +216,8 @@ sbus_decode(hrt_abstime frame_time)
/* we have received something we think is a frame */ /* we have received something we think is a frame */
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;
} }
@@ -245,7 +249,7 @@ sbus_decode(hrt_abstime frame_time)
system_state.rc_channels = chancount; system_state.rc_channels = chancount;
/* and note that we have received data from the R/C controller */ /* 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 */ /* trigger an immediate report to the FMU */
system_state.fmu_report_due = true; system_state.fmu_report_due = true;
+2
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;
+19 -13
View File
@@ -91,7 +91,7 @@ MixerGroup::reset()
mixer = _first; mixer = _first;
_first = mixer->_next; _first = mixer->_next;
delete mixer; delete mixer;
} }
} }
unsigned 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 */ /* 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;
} }
+7 -3
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;
} }