From 0b0b9c29a2677a2ee5e2287e4f1259b2bcf8f614 Mon Sep 17 00:00:00 2001 From: Amilcar Lucas Date: Wed, 8 Aug 2012 23:11:23 +0200 Subject: [PATCH] Allow fully independent operation of two AP_Mount instances --- ArduCopter/ArduCopter.pde | 4 ++-- libraries/AP_Mount/AP_Mount.cpp | 30 +++++++++++++++++++-------- libraries/AP_Mount/AP_Mount.h | 7 ++++++- libraries/RC_Channel/RC_Channel_aux.h | 5 ++++- 4 files changed, 33 insertions(+), 13 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 5a910f9c13..517955898e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -960,13 +960,13 @@ AP_Relay relay; #if MOUNT == ENABLED // current_loc uses the baro/gps soloution for altitude rather than gps only. // mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether? -AP_Mount camera_mount(¤t_loc, g_gps, &ahrs); +AP_Mount camera_mount(¤t_loc, g_gps, &ahrs, 0); #endif #if MOUNT2 == ENABLED // current_loc uses the baro/gps soloution for altitude rather than gps only. // mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether? -AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs); +AP_Mount camera_mount2(¤t_loc, g_gps, &ahrs, 1); #endif #if CAMERA == ENABLED diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 79e91a2e6a..c7f365fc15 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -150,7 +150,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = { extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function extern RC_Channel* rc_ch[NUM_CHANNELS]; -AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs): +AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs, uint8_t id): _gps(gps) { _ahrs = ahrs; @@ -163,21 +163,33 @@ _gps(gps) // default unknown mount type _mount_type = k_unknown; + + if (id == 0) { + _roll_idx = RC_Channel_aux::k_mount_roll; + _tilt_idx = RC_Channel_aux::k_mount_tilt; + _pan_idx = RC_Channel_aux::k_mount_pan; + _open_idx = RC_Channel_aux::k_mount_open; + } else { + _roll_idx = RC_Channel_aux::k_mount2_roll; + _tilt_idx = RC_Channel_aux::k_mount2_tilt; + _pan_idx = RC_Channel_aux::k_mount2_pan; + _open_idx = RC_Channel_aux::k_mount2_open; + } } /// Auto-detect the mount gimbal type depending on the functions assigned to the servos void AP_Mount::update_mount_type() { - if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] != NULL)) + if ((g_rc_function[_roll_idx] == NULL) && (g_rc_function[_tilt_idx] != NULL) && (g_rc_function[_pan_idx] != NULL)) { _mount_type = k_pan_tilt; } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] == NULL)) + if ((g_rc_function[_roll_idx] != NULL) && (g_rc_function[_tilt_idx] != NULL) && (g_rc_function[_pan_idx] == NULL)) { _mount_type = k_tilt_roll; } - if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_tilt] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pan] != NULL)) + if ((g_rc_function[_roll_idx] != NULL) && (g_rc_function[_tilt_idx] != NULL) && (g_rc_function[_pan_idx] != NULL)) { _mount_type = k_pan_tilt_roll; } @@ -299,18 +311,18 @@ void AP_Mount::update_mount_position() } // move mount to a "retracted position" into the fuselage with a fourth servo - if (g_rc_function[RC_Channel_aux::k_mount_open]){ + if (g_rc_function[_open_idx]){ bool mount_open_new = (enum MAV_MOUNT_MODE)_mount_mode.get()==MAV_MOUNT_MODE_RETRACT?0:1; if (mount_open != mount_open_new) { mount_open = mount_open_new; - move_servo(g_rc_function[RC_Channel_aux::k_mount_open], mount_open_new, 0, 1); + move_servo(g_rc_function[_open_idx], mount_open_new, 0, 1); } } // write the results to the servos - move_servo(g_rc_function[RC_Channel_aux::k_mount_roll], _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1); - move_servo(g_rc_function[RC_Channel_aux::k_mount_tilt], _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1); - move_servo(g_rc_function[RC_Channel_aux::k_mount_pan ], _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1); + move_servo(g_rc_function[_roll_idx], _roll_angle*10, _roll_angle_min*0.1, _roll_angle_max*0.1); + move_servo(g_rc_function[_tilt_idx], _tilt_angle*10, _tilt_angle_min*0.1, _tilt_angle_max*0.1); + move_servo(g_rc_function[_pan_idx ], _pan_angle*10, _pan_angle_min*0.1, _pan_angle_max*0.1); } void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode) diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 08a7b39297..dad74031ed 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -33,7 +33,7 @@ class AP_Mount { public: //Constructor - AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs); + AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs, uint8_t id); //enums enum MountType{ @@ -85,6 +85,11 @@ private: struct Location _target_GPS_location; MountType _mount_type; + uint8_t _roll_idx; ///< RC_Channel_aux mount roll function index + uint8_t _tilt_idx; ///< RC_Channel_aux mount tilt function index + uint8_t _pan_idx ; ///< RC_Channel_aux mount pan function index + uint8_t _open_idx; ///< RC_Channel_aux mount open function index + float _roll_control_angle; ///< radians float _tilt_control_angle; ///< radians float _pan_control_angle; ///< radians diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index b5cfd5bf81..d50c3f33ff 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -14,7 +14,6 @@ /// @class RC_Channel_aux /// @brief Object managing one aux. RC channel (CH5-8), with information about its function -/// Also contains physical min,max angular deflection, to allow calibrating open-loop servo movements class RC_Channel_aux : public RC_Channel{ public: /// Constructor @@ -40,6 +39,10 @@ public: k_mount_open = 9, ///< mount open (deploy) / close (retract) k_cam_trigger = 10, ///< camera trigger k_egg_drop = 11, ///< egg drop + k_mount2_pan = 12, ///< mount2 yaw (pan) + k_mount2_tilt = 13, ///< mount2 pitch (tilt) + k_mount2_roll = 14, ///< mount2 roll + k_mount2_open = 15, ///< mount2 open (deploy) / close (retract) k_nr_aux_servo_functions ///< This must be the last enum value (only add new values _before_ this one) } Aux_servo_function_t;