Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Allow fully independent operation of two AP_Mount instances

Browse Source
This commit is contained in:
Amilcar Lucas 2012-08-08 23:11:23 +02:00
parent f321f5d9c5
commit 1683f18bff
4 changed files with 33 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/ArduCopter.pde
View File

@ -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(&current_loc, g_gps, &ahrs);
AP_Mount camera_mount(&current_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(&current_loc, g_gps, &ahrs);
AP_Mount camera_mount2(&current_loc, g_gps, &ahrs, 1);
#endif
#if CAMERA == ENABLED

30
libraries/AP_Mount/AP_Mount.cpp
View File

@ -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)

7
libraries/AP_Mount/AP_Mount.h
View File

@ -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

5
libraries/RC_Channel/RC_Channel_aux.h
View File

@ -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;