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

AP_Mount: correct use of disparate altitude frames in AP_Mount

This commit is contained in:
Peter Barker 2019-11-04 13:09:30 +11:00 committed by Peter Barker
parent 9f719ec02e
commit 7139c1121f
8 changed files with 46 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AP_Mount/AP_Mount.h
View File

@ -167,6 +167,7 @@ protected:
MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum)
struct Location _roi_target; // roi target location
uint32_t _roi_target_set_ms;
} state[AP_MOUNT_MAX_INSTANCES];
private:

3
libraries/AP_Mount/AP_Mount_Alexmos.cpp
View File

@ -52,8 +52,7 @@ void AP_Mount_Alexmos::update()
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, false);
if (calc_angle_to_roi_target(_angle_ef_target_rad, true, false)) {
control_axis(_angle_ef_target_rad, false);
}
break;

27
libraries/AP_Mount/AP_Mount_Backend.cpp
View File

@ -20,6 +20,7 @@ void AP_Mount_Backend::set_roi_target(const struct Location &target_loc)
{
// set the target gps location
_state._roi_target = target_loc;
_state._roi_target_set_ms = AP_HAL::millis();
// set the mode to GPS tracking mode
_frontend.set_mode(_instance, MAV_MOUNT_MODE_GPS_POINT);
@ -131,16 +132,35 @@ float AP_Mount_Backend::angle_input_rad(const RC_Channel* rc, int16_t angle_min,
return radians(((rc->norm_input() + 1.0f) * 0.5f * (angle_max - angle_min) + angle_min)*0.01f);
}
bool AP_Mount_Backend::calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan)
{
if (_state._roi_target_set_ms == 0) {
return false;
}
return calc_angle_to_location(_state._roi_target, angles_to_target_rad, calc_tilt, calc_pan, relative_pan);
}
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
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)
bool 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)
{
Location current_loc;
if (!AP::ahrs().get_position(current_loc)) {
// completely ignore this error; ahrs will give us its best guess
return false;
}
const float GPS_vector_x = (target.lng-current_loc.lng)*cosf(ToRad((current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
const float GPS_vector_y = (target.lat-current_loc.lat)*0.01113195f;
const float GPS_vector_z = (target.alt-current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
int32_t target_alt_cm = 0;
if (!target.get_alt_cm(Location::AltFrame::ABOVE_HOME, target_alt_cm)) {
return false;
}
int32_t current_alt_cm = 0;
if (!current_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, current_alt_cm)) {
return false;
}
float GPS_vector_z = target_alt_cm - current_alt_cm;
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.
// initialise all angles to zero
@ -159,4 +179,5 @@ void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vec
angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - AP::ahrs().yaw);
}
}
return true;
}

20
libraries/AP_Mount/AP_Mount_Backend.h
View File

@ -52,10 +52,10 @@ public:
virtual void set_mode(enum MAV_MOUNT_MODE mode) = 0;
// set_angle_targets - sets angle targets in degrees
virtual void set_angle_targets(float roll, float tilt, float pan);
void set_angle_targets(float roll, float tilt, float pan);
// set_roi_target - sets target location that mount should attempt to point towards
virtual void set_roi_target(const struct Location &target_loc);
void set_roi_target(const struct Location &target_loc);
// control - control the mount
virtual void control(int32_t pitch_or_lat, int32_t roll_or_lon, int32_t yaw_or_alt, MAV_MOUNT_MODE mount_mode);
@ -86,8 +86,20 @@ protected:
// angle_input_rad - convert RC input into an earth-frame target angle
float angle_input_rad(const RC_Channel* rc, int16_t angle_min, int16_t angle_max);
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
void calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan = true);
// calc_angle_to_location - calculates the earth-frame roll, tilt
// and pan angles (and radians) to point at the given target
bool calc_angle_to_location(const struct Location &target,
Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan = true) WARN_IF_UNUSED;
// calc_angle_to_roi_target - calculates the earth-frame roll, tilt
// and pan angles (and radians) to point at the ROI-target (as set
// by various mavlink messages)
bool calc_angle_to_roi_target(Vector3f& angles_to_target_rad,
bool calc_tilt,
bool calc_pan,
bool relative_pan = true) WARN_IF_UNUSED;
// get the mount mode from frontend
MAV_MOUNT_MODE get_mode(void) const { return _frontend.get_mode(_instance); }

3
libraries/AP_Mount/AP_Mount_SToRM32.cpp
View File

@ -59,8 +59,7 @@ void AP_Mount_SToRM32::update()
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) {
resend_now = true;
}
break;

3
libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp
View File

@ -75,8 +75,7 @@ void AP_Mount_SToRM32_serial::update()
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) {
resend_now = true;
}
break;

3
libraries/AP_Mount/AP_Mount_Servo.cpp
View File

@ -70,8 +70,7 @@ void AP_Mount_Servo::update()
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
{
if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false);
if (calc_angle_to_roi_target(_angle_ef_target_rad, _flags.tilt_control, _flags.pan_control, false)) {
stabilize();
}
break;

3
libraries/AP_Mount/AP_Mount_SoloGimbal.cpp
View File

@ -70,8 +70,7 @@ void AP_Mount_SoloGimbal::update()
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
_gimbal.set_lockedToBody(false);
if(AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
if (calc_angle_to_roi_target(_angle_ef_target_rad, true, true)) {
}
break;