forked from Archive/PX4-Autopilot
whitespace/formatting
This commit is contained in:
px4dev
parent
f9520ee39d
commit
d81edb09cf
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue