From 8911e67900e5a90a7a146c7cd36de89a44cd8143 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 30 Jan 2019 22:18:16 +1100 Subject: [PATCH] AP_Rally: adjust to allow for uploading via the mission item protocol AP_Rally: add a set_rally_total method Rally: remove restriction of only setting rally points below the param count Rally: implement truncate/append interface --- libraries/AP_Rally/AP_Rally.cpp | 20 ++++++++++++++++++++ libraries/AP_Rally/AP_Rally.h | 4 ++++ 2 files changed, 24 insertions(+) diff --git a/libraries/AP_Rally/AP_Rally.cpp b/libraries/AP_Rally/AP_Rally.cpp index 0c87c81549..209eeb4cf0 100644 --- a/libraries/AP_Rally/AP_Rally.cpp +++ b/libraries/AP_Rally/AP_Rally.cpp @@ -78,6 +78,26 @@ bool AP_Rally::get_rally_point_with_index(uint8_t i, RallyLocation &ret) const return true; } +void AP_Rally::truncate(uint8_t num) +{ + if (num > _rally_point_total_count) { + // we never make the space larger this way + return; + } + _rally_point_total_count.set_and_save_ifchanged(num); +} + +bool AP_Rally::append(const RallyLocation &loc) +{ + const uint8_t current_total = get_rally_total(); + _rally_point_total_count.set_and_save_ifchanged(current_total + 1); + if (!set_rally_point_with_index(current_total, loc)) { + _rally_point_total_count.set_and_save_ifchanged(current_total); + return false; + } + return true; +} + // save a rally point to EEPROM - this assumes that the RALLY_TOTAL param has been incremented beforehand, which is the case in Mission Planner bool AP_Rally::set_rally_point_with_index(uint8_t i, const RallyLocation &rallyLoc) { diff --git a/libraries/AP_Rally/AP_Rally.h b/libraries/AP_Rally/AP_Rally.h index 34065564e4..8d6fa2935e 100644 --- a/libraries/AP_Rally/AP_Rally.h +++ b/libraries/AP_Rally/AP_Rally.h @@ -52,6 +52,10 @@ public: } return (uint8_t)ret; } + // reduce point count: + void truncate(uint8_t num); + // append a rally point to the list + bool append(const RallyLocation &loc) WARN_IF_UNUSED; float get_rally_limit_km() const { return _rally_limit_km; }