APM:Libraries
AP_Mount_Backend.cpp
Go to the documentation of this file.
1 #include "AP_Mount_Backend.h"
2 
3 extern const AP_HAL::HAL& hal;
4 
5 // set_angle_targets - sets angle targets in degrees
6 void AP_Mount_Backend::set_angle_targets(float roll, float tilt, float pan)
7 {
8  // set angle targets
12 
13  // set the mode to mavlink targeting
14  _frontend.set_mode(_instance, MAV_MOUNT_MODE_MAVLINK_TARGETING);
15 }
16 
17 // set_roi_target - sets target location that mount should attempt to point towards
18 void AP_Mount_Backend::set_roi_target(const struct Location &target_loc)
19 {
20  // set the target gps location
21  _state._roi_target = target_loc;
22 
23  // set the mode to GPS tracking mode
24  _frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT);
25 }
26 
27 // configure_msg - process MOUNT_CONFIGURE messages received from GCS. deprecated.
28 void AP_Mount_Backend::configure_msg(mavlink_message_t* msg)
29 {
30  __mavlink_mount_configure_t packet;
31  mavlink_msg_mount_configure_decode(msg, &packet);
32 
33  set_mode((MAV_MOUNT_MODE)packet.mount_mode);
34 }
35 
36 // control_msg - process MOUNT_CONTROL messages received from GCS. deprecated.
37 void AP_Mount_Backend::control_msg(mavlink_message_t *msg)
38 {
39  __mavlink_mount_control_t packet;
40  mavlink_msg_mount_control_decode(msg, &packet);
41 
42  control((int32_t)packet.input_a, (int32_t)packet.input_b, (int32_t)packet.input_c, _state._mode);
43 }
44 
45 void AP_Mount_Backend::control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode)
46 {
47  _frontend.set_mode(_instance, mount_mode);
48 
49  // interpret message fields based on mode
50  switch (_frontend.get_mode(_instance)) {
51  case MAV_MOUNT_MODE_RETRACT:
52  case MAV_MOUNT_MODE_NEUTRAL:
53  // do nothing with request if mount is retracted or in neutral position
54  break;
55 
56  // set earth frame target angles from mavlink message
57  case MAV_MOUNT_MODE_MAVLINK_TARGETING:
58  set_angle_targets(roll_or_lon*0.01f, pitch_or_lat*0.01f, yaw_or_alt*0.01f);
59  break;
60 
61  // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
62  case MAV_MOUNT_MODE_RC_TARGETING:
63  // do nothing if pilot is controlling the roll, pitch and yaw
64  break;
65 
66  // set lat, lon, alt position targets from mavlink message
67  case MAV_MOUNT_MODE_GPS_POINT:
68  Location target_location;
69  memset(&target_location, 0, sizeof(target_location));
70  target_location.lat = pitch_or_lat;
71  target_location.lng = roll_or_lon;
72  target_location.alt = yaw_or_alt;
73  target_location.flags.relative_alt = true;
74  set_roi_target(target_location);
75  break;
76 
77  default:
78  // do nothing
79  break;
80  }
81 }
82 
83 // update_targets_from_rc - updates angle targets using input from receiver
85 {
86 #define rc_ch(i) RC_Channels::rc_channel(i-1)
87 
88  uint8_t roll_rc_in = _state._roll_rc_in;
89  uint8_t tilt_rc_in = _state._tilt_rc_in;
90  uint8_t pan_rc_in = _state._pan_rc_in;
91 
92  // if joystick_speed is defined then pilot input defines a rate of change of the angle
94  // allow pilot speed position input to come directly from an RC_Channel
95  if (roll_rc_in && rc_ch(roll_rc_in)) {
96  _angle_ef_target_rad.x += rc_ch(roll_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed;
98  }
99  if (tilt_rc_in && (rc_ch(tilt_rc_in))) {
100  _angle_ef_target_rad.y += rc_ch(tilt_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed;
102  }
103  if (pan_rc_in && (rc_ch(pan_rc_in))) {
104  _angle_ef_target_rad.z += rc_ch(pan_rc_in)->norm_input_dz() * 0.0001f * _frontend._joystick_speed;
106  }
107  } else {
108  // allow pilot position input to come directly from an RC_Channel
109  if (roll_rc_in && (rc_ch(roll_rc_in))) {
111  }
112  if (tilt_rc_in && (rc_ch(tilt_rc_in))) {
114  }
115  if (pan_rc_in && (rc_ch(pan_rc_in))) {
117  }
118  }
119 }
120 
121 // returns the angle (degrees*100) that the RC_Channel input is receiving
122 int32_t AP_Mount_Backend::angle_input(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
123 {
124  return (rc->get_reverse() ? -1 : 1) * (rc->get_radio_in() - rc->get_radio_min())
125  * (int32_t)(angle_max - angle_min) / (rc->get_radio_max() - rc->get_radio_min()) + (rc->get_reverse() ? angle_max : angle_min);
126 }
127 
128 // returns the angle (radians) that the RC_Channel input is receiving
129 float AP_Mount_Backend::angle_input_rad(RC_Channel* rc, int16_t angle_min, int16_t angle_max)
130 {
131  return radians(angle_input(rc, angle_min, angle_max)*0.01f);
132 }
133 
134 // calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
135 void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan)
136 {
137  float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
138  float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f;
139  float GPS_vector_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
140  float target_distance = 100.0f*norm(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
141 
142  // initialise all angles to zero
143  angles_to_target_rad.zero();
144 
145  // tilt calcs
146  if (calc_tilt) {
147  angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance);
148  }
149 
150  // pan calcs
151  if (calc_pan) {
152  // calc absolute heading and then onvert to vehicle relative yaw
153  angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
154  if (relative_pan) {
155  angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - _frontend._ahrs.yaw);
156  }
157  }
158 }
float norm(const T first, const U second, const Params... parameters)
Definition: AP_Math.h:190
MAV_MOUNT_MODE _mode
Definition: AP_Mount.h:182
AP_Int16 _pan_angle_max
Definition: AP_Mount.h:174
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
Definition: AC_PID_test.cpp:14
Vector3f _angle_ef_target_rad
void set_mode(enum MAV_MOUNT_MODE mode)
Definition: AP_Mount.h:98
virtual void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode)
int16_t get_radio_in() const
Definition: RC_Channel.h:79
AP_Int8 _joystick_speed
Definition: AP_Mount.h:147
#define ToRad(x)
Definition: AP_Common.h:53
int32_t angle_input(RC_Channel *rc, int16_t angle_min, int16_t angle_max)
float angle_input_rad(RC_Channel *rc, int16_t angle_min, int16_t angle_max)
enum MAV_MOUNT_MODE get_mode() const
Definition: AP_Mount.h:93
virtual void control_msg(mavlink_message_t *msg)
static RC_Channel * rc
Definition: RC_Channel.cpp:17
AP_Mount::mount_state & _state
virtual void set_mode(enum MAV_MOUNT_MODE mode)=0
int32_t lat
param 3 - Latitude * 10**7
Definition: AP_Common.h:143
virtual void set_angle_targets(float roll, float tilt, float pan)
AP_Mount & _frontend
float wrap_PI(const T radian)
Definition: AP_Math.cpp:152
int32_t alt
param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
Definition: AP_Common.h:142
Object managing one RC channel.
Definition: RC_Channel.h:15
#define f(i)
void calc_angle_to_location(const struct Location &target, Vector3f &angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan=true)
T y
Definition: vector3.h:67
struct Location _roi_target
Definition: AP_Mount.h:183
T z
Definition: vector3.h:67
virtual void configure_msg(mavlink_message_t *msg)
Location_Option_Flags flags
options bitmask (1<<0 = relative altitude)
Definition: AP_Common.h:135
virtual void set_roi_target(const struct Location &target_loc)
float constrain_float(const float amt, const float low, const float high)
Definition: AP_Math.h:142
int32_t lng
param 4 - Longitude * 10**7
Definition: AP_Common.h:144
const AP_AHRS_TYPE & _ahrs
Definition: AP_Mount.h:143
static constexpr float radians(float deg)
Definition: AP_Math.h:158
bool get_reverse(void) const
Definition: RC_Channel.cpp:106
AP_Int16 _pan_angle_min
Definition: AP_Mount.h:173
AP_Int16 _roll_angle_max
Definition: AP_Mount.h:170
int16_t get_radio_min() const
Definition: RC_Channel.h:88
int16_t get_radio_max() const
Definition: RC_Channel.h:91
AP_Int16 _roll_angle_min
Definition: AP_Mount.h:169
#define rc_ch(i)
const struct Location & _current_loc
Definition: AP_Mount.h:144
AP_Int16 _tilt_angle_max
Definition: AP_Mount.h:172
void zero()
Definition: vector3.h:182
T x
Definition: vector3.h:67
AP_Int16 _tilt_angle_min
Definition: AP_Mount.h:171