From e07e4743c5bd10e582f2d64f341ad0da051a2308 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 8 Aug 2015 23:50:05 +0200 Subject: [PATCH] PX4IO: Enable S.BUS if config param is set, ignore if no param present --- src/drivers/px4io/px4io.cpp | 37 +++++++++++++++++++++++++++---------- 1 file changed, 27 insertions(+), 10 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index be3c2e1375..716a89d254 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1116,25 +1116,42 @@ PX4IO::task_main() (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask); float trim_val; - param_t trim_parm; + param_t parm_handle; - trim_parm = param_find("TRIM_ROLL"); - if (trim_parm != PARAM_INVALID) { - param_get(trim_parm, &trim_val); + parm_handle = param_find("TRIM_ROLL"); + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &trim_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val)); } - trim_parm = param_find("TRIM_PITCH"); - if (trim_parm != PARAM_INVALID) { - param_get(trim_parm, &trim_val); + parm_handle = param_find("TRIM_PITCH"); + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &trim_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val)); } - trim_parm = param_find("TRIM_YAW"); - if (trim_parm != PARAM_INVALID) { - param_get(trim_parm, &trim_val); + parm_handle = param_find("TRIM_YAW"); + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &trim_val); (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val)); } + + /* S.BUS output */ + int sbus_mode; + parm_handle = param_find("PWM_SBUS_MODE"); + if (parm_handle != PARAM_INVALID) { + param_get(parm_handle, &sbus_mode); + if (sbus_mode == 1) { + /* enable S.BUS 1 */ + (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (sbus_mode == 2) { + /* enable S.BUS 2 */ + (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { + /* disable S.BUS */ + (void)io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + } } }