From b46b1228088f558a81f435013924b168aeec4317 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jun 2015 14:16:05 +0200 Subject: [PATCH] PX4IO firmware: Support trim parameters for RPY --- src/modules/px4iofirmware/mixer.cpp | 21 +++++++++++++++++++++ src/modules/px4iofirmware/protocol.h | 3 +++ src/modules/px4iofirmware/px4io.h | 4 ++++ src/modules/px4iofirmware/registers.c | 3 +++ 4 files changed, 31 insertions(+) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1bb25104fa..596064f15f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -311,6 +311,13 @@ mixer_callback(uintptr_t handle, case MIX_FMU: if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } return -1; @@ -318,6 +325,13 @@ mixer_callback(uintptr_t handle, case MIX_OVERRIDE: if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } return -1; @@ -326,6 +340,13 @@ mixer_callback(uintptr_t handle, /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); + if (control_group == 0 && control_index == 0) { + control += REG_TO_FLOAT(r_setup_trim_roll); + } else if (control_group == 0 && control_index == 1) { + control += REG_TO_FLOAT(r_setup_trim_pitch); + } else if (control_group == 0 && control_index == 2) { + control += REG_TO_FLOAT(r_setup_trim_yaw); + } break; } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index a649920320..64ba0f93c4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -234,6 +234,9 @@ enum { /* DSM bind states */ #define PX4IO_FORCE_SAFETY_MAGIC 22027 /* required argument for force safety (random) */ #define PX4IO_P_SETUP_PWM_REVERSE 15 /**< Bitmask to reverse PWM channels 1-8 */ +#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 */ /* 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 b38d374b9a..f0c7bde5dd 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -113,6 +113,10 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #define r_setup_pwm_reverse r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] +#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_control_values (&r_page_controls[0]) /* diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 977bc59fff..3bcf3968a7 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -174,6 +174,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, [PX4IO_P_SETUP_PWM_REVERSE] = 0, + [PX4IO_P_SETUP_TRIM_ROLL] = 0, + [PX4IO_P_SETUP_TRIM_PITCH] = 0, + [PX4IO_P_SETUP_TRIM_YAW] = 0 }; #ifdef CONFIG_ARCH_BOARD_PX4IO_V2