forked from Archive/PX4-Autopilot
Set default failsafe value to 0 of mixer
This commit is contained in:
parent
d2c60a248d
commit
f6570172da
|
@ -704,12 +704,8 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
|
|||
|
||||
unsigned max = (len < _max_actuators) ? len : _max_actuators;
|
||||
|
||||
/* set failsafe values */
|
||||
for (unsigned i = 0; i < max; i++)
|
||||
regs[i] = FLOAT_TO_REG(vals[i]);
|
||||
|
||||
/* copy values to registers in IO */
|
||||
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max);
|
||||
return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, max);
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -1744,11 +1740,28 @@ px4io_main(int argc, char *argv[])
|
|||
|
||||
if (!strcmp(argv[1], "failsafe")) {
|
||||
|
||||
/* XXX parse arguments here */
|
||||
if (argc < 3) {
|
||||
errx(1, "failsafe command needs at least one channel value (ppm)");
|
||||
}
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
/* XXX testing values */
|
||||
uint16_t failsafe[4] = {1500, 1500, 1200, 1200};
|
||||
|
||||
/* set values for first 8 channels, fill unassigned channels with 1500. */
|
||||
uint16_t failsafe[8];
|
||||
|
||||
for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++)
|
||||
{
|
||||
/* set channel to commanline argument or to 900 for non-provided channels */
|
||||
if (argc > i + 2) {
|
||||
failsafe[i] = atoi(argv[i+2]);
|
||||
if (failsafe[i] < 800 || failsafe[i] > 2200) {
|
||||
errx(1, "value out of range of 800 < value < 2200. Aborting.");
|
||||
}
|
||||
} else {
|
||||
failsafe[i] = 1500;
|
||||
}
|
||||
}
|
||||
|
||||
g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0]));
|
||||
} else {
|
||||
errx(1, "not loaded");
|
||||
|
|
|
@ -85,6 +85,9 @@ static int mixer_callback(uintptr_t handle,
|
|||
|
||||
static MixerGroup mixer_group(mixer_callback, 0);
|
||||
|
||||
/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */
|
||||
static void mixer_set_failsafe();
|
||||
|
||||
void
|
||||
mixer_tick(void)
|
||||
{
|
||||
|
@ -243,6 +246,7 @@ mixer_callback(uintptr_t handle,
|
|||
|
||||
case MIX_FAILSAFE:
|
||||
case MIX_NONE:
|
||||
control = 0.0f;
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
@ -320,8 +324,41 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||
memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
|
||||
|
||||
mixer_text_length = resid;
|
||||
|
||||
/* update failsafe values */
|
||||
mixer_set_failsafe();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
static void
|
||||
mixer_set_failsafe()
|
||||
{
|
||||
/*
|
||||
* Check if a custom failsafe value has been written,
|
||||
* else use the opportunity to set decent defaults.
|
||||
*/
|
||||
if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
|
||||
return;
|
||||
|
||||
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++) {
|
||||
|
||||
/* scale to servo output */
|
||||
r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500;
|
||||
|
||||
}
|
||||
|
||||
/* disable the rest of the outputs */
|
||||
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
|
||||
r_page_servo_failsafe[i] = 0;
|
||||
|
||||
}
|
||||
|
|
|
@ -231,11 +231,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
|
|||
case PX4IO_PAGE_FAILSAFE_PWM:
|
||||
|
||||
/* copy channel data */
|
||||
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
|
||||
while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
|
||||
|
||||
/* XXX range-check value? */
|
||||
r_page_servo_failsafe[offset] = *values;
|
||||
|
||||
/* flag the failsafe values as custom */
|
||||
r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM;
|
||||
|
||||
offset++;
|
||||
num_values--;
|
||||
values++;
|
||||
|
|
Loading…
Reference in New Issue