mirror of https://github.com/ArduPilot/ardupilot
Copter: Allowing Terrain Following in Guided Mode
Uses frame type of MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT message to decide the value of terrain_alt boolean.
This commit is contained in:
parent
53fb333f0c
commit
f6125b26e8
|
@ -1085,6 +1085,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
|
||||||
*/
|
*/
|
||||||
|
|
||||||
Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters
|
Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters
|
||||||
|
bool terrain_alt = false;
|
||||||
|
|
||||||
if(!pos_ignore) {
|
if(!pos_ignore) {
|
||||||
// sanity check location
|
// sanity check location
|
||||||
|
@ -1102,6 +1103,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
|
||||||
int32_t(packet.alt*100),
|
int32_t(packet.alt*100),
|
||||||
frame,
|
frame,
|
||||||
};
|
};
|
||||||
|
terrain_alt = (loc.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN);
|
||||||
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
|
if (!loc.get_vector_from_origin_NEU(pos_neu_cm)) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -1124,7 +1126,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
|
||||||
} else if (pos_ignore && !vel_ignore && acc_ignore) {
|
} else if (pos_ignore && !vel_ignore && acc_ignore) {
|
||||||
copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
||||||
} else if (!pos_ignore && vel_ignore && acc_ignore) {
|
} else if (!pos_ignore && vel_ignore && acc_ignore) {
|
||||||
copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
|
copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative, terrain_alt);
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -784,7 +784,7 @@ public:
|
||||||
bool requires_terrain_failsafe() const override { return true; }
|
bool requires_terrain_failsafe() const override { return true; }
|
||||||
|
|
||||||
void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
|
void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads);
|
||||||
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool set_destination(const Vector3f& destination, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool terrain_alt = false);
|
||||||
bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
bool set_destination(const Location& dest_loc, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
|
||||||
bool get_wp(Location &loc) override;
|
bool get_wp(Location &loc) override;
|
||||||
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
|
void set_velocity(const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false, bool log_request = true);
|
||||||
|
|
|
@ -176,7 +176,7 @@ void ModeGuided::angle_control_start()
|
||||||
// guided_set_destination - sets guided mode's target destination
|
// guided_set_destination - sets guided mode's target destination
|
||||||
// Returns true if the fence is enabled and guided waypoint is within the fence
|
// Returns true if the fence is enabled and guided waypoint is within the fence
|
||||||
// else return false if the waypoint is outside the fence
|
// else return false if the waypoint is outside the fence
|
||||||
bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw)
|
bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_yaw, bool terrain_alt)
|
||||||
{
|
{
|
||||||
// ensure we are in position control mode
|
// ensure we are in position control mode
|
||||||
if (guided_mode != Guided_WP) {
|
if (guided_mode != Guided_WP) {
|
||||||
|
@ -197,7 +197,7 @@ bool ModeGuided::set_destination(const Vector3f& destination, bool use_yaw, floa
|
||||||
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
|
||||||
|
|
||||||
// no need to check return status because terrain data is not used
|
// no need to check return status because terrain data is not used
|
||||||
wp_nav->set_wp_destination(destination, false);
|
wp_nav->set_wp_destination(destination, terrain_alt);
|
||||||
|
|
||||||
// log target
|
// log target
|
||||||
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
|
copter.Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
|
||||||
|
|
Loading…
Reference in New Issue