fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add support for raw PWM passthrough from FMU via IO.

Browse Source
This commit is contained in:
px4dev 2013-01-15 00:41:13 -08:00
parent f3a587dfce
commit 112f5ea969
4 changed files with 71 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
apps/px4io/mixer.cpp
View File

@ -81,6 +81,7 @@ static int mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control);
static void mix();
static MixerGroup mixer_group(mixer_callback, 0);
void
@ -91,7 +92,7 @@ mixer_tick(void)
/* too long without FMU input, time to go to failsafe */
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
debug("AP RX timeout");
}
@ -100,13 +101,26 @@ mixer_tick(void)
* Decide which set of controls we're using.
*/
if ((r_setup_features & PX4IO_P_FEAT_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
/* this is for planes, where manual override makes sense */
source = MIX_OVERRIDE;
/* mix from the override controls */
mix();
} else if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
source = MIX_FMU;
if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PPM) {
/* FMU has already provided PWM values */
} else {
/* mix from FMU controls */
source = MIX_FMU;
mix();
}
} else {
source = MIX_FAILSAFE;
/* XXX actually, have no idea what to do here... load hardcoded failsafe controls? */
}
#if 0
@ -173,27 +187,6 @@ mixer_tick(void)
}
}
#endif
/*
* Run the mixers.
*/
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
}
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
/*
* Update the servo outputs.
@ -219,6 +212,33 @@ mixer_tick(void)
}
}
static void
mix()
{
/*
* Run the mixers.
*/
float outputs[IO_SERVO_COUNT];
unsigned mixed;
/* mix */
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < mixed; i++) {
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
}
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
static int
mixer_callback(uintptr_t handle,
uint8_t control_group,

6
apps/px4io/protocol.h
View File

@ -100,6 +100,7 @@
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
#define PX4IO_P_STATUS_FLAGS_RAW_PPM (1 << 7) /* raw PPM from FMU is bypassing the mixer */
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
@ -149,7 +150,7 @@
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 101 /* 0..STATUS_CONTROL_COUNT */
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 102
@ -166,6 +167,9 @@
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
/* PWM output - overrides mixer */
#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
*

1
apps/px4io/px4io.c
View File

@ -115,6 +115,7 @@ int user_start(int argc, char *argv[])
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* run the mixer at 100Hz (for now...) */
/* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */
for (;;) {
poll(NULL, 0, 10);
perf_begin(mixer_perf);

22
apps/px4io/registers.c
View File

@ -152,7 +152,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX scaling - should be -10000..10000 */
/* XXX range-check value? */
r_page_controls[offset] = *values;
offset++;
@ -163,6 +163,26 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
/* XXX we should cause a mixer tick ASAP */
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PPM;
break;
/* handle raw PWM input */
case PX4IO_PAGE_DIRECT_PWM:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servos[offset] = *values;
offset++;
num_values--;
values++;
}
/* XXX need to force these values to the servos */
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PPM;
break;
/* handle text going to the mixer parser */