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
This commit is contained in:
Peter Barker 2019-01-30 22:18:16 +11:00 committed by Peter Barker
parent f97ea174a6
commit 8911e67900
2 changed files with 24 additions and 0 deletions

View File

@ -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)
{

View File

@ -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; }