Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mount: added set_manual_rc_channel and set_manual_rc_channel_function to allow input of pitch, roll and yaw controls from a regular RC_Channel (i.e. not RC_Channel_aux) 

For now, angles red in are always of the range -45 deg ~ 45 deg which is consistent with the existing arducopter implementation but should be improved to scale the pilot's input to the mount's actual range.
This commit is contained in:
rmackay9 2012-07-18 23:20:05 +09:00
parent 83d49dbb88
commit e61cf0e1f9
2 changed files with 66 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
libraries/AP_Mount/AP_Mount.cpp
View File

@ -78,6 +78,10 @@ _gps(gps)
_retract_angles = Vector3f(0,0,0);
_neutral_angles = Vector3f(0,0,0);
_control_angles = Vector3f(0,0,0);
// default manual rc channel to disabled
_manual_rc = NULL;
_manual_rc_function = AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED;
}
//sets the servo angles for retraction, note angles are in degrees
@ -150,12 +154,32 @@ void AP_Mount::update_mount_position()
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();
// allow pilot input to come directly from an RC_Channel
if( _manual_rc != NULL ) {
float manual_angle = radians((float)(4500 - _manual_rc->control_in ) / 100.0);
switch( _manual_rc_function ) {
case AP_MOUNT_MANUAL_RC_FUNCTION_ROLL:
_roll_control_angle = manual_angle;
break;
case AP_MOUNT_MANUAL_RC_FUNCTION_PITCH:
_pitch_control_angle = manual_angle;
break;
case AP_MOUNT_MANUAL_RC_FUNCTION_YAW:
_yaw_control_angle = manual_angle;
break;
default:
// do nothing
break;
}
}else{
// take pilot's input from RC_Channel_aux
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;
@ -376,6 +400,28 @@ AP_Mount::stabilize()
}
}
// set_manual_rc_channel - define which RC_Channel is to be used for manual control
void
AP_Mount::set_manual_rc_channel(RC_Channel* rc)
{
_manual_rc = rc;
}
// set_manual_rc_channel_function - set whether manual rc channel is disabled, controls roll (1), pitch (2) or yaw (3).
void
AP_Mount::set_manual_rc_channel_function(int8_t fn)
{
// update scaler if the function has changed
if( _manual_rc_function != fn ) {
_manual_rc_function = fn;
// ensure pilot's input appears in the range 0 ~ 9000 ( 90 degrees * 100 ). We will subtract 45 degrees later to make the range +-45 degrees
if( _manual_rc != NULL && fn != AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED ) {
_manual_rc->set_range( 0, 9000 );
}
}
}
// For testing and development. Called in the medium loop.
void
AP_Mount::debug_output()

15
libraries/AP_Mount/AP_Mount.h
View File

@ -29,6 +29,12 @@
#include <GCS_MAVLink.h>
#include <../RC_Channel/RC_Channel_aux.h>
// #defines to control function of RC Channel used to manually provide angular offset to AP_Mount when we can't use RC_Channel_aux (which is the case for ArduCopter).
#define AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED 0
#define AP_MOUNT_MANUAL_RC_FUNCTION_ROLL 1
#define AP_MOUNT_MANUAL_RC_FUNCTION_PITCH 2
#define AP_MOUNT_MANUAL_RC_FUNCTION_YAW 3
class AP_Mount
{
public:
@ -46,7 +52,10 @@ public:
// should be called periodically
void update_mount_position();
void debug_output(); ///< For testing and development. Called in the medium loop.
// Accessors
// to allow manual input of an angle from the pilot when RC_Channel_aux cannot be used
void set_manual_rc_channel(RC_Channel *rc); // define which RC_Channel is to be used for manual control
void set_manual_rc_channel_function(int8_t fn); // set whether manual rc channel controlls roll (1), pitch (2) or yaw (3).
// hook for eeprom variables
static const struct AP_Param::GroupInfo var_info[];
@ -89,5 +98,9 @@ private:
AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
// RC_Channel for providing direct angular input from pilot
RC_Channel* _manual_rc;
int8_t _manual_rc_function;
};
#endif