mirror of https://github.com/ArduPilot/ardupilot
Added camera and/or antenna mount support.
It is fully configurable with the mission planner, there is no need to change the source code to adapt to your setup. It needs more testing, but the SIL is not working for me.
This commit is contained in:
Amilcar Lucas
parent
75751900d6
commit
ab730ff919
|
|
@ -42,6 +42,7 @@ version 2.1 of the License, or (at your option) any later version.
|
|||
#include <AP_RangeFinder.h> // Range finder library
|
||||
#include <ModeFilter.h>
|
||||
#include <AP_Relay.h> // APM relay
|
||||
#include <AP_Mount.h> // Camera/Antenna mount
|
||||
#include <GCS_MAVLink.h> // MAVLink GCS definitions
|
||||
#include <memcheck.h>
|
||||
|
||||
|
|
@ -404,6 +405,13 @@ static float load; // % MCU cycles used
|
|||
|
||||
AP_Relay relay;
|
||||
|
||||
// Camera/Antenna mount tracking and stabilisation stuff
|
||||
// --------------------------------------
|
||||
#if MOUNT == ENABLED
|
||||
AP_Mount camera_mount(g_gps, &dcm);
|
||||
#endif
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Top-level logic
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -529,6 +537,10 @@ static void fast_loop()
|
|||
|
||||
static void medium_loop()
|
||||
{
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update_mount_position();
|
||||
#endif
|
||||
|
||||
// This is the start of the medium (10 Hz) loop pieces
|
||||
// -----------------------------------------
|
||||
switch(medium_loopCounter) {
|
||||
|
|
@ -669,6 +681,9 @@ static void slow_loop()
|
|||
|
||||
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
camera_mount.update_mount_type();
|
||||
#endif
|
||||
break;
|
||||
|
||||
case 2:
|
||||
|
|
|
|||
|
|
@ -1871,6 +1871,26 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
}
|
||||
#endif // HIL_MODE
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
|
||||
{
|
||||
camera_mount.configure_msg(msg);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_MOUNT_CONTROL:
|
||||
{
|
||||
camera_mount.control_msg(msg);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_MOUNT_STATUS:
|
||||
{
|
||||
camera_mount.status_msg(msg);
|
||||
break;
|
||||
}
|
||||
#endif // MOUNT == ENABLED
|
||||
} // end switch
|
||||
} // end handle mavlink
|
||||
|
||||
|
|
|
|||
|
|
@ -115,6 +115,25 @@ static void handle_process_do_command()
|
|||
case MAV_CMD_DO_REPEAT_RELAY:
|
||||
do_repeat_relay();
|
||||
break;
|
||||
|
||||
#if MOUNT == ENABLED
|
||||
// Sets the region of interest (ROI) for a sensor set or the
|
||||
// vehicle itself. This can then be used by the vehicles control
|
||||
// system to control the vehicle attitude and the attitude of various
|
||||
// devices such as cameras.
|
||||
// |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
|
||||
case MAV_CMD_DO_SET_ROI:
|
||||
camera_mount.set_roi_cmd();
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
|
||||
camera_mount.configure_cmd();
|
||||
break;
|
||||
|
||||
case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|
|
||||
camera_mount.control_cmd();
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -331,6 +331,20 @@
|
|||
# define ELEVON_CH2_REVERSE DISABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// MOUNT (ANTENNA OR CAMERA)
|
||||
//
|
||||
#ifndef MOUNT
|
||||
# define MOUNT DISABLED
|
||||
#endif
|
||||
|
||||
#if defined( __AVR_ATmega1280__ ) && CAMERA == ENABLED
|
||||
// The small ATmega1280 chip does not have enough memory for camera support
|
||||
// so disable CLI, this will allow camera support and other improvements to fit.
|
||||
// This should almost have no side effects, because the APM planner can now do a complete board setup.
|
||||
#define CLI_ENABLED DISABLED
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
|
|
|||
|
|
@ -0,0 +1,296 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
#include <AP_Mount.h>
|
||||
|
||||
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||
|
||||
AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm)
|
||||
{
|
||||
_dcm=dcm;
|
||||
_dcm_hil=NULL;
|
||||
_gps=gps;
|
||||
//set_mode(MAV_MOUNT_MODE_RETRACT);
|
||||
//set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
|
||||
set_mode(MAV_MOUNT_MODE_GPS_POINT); // FIXME: this is to test ONLY targeting
|
||||
|
||||
_retract_angles.x=0;
|
||||
_retract_angles.y=0;
|
||||
_retract_angles.z=0;
|
||||
}
|
||||
|
||||
AP_Mount::AP_Mount(GPS *gps, AP_DCM_HIL *dcm)
|
||||
{
|
||||
_dcm=NULL;
|
||||
_dcm_hil=dcm;
|
||||
_gps=gps;
|
||||
//set_mode(MAV_MOUNT_MODE_RETRACT);
|
||||
//set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
|
||||
set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); // FIXME: this is to test ONLY targeting
|
||||
|
||||
_retract_angles.x=0;
|
||||
_retract_angles.y=0;
|
||||
_retract_angles.z=0;
|
||||
}
|
||||
|
||||
//sets the servo angles for retraction, note angles are * 100
|
||||
void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
|
||||
{
|
||||
_retract_angles.x=roll;
|
||||
_retract_angles.y=pitch;
|
||||
_retract_angles.z=yaw;
|
||||
}
|
||||
|
||||
//sets the servo angles for neutral, note angles are * 100
|
||||
void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw)
|
||||
{
|
||||
_neutral_angles.x=roll;
|
||||
_neutral_angles.y=pitch;
|
||||
_neutral_angles.z=yaw;
|
||||
}
|
||||
|
||||
//sets the servo angles for MAVLink, note angles are * 100
|
||||
void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw)
|
||||
{
|
||||
_mavlink_angles.x = roll;
|
||||
_mavlink_angles.y = pitch;
|
||||
_mavlink_angles.z = yaw;
|
||||
}
|
||||
|
||||
// used to tell the mount to track GPS location
|
||||
void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
|
||||
{
|
||||
_target_GPS_location=targetGPSLocation;
|
||||
}
|
||||
|
||||
// This one should be called periodically
|
||||
void AP_Mount::update_mount_position()
|
||||
{
|
||||
Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs
|
||||
Vector3f targ; //holds target vector, var is used as temp in calcs
|
||||
Vector3f aux_vec; //holds target vector, var is used as temp in calcs
|
||||
|
||||
switch(_mount_mode)
|
||||
{
|
||||
case MAV_MOUNT_MODE_RETRACT:
|
||||
roll_angle =100*_retract_angles.x;
|
||||
pitch_angle=100*_retract_angles.y;
|
||||
yaw_angle =100*_retract_angles.z;
|
||||
break;
|
||||
|
||||
case MAV_MOUNT_MODE_NEUTRAL:
|
||||
roll_angle =100*_neutral_angles.x;
|
||||
pitch_angle=100*_neutral_angles.y;
|
||||
yaw_angle =100*_neutral_angles.z;
|
||||
break;
|
||||
|
||||
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
|
||||
{
|
||||
aux_vec.x = _mavlink_angles.x;
|
||||
aux_vec.y = _mavlink_angles.y;
|
||||
aux_vec.z = _mavlink_angles.z;
|
||||
m = _dcm?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
|
||||
//rotate vector
|
||||
targ = m*aux_vec;
|
||||
// TODO The next three lines are probably not correct yet
|
||||
roll_angle = _stab_roll? degrees(atan2( targ.y,targ.z))*100:_mavlink_angles.y; //roll
|
||||
pitch_angle = _stab_pitch?degrees(atan2(-targ.x,targ.z))*100:_neutral_angles.x; //pitch
|
||||
yaw_angle = _stab_yaw? degrees(atan2(-targ.x,targ.y))*100:_neutral_angles.z; //yaw
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_MOUNT_MODE_RC_TARGETING: // radio manual control
|
||||
{
|
||||
// TODO It does work, but maybe is a good idea to replace this simplified implementation with a proper one
|
||||
if (_dcm)
|
||||
{
|
||||
roll_angle = -_dcm->roll_sensor;
|
||||
pitch_angle = -_dcm->pitch_sensor;
|
||||
yaw_angle = -_dcm->yaw_sensor;
|
||||
}
|
||||
if (_dcm_hil)
|
||||
{
|
||||
roll_angle = -_dcm_hil->roll_sensor;
|
||||
pitch_angle = -_dcm_hil->pitch_sensor;
|
||||
yaw_angle = -_dcm_hil->yaw_sensor;
|
||||
}
|
||||
if (g_rc_function[RC_Channel_aux::k_mount_roll])
|
||||
roll_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]);
|
||||
if (g_rc_function[RC_Channel_aux::k_mount_pitch])
|
||||
pitch_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_pitch]);
|
||||
if (g_rc_function[RC_Channel_aux::k_mount_yaw])
|
||||
yaw_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_yaw]);
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_MOUNT_MODE_GPS_POINT:
|
||||
{
|
||||
if(_gps->fix)
|
||||
{
|
||||
calc_GPS_target_vector(&_target_GPS_location);
|
||||
}
|
||||
m = (_dcm)?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
|
||||
targ = m*_GPS_vector;
|
||||
/* disable stabilization for now, this will help debug */
|
||||
_stab_roll = 0;_stab_pitch=0;_stab_yaw=0;
|
||||
/**/
|
||||
// TODO The next three lines are probably not correct yet
|
||||
roll_angle = _stab_roll? degrees(atan2( targ.y,targ.z))*100:_GPS_vector.y; //roll
|
||||
pitch_angle = _stab_pitch?degrees(atan2(-targ.x,targ.z))*100:0; //pitch
|
||||
yaw_angle = _stab_yaw? degrees(atan2(-targ.x,targ.y))*100:degrees(atan2(-_GPS_vector.x,_GPS_vector.y))*100; //yaw
|
||||
break;
|
||||
}
|
||||
default:
|
||||
//do nothing
|
||||
break;
|
||||
}
|
||||
|
||||
// write the results to the servos
|
||||
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
|
||||
G_RC_AUX(k_mount_roll)->closest_limit(roll_angle/10);
|
||||
G_RC_AUX(k_mount_pitch)->closest_limit(pitch_angle/10);
|
||||
G_RC_AUX(k_mount_yaw)->closest_limit(yaw_angle/10);
|
||||
}
|
||||
|
||||
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
|
||||
{
|
||||
_mount_mode=mode;
|
||||
}
|
||||
|
||||
void AP_Mount::configure_msg(mavlink_message_t* msg)
|
||||
{
|
||||
__mavlink_mount_configure_t packet;
|
||||
mavlink_msg_mount_configure_decode(msg, &packet);
|
||||
if (mavlink_check_target(packet.target_system, packet.target_component)) {
|
||||
// not for us
|
||||
return;
|
||||
}
|
||||
set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
|
||||
_stab_pitch = packet.stab_pitch;
|
||||
_stab_roll = packet.stab_roll;
|
||||
_stab_yaw = packet.stab_yaw;
|
||||
}
|
||||
|
||||
void AP_Mount::control_msg(mavlink_message_t *msg)
|
||||
{
|
||||
__mavlink_mount_control_t packet;
|
||||
mavlink_msg_mount_control_decode(msg, &packet);
|
||||
if (mavlink_check_target(packet.target_system, packet.target_component)) {
|
||||
// not for us
|
||||
return;
|
||||
}
|
||||
|
||||
switch (_mount_mode)
|
||||
{
|
||||
case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
|
||||
set_retract_angles(packet.input_b, packet.input_a, packet.input_c);
|
||||
if (packet.save_position)
|
||||
{
|
||||
// TODO: Save current trimmed position on EEPROM
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
|
||||
set_neutral_angles(packet.input_b, packet.input_a, packet.input_c);
|
||||
if (packet.save_position)
|
||||
{
|
||||
// TODO: Save current trimmed position on EEPROM
|
||||
}
|
||||
break;
|
||||
|
||||
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
|
||||
set_mavlink_angles(packet.input_b, packet.input_a, packet.input_c);
|
||||
break;
|
||||
|
||||
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
|
||||
break;
|
||||
|
||||
case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt
|
||||
Location targetGPSLocation;
|
||||
targetGPSLocation.lat = packet.input_a;
|
||||
targetGPSLocation.lng = packet.input_b;
|
||||
targetGPSLocation.alt = packet.input_c;
|
||||
set_GPS_target_location(targetGPSLocation);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Mount::status_msg(mavlink_message_t *msg)
|
||||
{
|
||||
__mavlink_mount_status_t packet;
|
||||
mavlink_msg_mount_status_decode(msg, &packet);
|
||||
if (mavlink_check_target(packet.target_system, packet.target_component)) {
|
||||
// not for us
|
||||
return;
|
||||
}
|
||||
|
||||
switch (_mount_mode)
|
||||
{
|
||||
case MAV_MOUNT_MODE_RETRACT: // safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
|
||||
case MAV_MOUNT_MODE_NEUTRAL: // neutral position (Roll,Pitch,Yaw) from EEPROM
|
||||
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
|
||||
case MAV_MOUNT_MODE_RC_TARGETING: // neutral position and start RC Roll,Pitch,Yaw control with stabilization
|
||||
packet.pointing_b = roll_angle; ///< degrees*100
|
||||
packet.pointing_a = pitch_angle; ///< degrees*100
|
||||
packet.pointing_c = yaw_angle; ///< degrees*100
|
||||
break;
|
||||
case MAV_MOUNT_MODE_GPS_POINT: // neutral position and start to point to Lat,Lon,Alt
|
||||
packet.pointing_a = _target_GPS_location.lat; ///< latitude
|
||||
packet.pointing_b = _target_GPS_location.lng; ///< longitude
|
||||
packet.pointing_c = _target_GPS_location.alt; ///< altitude
|
||||
break;
|
||||
}
|
||||
|
||||
// status reply
|
||||
// TODO: is COMM_3 correct ?
|
||||
mavlink_msg_mount_status_send(MAVLINK_COMM_3, packet.target_system, packet.target_component,
|
||||
packet.pointing_a, packet.pointing_b, packet.pointing_c);
|
||||
}
|
||||
|
||||
void AP_Mount::set_roi_cmd()
|
||||
{
|
||||
// TODO get the information out of the mission command and use it
|
||||
}
|
||||
|
||||
void AP_Mount::configure_cmd()
|
||||
{
|
||||
// TODO get the information out of the mission command and use it
|
||||
}
|
||||
|
||||
void AP_Mount::control_cmd()
|
||||
{
|
||||
// TODO get the information out of the mission command and use it
|
||||
}
|
||||
|
||||
|
||||
void AP_Mount::calc_GPS_target_vector(struct Location *target)
|
||||
{
|
||||
_GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195;
|
||||
_GPS_vector.y = (target->lat-_gps->latitude)*.01113195;
|
||||
_GPS_vector.z = (_gps->altitude-target->alt);
|
||||
}
|
||||
|
||||
void
|
||||
AP_Mount::update_mount_type()
|
||||
{
|
||||
// Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
||||
if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != 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_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == 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_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
|
||||
{
|
||||
_mount_type = k_pan_tilt_roll;
|
||||
}
|
||||
}
|
||||
|
||||
// This function is needed to let the HIL code compile
|
||||
long
|
||||
AP_Mount::rc_map(RC_Channel_aux* rc_ch)
|
||||
{
|
||||
return (rc_ch->radio_in - rc_ch->radio_min) * (rc_ch->angle_max - rc_ch->angle_min) / (rc_ch->radio_max - rc_ch->radio_min) + rc_ch->angle_min;
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
||||
|
||||
/************************************************************
|
||||
* AP_mount -- library to control a 2 or 3 axis mount. *
|
||||
* *
|
||||
* Author: Joe Holdsworth; *
|
||||
* Ritchie Wilson; *
|
||||
* Amilcar Lucas; *
|
||||
* *
|
||||
* Purpose: Move a 2 or 3 axis mount attached to vehicle, *
|
||||
* Used for mount to track targets or stabilise *
|
||||
* camera plus other modes. *
|
||||
* *
|
||||
* Usage: Use in main code to control mounts attached to *
|
||||
* vehicle. *
|
||||
* *
|
||||
*Comments: All angles in degrees * 100, distances in meters*
|
||||
* unless otherwise stated. *
|
||||
************************************************************/
|
||||
#ifndef AP_Mount_H
|
||||
#define AP_Mount_H
|
||||
|
||||
//#include <AP_Common.h>
|
||||
#include <AP_Math.h>
|
||||
#include <AP_GPS.h>
|
||||
#include <AP_DCM.h>
|
||||
#include <GCS_MAVLink.h>
|
||||
#include <../RC_Channel/RC_Channel_aux.h>
|
||||
|
||||
class AP_Mount
|
||||
{
|
||||
public:
|
||||
//Constructors
|
||||
AP_Mount(GPS *gps, AP_DCM *dcm);
|
||||
AP_Mount(GPS *gps, AP_DCM_HIL *dcm); // constructor for HIL usage
|
||||
|
||||
//enums
|
||||
enum MountType{
|
||||
k_pan_tilt = 0, ///< yaw-pitch
|
||||
k_tilt_roll = 1, ///< pitch-roll
|
||||
k_pan_tilt_roll = 2, ///< yaw-pitch-roll
|
||||
};
|
||||
|
||||
// MAVLink methods
|
||||
void configure_msg(mavlink_message_t* msg);
|
||||
void control_msg(mavlink_message_t* msg);
|
||||
void status_msg(mavlink_message_t* msg);
|
||||
void set_roi_cmd();
|
||||
void configure_cmd();
|
||||
void control_cmd();
|
||||
|
||||
// should be called periodically
|
||||
void update_mount_position();
|
||||
void update_mount_type(); ///< Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
||||
|
||||
// Accessors
|
||||
enum MountType get_mount_type();
|
||||
|
||||
private:
|
||||
|
||||
//methods
|
||||
void set_mode(enum MAV_MOUNT_MODE mode);
|
||||
|
||||
void set_retract_angles(int roll, int pitch, int yaw); ///< set mount retracted position
|
||||
void set_neutral_angles(int roll, int pitch, int yaw);
|
||||
void set_mavlink_angles(int roll, int pitch, int yaw);
|
||||
void set_GPS_target_location(Location targetGPSLocation); ///< used to tell the mount to track GPS location
|
||||
|
||||
// internal methods
|
||||
void calc_GPS_target_vector(struct Location *target);
|
||||
long rc_map(RC_Channel_aux* rc_ch);
|
||||
|
||||
//members
|
||||
AP_DCM *_dcm;
|
||||
AP_DCM_HIL *_dcm_hil;
|
||||
GPS *_gps;
|
||||
|
||||
int roll_angle; ///< degrees*100
|
||||
int pitch_angle; ///< degrees*100
|
||||
int yaw_angle; ///< degrees*100
|
||||
|
||||
uint8_t _stab_roll; ///< (1 = yes, 0 = no)
|
||||
uint8_t _stab_pitch; ///< (1 = yes, 0 = no)
|
||||
uint8_t _stab_yaw; ///< (1 = yes, 0 = no)
|
||||
|
||||
enum MAV_MOUNT_MODE _mount_mode;
|
||||
MountType _mount_type;
|
||||
|
||||
struct Location _target_GPS_location;
|
||||
|
||||
Vector3i _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
Vector3i _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
Vector3i _mavlink_angles; ///< mavlink position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
||||
Vector3f _GPS_vector; ///< target vector calculated stored in meters
|
||||
};
|
||||
#endif
|
||||
|
|
@ -5,6 +5,42 @@
|
|||
|
||||
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
|
||||
|
||||
int16_t
|
||||
RC_Channel_aux::closest_limit(int16_t angle)
|
||||
{
|
||||
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
|
||||
int16_t min = angle_min / 10;
|
||||
int16_t max = angle_max / 10;
|
||||
|
||||
// Make sure the angle lies in the interval [-180 .. 180[ degrees
|
||||
while (angle < -1800) angle += 3600;
|
||||
while (angle >= 1800) angle -= 3600;
|
||||
|
||||
// Make sure the angle limits lie in the interval [-180 .. 180[ degrees
|
||||
while (min < -1800) min += 3600;
|
||||
while (min >= 1800) min -= 3600;
|
||||
while (max < -1800) max += 3600;
|
||||
while (max >= 1800) max -= 3600;
|
||||
// This is done every time because the user might change the min, max values on the fly
|
||||
set_range(min, max);
|
||||
|
||||
// If the angle is outside servo limits, saturate the angle to the closest limit
|
||||
// On a circle the closest angular position must be carefully calculated to account for wrap-around
|
||||
if ((angle < min) && (angle > max)){
|
||||
// angle error if min limit is used
|
||||
int16_t err_min = min - angle + (angle<min?0:3600); // add 360 degrees if on the "wrong side"
|
||||
// angle error if max limit is used
|
||||
int16_t err_max = angle - max + (angle>max?0:3600); // add 360 degrees if on the "wrong side"
|
||||
angle = err_min<err_max?min:max;
|
||||
}
|
||||
|
||||
servo_out = angle;
|
||||
// convert angle to PWM using a linear transformation (ignores trimming because the camera limits might not be symmetric)
|
||||
calc_pwm();
|
||||
|
||||
return angle;
|
||||
}
|
||||
|
||||
// map a function to a servo channel and output it
|
||||
void
|
||||
RC_Channel_aux::output_ch(unsigned char ch_nr)
|
||||
|
|
@ -53,4 +89,14 @@ void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Ch
|
|||
G_RC_AUX(k_flap_auto)->set_range(0,100);
|
||||
G_RC_AUX(k_aileron)->set_angle(4500);
|
||||
G_RC_AUX(k_flaperon)->set_range(0,100);
|
||||
G_RC_AUX(k_mount_yaw)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_yaw]->angle_max / 10);
|
||||
G_RC_AUX(k_mount_pitch)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_pitch]->angle_max / 10);
|
||||
G_RC_AUX(k_mount_roll)->set_range(
|
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_min / 10,
|
||||
g_rc_function[RC_Channel_aux::k_mount_roll]->angle_max / 10);
|
||||
G_RC_AUX(k_mount_open)->set_range(0,100);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
|
||||
/// @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
|
||||
|
|
@ -23,7 +24,9 @@ public:
|
|||
///
|
||||
RC_Channel_aux(AP_Var::Key key, const prog_char_t *name) :
|
||||
RC_Channel(key, name),
|
||||
function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0) // suppress name if group has no name
|
||||
function (&_group, 4, k_none, name ? PSTR("FUNCTION") : 0), // suppress name if group has no name
|
||||
angle_min (&_group, 5, -4500, name ? PSTR("ANGLE_MIN") : 0), // assume -45 degrees min deflection
|
||||
angle_max (&_group, 6, 4500, name ? PSTR("ANGLE_MAX") : 0) // assume 45 degrees max deflection
|
||||
{}
|
||||
|
||||
typedef enum
|
||||
|
|
@ -34,10 +37,18 @@ public:
|
|||
k_flap_auto = 3, // flap automated
|
||||
k_aileron = 4, // aileron
|
||||
k_flaperon = 5, // flaperon (flaps and aileron combined, needs two independent servos one for each wing)
|
||||
k_mount_yaw = 6, // mount yaw (pan)
|
||||
k_mount_pitch = 7, // mount pitch (tilt)
|
||||
k_mount_roll = 8, // mount roll
|
||||
k_mount_open = 9, // mount 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;
|
||||
|
||||
AP_Int8 function; // 0=disabled, 1=manual, 2=flap, 3=flap auto, 4=aileron, 5=flaperon, 6=mount yaw (pan), 7=mount pitch (tilt), 8=mount roll, 9=camera trigger, 10=camera open, 11=egg drop
|
||||
AP_Int16 angle_min; // min angle limit of actuated surface in 0.01 degree units
|
||||
AP_Int16 angle_max; // max angle limit of actuated surface in 0.01 degree units
|
||||
|
||||
int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval
|
||||
|
||||
void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue