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

AC_Fence: Replace with GCS_SEND_TEXT

This commit is contained in:
muramura 2023-10-14 22:19:09 +09:00 committed by Andrew Tridgell
parent 8a1872bd2a
commit f6d0f52876
2 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_Fence/AC_Fence.cpp
View File

@ -212,7 +212,7 @@ void AC_Fence::auto_enable_fence_after_takeoff(void)
case AC_Fence::AutoEnable::ALWAYS_ENABLED:
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
enable(true);
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)");
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)");
break;
default:
// fence does not auto-enable in other takeoff conditions
@ -228,11 +228,11 @@ void AC_Fence::auto_disable_fence_for_landing(void)
switch (auto_enabled()) {
case AC_Fence::AutoEnable::ALWAYS_ENABLED:
enable(false);
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)");
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)");
break;
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
disable_floor();
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
break;
default:
// fence does not auto-disable in other landing conditions

56
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -20,7 +20,7 @@
#define POLYFENCE_LOADER_DEBUGGING 0
#if POLYFENCE_LOADER_DEBUGGING
#define Debug(fmt, args ...) do { gcs().send_text(MAV_SEVERITY_INFO, fmt, ## args); } while (0)
#define Debug(fmt, args ...) do { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } while (0)
#else
#define Debug(fmt, args ...)
#endif
@ -356,7 +356,7 @@ bool AC_PolyFence_loader::scan_eeprom(scan_fn_t scan_fn)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Fence corrupt (offset=%u)", read_offset);
#endif
gcs().send_text(MAV_SEVERITY_WARNING, "Fence corrupt");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence corrupt");
return false;
}
@ -700,13 +700,13 @@ bool AC_PolyFence_loader::load_from_eeprom()
boundary.points_lla = next_storage_point_lla;
boundary.count = index.count;
if (index.count < 3) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
storage_valid = false;
break;
}
storage_offset += 1; // skip vertex count
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
storage_valid = false;
break;
}
@ -719,13 +719,13 @@ bool AC_PolyFence_loader::load_from_eeprom()
boundary.points_lla = next_storage_point_lla;
boundary.count = index.count;
if (index.count < 3) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: invalid polygon vertex count %u", index.count);
storage_valid = false;
break;
}
storage_offset += 1; // skip vertex count
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: polygon read failed");
storage_valid = false;
break;
}
@ -736,12 +736,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
case AC_PolyFenceType::CIRCLE_EXCLUSION: {
ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries];
if (!read_latlon_from_storage(storage_offset, circle.point)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
@ -752,7 +752,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
circle.radius = fence_storage.read_float(storage_offset);
}
if (!is_positive(circle.radius)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
storage_valid = false;
break;
}
@ -763,12 +763,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
case AC_PolyFenceType::CIRCLE_INCLUSION: {
InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries];
if (!read_latlon_from_storage(storage_offset, circle.point)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)){
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: latlon read failed");
storage_valid = false;
break;
}
@ -779,7 +779,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
circle.radius = fence_storage.read_float(storage_offset);
}
if (!is_positive(circle.radius)) {
gcs().send_text(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "AC_Fence: non-positive circle radius");
storage_valid = false;
break;
}
@ -788,13 +788,13 @@ bool AC_PolyFence_loader::load_from_eeprom()
}
case AC_PolyFenceType::RETURN_POINT:
if (_loaded_return_point != nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
storage_valid = false;
break;
}
_loaded_return_point = next_storage_point;
if (_loaded_return_point_lla != nullptr) {
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: Multiple return points found");
storage_valid = false;
break;
}
@ -802,12 +802,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
// Read the point from storage
if (!read_latlon_from_storage(storage_offset, *next_storage_point_lla)) {
storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break;
}
if (!scale_latlon_from_origin(ekf_origin, *next_storage_point_lla, *next_storage_point)) {
storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break;
}
next_storage_point++;
@ -914,7 +914,7 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
case AC_PolyFenceType::POLYGON_INCLUSION:
case AC_PolyFenceType::POLYGON_EXCLUSION:
if (new_items[i].vertex_count < 3) {
gcs().send_text(MAV_SEVERITY_WARNING, "Invalid vertex count (%u)", new_items[i].vertex_count);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Invalid vertex count (%u)", new_items[i].vertex_count);
return false;
}
if (expected_type_count == 0) {
@ -923,10 +923,10 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
expecting_type = new_items[i].type;
} else {
if (new_items[i].type != expecting_type) {
gcs().send_text(MAV_SEVERITY_WARNING, "Received incorrect vertex type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect vertex type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
return false;
} else if (new_items[i].vertex_count != orig_expected_type_count) {
gcs().send_text(MAV_SEVERITY_WARNING, "Unexpected vertex count want=%u got=%u\n", orig_expected_type_count, new_items[i].vertex_count);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unexpected vertex count want=%u got=%u\n", orig_expected_type_count, new_items[i].vertex_count);
return false;
}
}
@ -942,11 +942,11 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
case AC_PolyFenceType::CIRCLE_INCLUSION:
case AC_PolyFenceType::CIRCLE_EXCLUSION:
if (expected_type_count) {
gcs().send_text(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
return false;
}
if (!is_positive(new_items[i].radius)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Non-positive circle radius");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Non-positive circle radius");
return false;
}
validate_latlon = true;
@ -954,13 +954,13 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
case AC_PolyFenceType::RETURN_POINT:
if (expected_type_count) {
gcs().send_text(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Received incorrect type (want=%u got=%u)", (unsigned)expecting_type, (unsigned)new_items[i].type);
return false;
}
// spec says only one return point allowed
if (seen_return_point) {
gcs().send_text(MAV_SEVERITY_WARNING, "Multiple return points");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Multiple return points");
return false;
}
seen_return_point = true;
@ -972,14 +972,14 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
if (validate_latlon) {
if (!check_latlng(new_items[i].loc[0], new_items[i].loc[1])) {
gcs().send_text(MAV_SEVERITY_WARNING, "Bad lat or lon");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Bad lat or lon");
return false;
}
}
}
if (expected_type_count) {
gcs().send_text(MAV_SEVERITY_INFO, "Incorrect item count");
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Incorrect item count");
return false;
}
@ -1022,12 +1022,12 @@ uint16_t AC_PolyFence_loader::fence_storage_space_required(const AC_PolyFenceIte
bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_t count)
{
if (!validate_fence(new_items, count)) {
gcs().send_text(MAV_SEVERITY_WARNING, "Fence validation failed");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence validation failed");
return false;
}
if (fence_storage_space_required(new_items, count) > fence_storage.size()) {
gcs().send_text(MAV_SEVERITY_WARNING, "Fence exceeds storage size");
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence exceeds storage size");
return false;
}
@ -1118,7 +1118,7 @@ bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_
if (!index_eeprom()) {
AP_HAL::panic("Failed to index eeprom");
}
gcs().send_text(MAV_SEVERITY_DEBUG, "Fence Indexed OK");
GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "Fence Indexed OK");
#endif
#if HAL_LOGGING_ENABLED