Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Guided_PosVel mode: reject destination if outside the fence

Browse Source 
Also: log guided destination and velocity and rename a variable to pos_neu_cm to better reflect the meaning
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-12-03 18:12:38 +01:00 committed by Randy Mackay
parent ce3197e0ac
commit 63639a0838
3 changed files with 37 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
ArduSub/GCS_Mavlink.cpp
View File

@ -1398,11 +1398,18 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
// send request
if (!pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_destination_posvel(pos_vector, vel_vector);
if (sub.guided_set_destination_posvel(pos_vector, vel_vector)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_velocity(vel_vector);
result = MAV_RESULT_ACCEPTED;
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (!sub.guided_set_destination(pos_vector)) {
if (sub.guided_set_destination(pos_vector)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {
@ -1443,7 +1450,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
* bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
*/
Vector3f pos_ned;
Vector3f pos_neu_cm; // position (North, East, Up coordinates) in centimeters
if (!pos_ignore) {
// sanity check location
@ -1473,15 +1480,22 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
loc.flags.terrain_alt = false;
break;
}
pos_ned = sub.pv_location_to_vector(loc);
pos_neu_cm = sub.pv_location_to_vector(loc);
}
if (!pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_destination_posvel(pos_ned, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
if (sub.guided_set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f))) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else if (pos_ignore && !vel_ignore && acc_ignore) {
sub.guided_set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f));
result = MAV_RESULT_ACCEPTED;
} else if (!pos_ignore && vel_ignore && acc_ignore) {
if (!sub.guided_set_destination(pos_ned)) {
if (sub.guided_set_destination(pos_neu_cm)) {
result = MAV_RESULT_ACCEPTED;
} else {
result = MAV_RESULT_FAILED;
}
} else {

2
ArduSub/Sub.h
View File

@ -578,7 +578,7 @@ private:
bool guided_set_destination(const Vector3f& destination);
bool guided_set_destination(const Location_Class& dest_loc);
void guided_set_velocity(const Vector3f& velocity);
void guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
bool guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity);
void guided_set_angle(const Quaternion &q, float climb_rate_cms);
void guided_run();
void guided_pos_control_run();

18
ArduSub/control_guided.cpp
View File

@ -1,7 +1,7 @@
#include "Sub.h"
/*
* control_guided.pde - init and run calls for guided flight mode
* Init and run calls for guided flight mode
*/
#ifndef GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM
@ -212,18 +212,32 @@ void Sub::guided_set_velocity(const Vector3f& velocity)
}
// set guided mode posvel target
void Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
bool Sub::guided_set_destination_posvel(const Vector3f& destination, const Vector3f& velocity)
{
// check we are in velocity control mode
if (guided_mode != Guided_PosVel) {
guided_posvel_control_start();
}
#if AC_FENCE == ENABLED
// reject destination if outside the fence
Location_Class dest_loc(destination);
if (!fence.check_destination_within_fence(dest_loc)) {
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_DEST_OUTSIDE_FENCE);
// failure is propagated to GCS with NAK
return false;
}
#endif
posvel_update_time_ms = millis();
posvel_pos_target_cm = destination;
posvel_vel_target_cms = velocity;
pos_control.set_pos_target(posvel_pos_target_cm);
// log target
Log_Write_GuidedTarget(guided_mode, destination, velocity);
return true;
}
// set guided mode angle target