From e4763f15f62cfefe9ea0820f22725edf0d4e2f4b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 5 Oct 2021 14:32:46 +0200 Subject: [PATCH] Mantis: add RC hacks This changes the way RC is handled for the Mantis: - The RC values are re-written when arriving over MAVLink. They are rescaled from 0..4095 to 1000..2000 and the channel bits added to separate channels. This makes the downstream handling easier. - Gimbal pitch is moved from Aux1 to Aux2 as that should be the default. - Aux3 and Aux4 are used for the photo and video trigger. - The speed button is used as the FLTMODE channel and set to switch between POSCTL and ALTCTL. --- .../init.d/airframes/4061_atl_mantis_edu | 46 ++++--------------- boards/atl/mantis-edu/src/board_config.h | 3 ++ src/modules/mavlink/mavlink_receiver.cpp | 22 +++++++++ src/modules/rc_update/rc_update.cpp | 5 ++ 4 files changed, 39 insertions(+), 37 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index b98f0ece89..a4f0f22d79 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -33,12 +33,13 @@ param set-default CBRK_SUPPLY_CHK 894281 param set-default COM_DISARM_LAND 0.1 param set-default COM_DISARM_PRFLT 3 -param set-default COM_FLTMODE1 -1 +param set-default COM_DL_LOSS_T 10 +param set-default COM_FLTMODE1 2 param set-default COM_FLTMODE2 -1 param set-default COM_FLTMODE3 -1 -param set-default COM_FLTMODE4 2 +param set-default COM_FLTMODE4 -1 param set-default COM_FLTMODE5 -1 -param set-default COM_FLTMODE6 6 +param set-default COM_FLTMODE6 1 param set-default COM_RC_LOSS_T 3 @@ -146,7 +147,7 @@ param set-default MPC_Z_VEL_P 0.27 # gimbal configuration param set-default MNT_MODE_IN 1 param set-default MNT_MODE_OUT 1 -param set-default MNT_MAN_PITCH 1 +param set-default MNT_MAN_PITCH 2 param set-default MNT_RC_IN_MODE 1 param set-default MNT_RATE_PITCH 30 @@ -157,39 +158,10 @@ param set-default RC_MAP_THROTTLE 1 param set-default RC_MAP_YAW 2 param set-default RC_MAP_PITCH 3 param set-default RC_MAP_ROLL 4 -param set-default RC_MAP_AUX1 5 -param set-default RC_MAP_FLTMODE 9 - -param set-default RC1_DZ 10 -param set-default RC1_MAX 4095 -param set-default RC1_MIN 0 -param set-default RC1_REV 1 -param set-default RC1_TRIM 0 -param set-default RC2_DZ 10 -param set-default RC2_MAX 4095 -param set-default RC2_MIN 0 -param set-default RC2_REV 1 -param set-default RC2_TRIM 2047 -param set-default RC3_DZ 10 -param set-default RC3_MAX 4095 -param set-default RC3_MIN 0 -param set-default RC3_REV 1 -param set-default RC3_TRIM 2047 -param set-default RC4_DZ 10 -param set-default RC4_MAX 4095 -param set-default RC4_MIN 0 -param set-default RC4_REV 1 -param set-default RC4_TRIM 2047 -param set-default RC5_DZ 10 -param set-default RC5_MAX 4095 -param set-default RC5_MIN 0 -param set-default RC5_REV 1 -param set-default RC5_TRIM 2047 -param set-default RC7_DZ 10 -param set-default RC7_MAX 4095 -param set-default RC7_MIN 0 -param set-default RC7_REV 1 -param set-default RC7_TRIM 2048 +param set-default RC_MAP_AUX2 5 +param set-default RC_MAP_AUX3 10 +param set-default RC_MAP_AUX4 8 +param set-default RC_MAP_FLTMODE 6 # optical flow diff --git a/boards/atl/mantis-edu/src/board_config.h b/boards/atl/mantis-edu/src/board_config.h index adb7f54d66..2342446c13 100644 --- a/boards/atl/mantis-edu/src/board_config.h +++ b/boards/atl/mantis-edu/src/board_config.h @@ -64,6 +64,9 @@ // Hacks for MAVLink RC button input #define ATL_MANTIS_RC_INPUT_HACKS +// Hacks for MAVLink RC button input +#define ATL_MANTIS_RC_INPUT_HACKS + /* * ADC channels * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index afcced5eef..24ee4bb608 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1993,6 +1993,28 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) rc.values[16] = man.chan17_raw; rc.values[17] = man.chan18_raw; +#if defined(ATL_MANTIS_RC_INPUT_HACKS) + + // Sanity checking if the RC controller is really sending. + if (rc.values[5] == 2048 && rc.values[6] == 0 && (rc.values[8] & 0xff00) == 0x0C00) { + rc.values[0] = 1000 + (uint16_t)((float)rc.values[0] / 4095.f * 1000.f); // 0..4095 -> 1000..2000 + rc.values[1] = 1000 + (uint16_t)((float)rc.values[1] / 4095.f * 1000.f); // 0..4095 -> 1000..2000 + rc.values[2] = 1000 + (uint16_t)((float)rc.values[2] / 4095.f * 1000.f); // 0..4095 -> 1000..2000 + rc.values[3] = 1000 + (uint16_t)((float)rc.values[3] / 4095.f * 1000.f); // 0..4095 -> 1000..2000 + rc.values[4] = 1000 + (uint16_t)((float)rc.values[4] / 4095.f * 1000.f); // 0..4095 -> 1000..2000 -> Aux 2 + rc.values[5] = 1000 + rc.values[7] * 500; // Switch toggles from 0 to 2, we map it from 1000 to 2000 + rc.values[6] = 1000 + (((rc.values[8] & 0x02) != 0) ? 1000 : 0); // Return button. + rc.values[7] = 1000 + (((rc.values[8] & 0x01) != 0) ? 1000 : 0); // Video record button. -> Aux4 + rc.values[9] = 1000 + (((rc.values[8] & 0x10) != 0) ? 1000 : 0); // Photo record button. -> Aux3 + rc.values[8] = rc.values[8] & 0x00ff; // Remove magic identifier. + // leave rc.values[10] as is 0..4095 + // + // Note we are currently ignoring the stick buttons (sticks pressed down). + // They are on rc.values[8] & 0x20 and rc.values[8] & 0x40. + } + +#endif + // check how many channels are valid for (int i = 17; i >= 0; i--) { const bool ignore_max = rc.values[i] == UINT16_MAX; // ignore any channel with value UINT16_MAX diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 7bbb5ea19b..9f899e5f9d 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -638,6 +638,11 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) switches.transition_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get()); switches.gear_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get()); +#if defined(ATL_MANTIS_RC_INPUT_HACKS) + switches.photo_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_3, 0.5f); + switches.video_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_4, 0.5f); +#endif + // last 2 switch updates identical (simple protection from bad RC data) if (switches == _manual_switches_previous) { const bool switches_changed = (switches != _manual_switches_last_publish);