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

AC_Fence: Replace with GCS_SEND_TEXT

Browse Source
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::ALWAYS_ENABLED:
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
enable(true); enable(true);
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)"); GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence enabled (auto enabled)");
break; break;
default: default:
// fence does not auto-enable in other takeoff conditions // 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()) { switch (auto_enabled()) {
case AC_Fence::AutoEnable::ALWAYS_ENABLED: case AC_Fence::AutoEnable::ALWAYS_ENABLED:
enable(false); enable(false);
gcs().send_text(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)"); GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Fence disabled (auto disable)");
break; break;
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
disable_floor(); 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; break;
default: default:
// fence does not auto-disable in other landing conditions // 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 #define POLYFENCE_LOADER_DEBUGGING 0
#if POLYFENCE_LOADER_DEBUGGING #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 #else
#define Debug(fmt, args ...) #define Debug(fmt, args ...)
#endif #endif
@ -356,7 +356,7 @@ bool AC_PolyFence_loader::scan_eeprom(scan_fn_t scan_fn)
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Fence corrupt (offset=%u)", read_offset); AP_HAL::panic("Fence corrupt (offset=%u)", read_offset);
#endif #endif
gcs().send_text(MAV_SEVERITY_WARNING, "Fence corrupt"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Fence corrupt");
return false; return false;
} }
@ -700,13 +700,13 @@ bool AC_PolyFence_loader::load_from_eeprom()
boundary.points_lla = next_storage_point_lla; boundary.points_lla = next_storage_point_lla;
boundary.count = index.count; boundary.count = index.count;
if (index.count < 3) { 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; storage_valid = false;
break; break;
} }
storage_offset += 1; // skip vertex count storage_offset += 1; // skip vertex count
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) { 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; storage_valid = false;
break; break;
} }
@ -719,13 +719,13 @@ bool AC_PolyFence_loader::load_from_eeprom()
boundary.points_lla = next_storage_point_lla; boundary.points_lla = next_storage_point_lla;
boundary.count = index.count; boundary.count = index.count;
if (index.count < 3) { 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; storage_valid = false;
break; break;
} }
storage_offset += 1; // skip vertex count storage_offset += 1; // skip vertex count
if (!read_polygon_from_storage(ekf_origin, storage_offset, index.count, next_storage_point, next_storage_point_lla)) { 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; storage_valid = false;
break; break;
} }
@ -736,12 +736,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
case AC_PolyFenceType::CIRCLE_EXCLUSION: { case AC_PolyFenceType::CIRCLE_EXCLUSION: {
ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries]; ExclusionCircle &circle = _loaded_circle_exclusion_boundary[_num_loaded_circle_exclusion_boundaries];
if (!read_latlon_from_storage(storage_offset, circle.point)) { 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; storage_valid = false;
break; break;
} }
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)) { 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; storage_valid = false;
break; break;
} }
@ -752,7 +752,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
circle.radius = fence_storage.read_float(storage_offset); circle.radius = fence_storage.read_float(storage_offset);
} }
if (!is_positive(circle.radius)) { 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; storage_valid = false;
break; break;
} }
@ -763,12 +763,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
case AC_PolyFenceType::CIRCLE_INCLUSION: { case AC_PolyFenceType::CIRCLE_INCLUSION: {
InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries]; InclusionCircle &circle = _loaded_circle_inclusion_boundary[_num_loaded_circle_inclusion_boundaries];
if (!read_latlon_from_storage(storage_offset, circle.point)) { 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; storage_valid = false;
break; break;
} }
if (!scale_latlon_from_origin(ekf_origin, circle.point, circle.pos_cm)){ 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; storage_valid = false;
break; break;
} }
@ -779,7 +779,7 @@ bool AC_PolyFence_loader::load_from_eeprom()
circle.radius = fence_storage.read_float(storage_offset); circle.radius = fence_storage.read_float(storage_offset);
} }
if (!is_positive(circle.radius)) { 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; storage_valid = false;
break; break;
} }
@ -788,13 +788,13 @@ bool AC_PolyFence_loader::load_from_eeprom()
} }
case AC_PolyFenceType::RETURN_POINT: case AC_PolyFenceType::RETURN_POINT:
if (_loaded_return_point != nullptr) { 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; storage_valid = false;
break; break;
} }
_loaded_return_point = next_storage_point; _loaded_return_point = next_storage_point;
if (_loaded_return_point_lla != nullptr) { 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; storage_valid = false;
break; break;
} }
@ -802,12 +802,12 @@ bool AC_PolyFence_loader::load_from_eeprom()
// Read the point from storage // Read the point from storage
if (!read_latlon_from_storage(storage_offset, *next_storage_point_lla)) { if (!read_latlon_from_storage(storage_offset, *next_storage_point_lla)) {
storage_valid = false; storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break; break;
} }
if (!scale_latlon_from_origin(ekf_origin, *next_storage_point_lla, *next_storage_point)) { if (!scale_latlon_from_origin(ekf_origin, *next_storage_point_lla, *next_storage_point)) {
storage_valid = false; storage_valid = false;
gcs().send_text(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "PolyFence: latlon read failed");
break; break;
} }
next_storage_point++; 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_INCLUSION:
case AC_PolyFenceType::POLYGON_EXCLUSION: case AC_PolyFenceType::POLYGON_EXCLUSION:
if (new_items[i].vertex_count < 3) { 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; return false;
} }
if (expected_type_count == 0) { 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; expecting_type = new_items[i].type;
} else { } else {
if (new_items[i].type != expecting_type) { 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; return false;
} else if (new_items[i].vertex_count != orig_expected_type_count) { } 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; 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_INCLUSION:
case AC_PolyFenceType::CIRCLE_EXCLUSION: case AC_PolyFenceType::CIRCLE_EXCLUSION:
if (expected_type_count) { 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; return false;
} }
if (!is_positive(new_items[i].radius)) { 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; return false;
} }
validate_latlon = true; validate_latlon = true;
@ -954,13 +954,13 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
case AC_PolyFenceType::RETURN_POINT: case AC_PolyFenceType::RETURN_POINT:
if (expected_type_count) { 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; return false;
} }
// spec says only one return point allowed // spec says only one return point allowed
if (seen_return_point) { if (seen_return_point) {
gcs().send_text(MAV_SEVERITY_WARNING, "Multiple return points"); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Multiple return points");
return false; return false;
} }
seen_return_point = true; seen_return_point = true;
@ -972,14 +972,14 @@ bool AC_PolyFence_loader::validate_fence(const AC_PolyFenceItem *new_items, uint
if (validate_latlon) { if (validate_latlon) {
if (!check_latlng(new_items[i].loc[0], new_items[i].loc[1])) { 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; return false;
} }
} }
} }
if (expected_type_count) { if (expected_type_count) {
gcs().send_text(MAV_SEVERITY_INFO, "Incorrect item count"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Incorrect item count");
return false; 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) bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_t count)
{ {
if (!validate_fence(new_items, 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; return false;
} }
if (fence_storage_space_required(new_items, count) > fence_storage.size()) { 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; return false;
} }
@ -1118,7 +1118,7 @@ bool AC_PolyFence_loader::write_fence(const AC_PolyFenceItem *new_items, uint16_
if (!index_eeprom()) { if (!index_eeprom()) {
AP_HAL::panic("Failed to 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 #endif
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED