From cc02d85cdfbe6cac84d6386a5d3b2e299864aedd Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Tue, 10 Jul 2012 23:59:33 +0200 Subject: [PATCH] AP_Mount: add support for non-spring-loaded joysticks --- libraries/AP_Mount/AP_Mount.cpp | 10 ++++++++++ libraries/RC_Channel/RC_Channel_aux.cpp | 14 ++++++++++++++ libraries/RC_Channel/RC_Channel_aux.h | 4 ++++ 3 files changed, 28 insertions(+) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 3bedca3890..cf2c45c336 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -144,10 +144,20 @@ void AP_Mount::update_mount_position() // RC radio manual angle control, but with stabilization from the AHRS case MAV_MOUNT_MODE_RC_TARGETING: { +//#define MNT_SPRING_LOADED_JOYSTICK +#ifdef MNT_SPRING_LOADED_JOYSTICK // rc_input() takes degrees * 100 units G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle*100); G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle*100); G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle*100); +#else + if (g_rc_function[RC_Channel_aux::k_mount_roll]) + _roll_control_angle = g_rc_function[RC_Channel_aux::k_mount_roll]->angle_input_rad(); + if (g_rc_function[RC_Channel_aux::k_mount_pitch]) + _pitch_control_angle = g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_input_rad(); + if (g_rc_function[RC_Channel_aux::k_mount_yaw]) + _yaw_control_angle = g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_input_rad(); +#endif stabilize(); break; } diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index fc55d0c595..b3ee3c9020 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -84,6 +84,20 @@ RC_Channel_aux::rc_input(float *control_angle, int16_t angle) } } +/// returns the angle (degrees*100) that the RC_Channel input is receiving +long +RC_Channel_aux::angle_input() +{ + return (get_reverse()?-1:1) * ((long)radio_in - (long)radio_min) * ((long)angle_max - (long)angle_min) / ((long)radio_max - (long)radio_min) + (get_reverse()?angle_max:angle_min); +} + +/// returns the angle (radians) that the RC_Channel input is receiving +float +RC_Channel_aux::angle_input_rad() +{ + return radians(angle_input()*0.01); +} + /// map a function to a servo channel and output it void RC_Channel_aux::output_ch(unsigned char ch_nr) diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 748386f700..d253f25cce 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -55,6 +55,10 @@ public: void rc_input(float *control_angle, int16_t angle); + long angle_input(); + + float angle_input_rad(); + void output_ch(unsigned char ch_nr); static const struct AP_Param::GroupInfo var_info[];