From 8bb9707f3fc5a87213bb567a61416d9d90e6b239 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:38 +0200 Subject: [PATCH 01/10] FMU: Allow to pre-arm the non-throttle channels with the safety switch --- src/drivers/px4fmu/fmu.cpp | 60 +++++++++++++++++++++++--------------- 1 file changed, 36 insertions(+), 24 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 2047046b9b..b1766f7390 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -90,6 +90,7 @@ */ #define CONTROL_INPUT_DROP_LIMIT_MS 20 +#define NAN_VALUE (0.0f/0.0f) class PX4FMU : public device::CDev { @@ -136,7 +137,6 @@ private: int _armed_sub; int _param_sub; orb_advert_t _outputs_pub; - actuator_armed_s _armed; unsigned _num_outputs; int _class_instance; @@ -156,6 +156,7 @@ private: unsigned _poll_fds_num; static pwm_limit_t _pwm_limit; + static actuator_armed_s _armed; uint16_t _failsafe_pwm[_max_actuators]; uint16_t _disarmed_pwm[_max_actuators]; uint16_t _min_pwm[_max_actuators]; @@ -164,6 +165,8 @@ private: unsigned _num_failsafe_set; unsigned _num_disarmed_set; + static bool arm_nothrottle() { return (_armed.ready_to_arm && !_armed.armed); } + static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -240,8 +243,9 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { #endif }; -const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); -pwm_limit_t PX4FMU::_pwm_limit; +const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); +pwm_limit_t PX4FMU::_pwm_limit; +actuator_armed_s PX4FMU::_armed = {}; namespace { @@ -261,7 +265,6 @@ PX4FMU::PX4FMU() : _armed_sub(-1), _param_sub(-1), _outputs_pub(-1), - _armed{}, _num_outputs(0), _class_instance(0), _task_should_exit(false), @@ -695,24 +698,17 @@ PX4FMU::task_main() outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); outputs.timestamp = hrt_absolute_time(); - /* iterate actuators */ + /* disable unused ports by setting their output to NaN */ for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN and INF */ - if ((i >= outputs.noutputs) || - !isfinite(outputs.output[i])) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; + if (i >= outputs.noutputs) { + outputs.output[i] = NAN_VALUE; } } uint16_t pwm_limited[num_outputs]; - /* the PWM limit call takes care of out of band errors and constrains */ - pwm_limit_calc(_servo_armed, num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + /* the PWM limit call takes care of out of band errors, NaN and constrains */ + pwm_limit_calc(_servo_armed, arm_nothrottle(), num_outputs, _reverse_pwm_mask, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); /* output to the servos */ for (unsigned i = 0; i < num_outputs; i++) { @@ -737,13 +733,14 @@ PX4FMU::task_main() orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; + bool set_armed = (_armed.armed || _armed.ready_to_arm) && !_armed.lockdown; - if (_servo_armed != set_armed) + if (_servo_armed != set_armed) { _servo_armed = set_armed; + } /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + bool pwm_on = (set_armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; @@ -828,6 +825,13 @@ PX4FMU::control_callback(uintptr_t handle, input = controls[control_group].control[control_index]; + /* limit control input */ + if (input > 1.0f) { + input = 1.0f; + } else if (input < -1.0f) { + input = -1.0f; + } + /* motor spinup phase - lock throttle to zero */ if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && @@ -839,6 +843,15 @@ PX4FMU::control_callback(uintptr_t handle, } } + /* throttle not arming - mark throttle input as invalid */ + if (arm_nothrottle()) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* set the throttle to an invalid value */ + input = NAN_VALUE; + } + } + return 0; } @@ -847,14 +860,12 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - // XXX disabled, confusing users - //debug("ioctl 0x%04x 0x%08x", cmd, arg); - /* try it as a GPIO ioctl first */ ret = gpio_ioctl(filp, cmd, arg); - if (ret != -ENOTTY) + if (ret != -ENOTTY) { return ret; + } /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch (_mode) { @@ -873,8 +884,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) } /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } From 0ca6f46ef497419d50cae2cc14be061aafe8ed73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:51:59 +0200 Subject: [PATCH 02/10] IO: Allow to pre-arm the non-throttle channels with the safety switch --- src/modules/px4iofirmware/mixer.cpp | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2cfdf491b..050750d080 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -62,6 +62,7 @@ extern "C" { * Maximum interval in us before FMU signal is considered lost */ #define FMU_INPUT_DROP_LIMIT_US 500000 +#define NAN_VALUE (0.0f/0.0f) /* current servo arm/disarm state */ static bool mixer_servos_armed = false; @@ -243,12 +244,12 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = 0; - outputs[i] = 0; + outputs[i] = 0.0f; } /* store normalized outputs */ @@ -369,7 +370,14 @@ mixer_callback(uintptr_t handle, } } - /* motor spinup phase or only safety off, but not armed - lock throttle to zero */ + /* limit output */ + if (control > 1.0f) { + control = 1.0f; + } else if (control < -1.0f) { + control = -1.0f; + } + + /* motor spinup phase - lock throttle to zero */ if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_THROTTLE) { @@ -380,11 +388,13 @@ mixer_callback(uintptr_t handle, } } - /* limit output */ - if (control > 1.0f) { - control = 1.0f; - } else if (control < -1.0f) { - control = -1.0f; + /* only safety off, but not armed - set throttle as invalid */ + if (should_arm_nothrottle && !should_arm) { + if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + /* mark the throttle as invalid */ + control = NAN_VALUE; + } } return 0; From 433c9bf42d55165c9e45616514cda2765217af63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 09:52:15 +0200 Subject: [PATCH 03/10] PWM limit lib: Support pre-arming --- src/modules/systemlib/pwm_limit/pwm_limit.c | 28 ++++++++++++++++++--- src/modules/systemlib/pwm_limit/pwm_limit.h | 2 +- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 2f72d347c6..09965b96b9 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -54,7 +54,7 @@ void pwm_limit_init(pwm_limit_t *limit) return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t reverse_mask, +void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { @@ -99,6 +99,16 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ break; } + /* if the system is pre-armed, the limit state is temporarily on, + * as some outputs are valid and the non-valid outputs have been + * set to NaN. This is not stored in the state machine though, + * as the throttle channels need to go through the ramp at + * regular arming time. + */ + if (pre_armed) { + limit->state = PWM_LIMIT_STATE_ON; + } + unsigned progress; /* then set effective_pwm based on state */ @@ -120,6 +130,14 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } for (unsigned i=0; i Date: Sat, 4 Jul 2015 11:35:11 +0200 Subject: [PATCH 04/10] Mixer test: Add routine to test pre-arming --- src/systemcmds/tests/test_mixer.cpp | 64 +++++++++++++++++++++++------ 1 file changed, 52 insertions(+), 12 deletions(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index acde4a1a50..20b77f1e27 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -56,6 +56,8 @@ #include #include +#include + #include "tests.h" static int mixer_callback(uintptr_t handle, @@ -65,6 +67,9 @@ static int mixer_callback(uintptr_t handle, const unsigned output_max = 8; static float actuator_controls[output_max]; +static bool should_prearm = false; + +#define NAN_VALUE 0.0f/0.0f int test_mixer(int argc, char *argv[]) { @@ -72,7 +77,7 @@ int test_mixer(int argc, char *argv[]) * PWM limit structure */ pwm_limit_t pwm_limit; - static bool should_arm = false; + bool should_arm = false; uint16_t r_page_servo_disarmed[output_max]; uint16_t r_page_servo_control_min[output_max]; uint16_t r_page_servo_control_max[output_max]; @@ -184,7 +189,6 @@ int test_mixer(int argc, char *argv[]) const int jmax = 5; pwm_limit_init(&pwm_limit); - should_arm = true; /* run through arming phase */ for (unsigned i = 0; i < output_max; i++) { @@ -194,6 +198,35 @@ int test_mixer(int argc, char *argv[]) r_page_servo_control_max[i] = PWM_DEFAULT_MAX; } + warnx("PRE-ARM TEST: DISABLING SAFETY"); + /* mix */ + should_prearm = true; + mixed = mixer_group.mix(&outputs[0], output_max, NULL); + + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + + //warnx("mixed %d outputs (max %d), values:", mixed, output_max); + for (unsigned i = 0; i < mixed; i++) { + + warnx("pre-arm:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + + if (i != actuator_controls_s::INDEX_THROTTLE) { + if (r_page_servos[i] < r_page_servo_control_min[i]) { + warnx("active servo < min"); + return 1; + } + } else { + if (r_page_servos[i] != r_page_servo_disarmed[i]) { + warnx("throttle output != 0 (this check assumed the IO pass mixer!)"); + return 1; + } + } + } + + should_arm = true; + should_prearm = false; + warnx("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; @@ -205,11 +238,14 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { @@ -222,8 +258,6 @@ int test_mixer(int argc, char *argv[]) warnx("ramp servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -251,7 +285,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); @@ -278,18 +312,19 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); for (unsigned i = 0; i < mixed; i++) { + + warnx("disarmed:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { warnx("disarmed servo value mismatch"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -314,7 +349,7 @@ int test_mixer(int argc, char *argv[]) /* mix */ mixed = mixer_group.mix(&outputs[0], output_max, NULL); - pwm_limit_calc(should_arm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, + pwm_limit_calc(should_arm, should_prearm, mixed, reverse_pwm_mask, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); @@ -324,6 +359,8 @@ int test_mixer(int argc, char *argv[]) /* check ramp */ + warnx("ramp:\t %d: out: %8.4f, servo: %d", i, (double)outputs[i], (int)r_page_servos[i]); + if (hrt_elapsed_time(&starttime) < RAMP_TIME_US && (r_page_servos[i] + 1 <= r_page_servo_disarmed[i] || r_page_servos[i] > servo_predicted[i])) { @@ -338,8 +375,6 @@ int test_mixer(int argc, char *argv[]) warnx("mixer violated predicted value"); return 1; } - - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); @@ -397,5 +432,10 @@ mixer_callback(uintptr_t handle, control = actuator_controls[control_index]; + if (should_prearm && control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && + control_index == actuator_controls_s::INDEX_THROTTLE) { + control = NAN_VALUE; + } + return 0; } From ef4946f81bb0ab897e4b7ade94d39cc8423fc632 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Jul 2015 11:38:25 +0200 Subject: [PATCH 05/10] PWM limit: Avoid writing back into state struct --- src/modules/systemlib/pwm_limit/pwm_limit.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 09965b96b9..8f2cec6fc4 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -105,14 +105,17 @@ void pwm_limit_calc(const bool armed, const bool pre_armed, const unsigned num_c * as the throttle channels need to go through the ramp at * regular arming time. */ + + unsigned local_limit_state = limit->state; + if (pre_armed) { - limit->state = PWM_LIMIT_STATE_ON; + local_limit_state = PWM_LIMIT_STATE_ON; } unsigned progress; /* then set effective_pwm based on state */ - switch (limit->state) { + switch (local_limit_state) { case PWM_LIMIT_STATE_OFF: case PWM_LIMIT_STATE_INIT: for (unsigned i=0; i Date: Sat, 4 Jul 2015 11:47:55 +0200 Subject: [PATCH 06/10] Tests: Reset mixer inputs --- src/systemcmds/tests/test_mixer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 20b77f1e27..3b2f42b21d 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -227,6 +227,11 @@ int test_mixer(int argc, char *argv[]) should_arm = true; should_prearm = false; + /* simulate another orb_copy() from actuator controls */ + for (unsigned i = 0; i < output_max; i++) { + actuator_controls[i] = 0.1f; + } + warnx("ARMING TEST: STARTING RAMP"); unsigned sleep_quantum_us = 10000; @@ -249,7 +254,7 @@ int test_mixer(int argc, char *argv[]) /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && r_page_servos[i] != r_page_servo_disarmed[i]) { - warnx("disarmed servo value mismatch"); + warnx("disarmed servo value mismatch: %d vs %d", r_page_servos[i], r_page_servo_disarmed[i]); return 1; } From 7b14a0258e8edefbc5b5eda1a86df925fa0d039d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 09:50:07 +0200 Subject: [PATCH 07/10] pwm limit: Fix author list --- src/modules/systemlib/pwm_limit/pwm_limit.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index 8f2cec6fc4..cf71d7e335 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -37,6 +37,7 @@ * Library for PWM output limiting * * @author Julian Oes + * @author Lorenz Meier */ #include "pwm_limit.h" From 87b801034fdc897095bcdc959ae31225f3ed805b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 09:50:44 +0200 Subject: [PATCH 08/10] IO firmware: Fix condition for output enable to also allow no throttle arming to enable outputs --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 050750d080..1fa327613e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -281,7 +281,7 @@ mixer_tick(void) isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed && should_arm) { + if (mixer_servos_armed && (should_arm || should_arm_nothrottle)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); From 1795962328ffdaed093495409d09ad4bf374bf8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 10:11:54 +0200 Subject: [PATCH 09/10] Default Skywalker mixer to wing wing gains --- ROMFS/px4fmu_common/init.d/3032_skywalker_x5 | 22 +++++++++----------- 1 file changed, 10 insertions(+), 12 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 index 3d464a4ae9..4950c3183f 100644 --- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 +++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 @@ -14,18 +14,16 @@ then param set FW_AIRSPD_MAX 40 param set FW_ATT_TC 0.3 param set FW_L1_DAMPING 0.74 - param set FW_L1_PERIOD 15 - param set FW_PR_FF 0.3 - param set FW_PR_I 0 - param set FW_PR_IMAX 0.2 - param set FW_PR_P 0.03 - param set FW_P_ROLLFF 1 - param set FW_RR_FF 0.3 - param set FW_RR_I 0 - param set FW_RR_IMAX 0.2 - param set FW_RR_P 0.03 - param set FW_R_LIM 60 - param set FW_R_RMAX 0 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_RR_FF 0.6 + param set FW_RR_P 0.04 fi set MIXER X5 From c05c5bfceb70c6947d9f35c234d68187647af91b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 7 Jul 2015 10:12:23 +0200 Subject: [PATCH 10/10] Multicopters: Load gimbal mixer by default --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index a5c326ebc6..6506ed8c3e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -20,3 +20,11 @@ set PWM_RATE 400 set PWM_DISARMED 900 set PWM_MIN 1075 set PWM_MAX 2000 + +# This is the gimbal pass mixer +set MIXER_AUX pass +set PWM_AUX_RATE 50 +set PWM_AUX_OUT 1234 +set PWM_AUX_DISARMED 1500 +set PWM_AUX_MIN 1000 +set PWM_AUX_MAX 2000