From 08bbd6dbfaff58524928437a56fd7976d69fa2f5 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 12 Jul 2016 13:48:57 +0200 Subject: [PATCH] px4iofirmware: added scale parameters for roll, pitch and yaw since pure manual control for fixed wings in handled on the io side the scale parameters for roll, pitch and yaw had to be introduced there as well. Signed-off-by: Roman --- src/modules/px4iofirmware/mixer.cpp | 6 +++--- src/modules/px4iofirmware/protocol.h | 5 ++++- src/modules/px4iofirmware/px4io.h | 3 +++ src/modules/px4iofirmware/registers.c | 8 +++++++- 4 files changed, 17 insertions(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0ff0eb4892..f41a80ef66 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -373,15 +373,15 @@ mixer_callback(uintptr_t handle, if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_ROLL) { - control += REG_TO_FLOAT(r_setup_trim_roll); + control *= REG_TO_FLOAT(r_setup_scale_roll) + REG_TO_FLOAT(r_setup_trim_roll); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_PITCH) { - control += REG_TO_FLOAT(r_setup_trim_pitch); + control *= REG_TO_FLOAT(r_setup_scale_pitch) + REG_TO_FLOAT(r_setup_trim_pitch); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && control_index == actuator_controls_s::INDEX_YAW) { - control += REG_TO_FLOAT(r_setup_trim_yaw); + control *= REG_TO_FLOAT(r_setup_scale_yaw) + REG_TO_FLOAT(r_setup_trim_yaw); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 6c984ccac5..a4c9ce235a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -236,8 +236,11 @@ enum { /* DSM bind states */ #define PX4IO_P_SETUP_TRIM_ROLL 16 /**< Roll trim, in actuator units */ #define PX4IO_P_SETUP_TRIM_PITCH 17 /**< Pitch trim, in actuator units */ #define PX4IO_P_SETUP_TRIM_YAW 18 /**< Yaw trim, in actuator units */ +#define PX4IO_P_SETUP_SCALE_ROLL 19 /**< Roll scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_PITCH 20 /**< Pitch scale, in actuator units */ +#define PX4IO_P_SETUP_SCALE_YAW 21 /**< Yaw scale, in actuator units */ -#define PX4IO_P_SETUP_SBUS_RATE 19 /* frame rate of SBUS1 output in Hz */ +#define PX4IO_P_SETUP_SBUS_RATE 22 /* frame rate of SBUS1 output in Hz */ /* autopilot control values, -10000..10000 */ #define PX4IO_PAGE_CONTROLS 51 /**< actuator control groups, one after the other, 8 wide */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ed38b7769e..89dae97710 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -122,6 +122,9 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_trim_roll r_page_setup[PX4IO_P_SETUP_TRIM_ROLL] #define r_setup_trim_pitch r_page_setup[PX4IO_P_SETUP_TRIM_PITCH] #define r_setup_trim_yaw r_page_setup[PX4IO_P_SETUP_TRIM_YAW] +#define r_setup_scale_roll r_page_setup[PX4IO_P_SETUP_SCALE_ROLL] +#define r_setup_scale_pitch r_page_setup[PX4IO_P_SETUP_SCALE_PITCH] +#define r_setup_scale_yaw r_page_setup[PX4IO_P_SETUP_SCALE_YAW] #define r_setup_sbus_rate r_page_setup[PX4IO_P_SETUP_SBUS_RATE] #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3a7d90aecf..901dc061da 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -177,7 +177,10 @@ volatile uint16_t r_page_setup[] = { [PX4IO_P_SETUP_PWM_REVERSE] = 0, [PX4IO_P_SETUP_TRIM_ROLL] = 0, [PX4IO_P_SETUP_TRIM_PITCH] = 0, - [PX4IO_P_SETUP_TRIM_YAW] = 0 + [PX4IO_P_SETUP_TRIM_YAW] = 0, + [PX4IO_P_SETUP_SCALE_ROLL] = 10000, + [PX4IO_P_SETUP_SCALE_PITCH] = 10000, + [PX4IO_P_SETUP_SCALE_YAW] = 10000 }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 @@ -681,6 +684,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_TRIM_ROLL: case PX4IO_P_SETUP_TRIM_PITCH: case PX4IO_P_SETUP_TRIM_YAW: + case PX4IO_P_SETUP_SCALE_ROLL: + case PX4IO_P_SETUP_SCALE_PITCH: + case PX4IO_P_SETUP_SCALE_YAW: r_page_setup[offset] = value; break;