AP_Rally: added last_change_time_ms() call

This commit is contained in:
Andrew Tridgell 2014-08-06 16:35:43 +10:00
parent 7da939047f
commit 723aa7e336
2 changed files with 8 additions and 0 deletions

View File

@ -43,6 +43,7 @@ AP_Rally::AP_Rally(AP_AHRS &ahrs, uint16_t max_rally_points, uint16_t rally_star
: _ahrs(ahrs)
, _max_rally_points(max_rally_points)
, _rally_start_byte(rally_start_byte)
, _last_change_time_ms(0xFFFFFFFF)
{
AP_Param::setup_object_defaults(this, var_info);
}
@ -76,6 +77,8 @@ bool AP_Rally::set_rally_point_with_index(uint8_t i, const RallyLocation &rallyL
hal.storage->write_block(_rally_start_byte + (i * sizeof(RallyLocation)), &rallyLoc, sizeof(RallyLocation));
_last_change_time_ms = hal.scheduler->millis();
return true;
}

View File

@ -54,6 +54,9 @@ public:
Location calc_best_rally_or_home_location(const Location &current_loc, float rtl_home_alt) const;
bool find_nearest_rally_point(const Location &myloc, RallyLocation &ret) const;
// last time rally points changed
uint32_t last_change_time_ms(void) const { return _last_change_time_ms; }
// parameter block
static const struct AP_Param::GroupInfo var_info[];
@ -66,6 +69,8 @@ private:
// parameters
AP_Int8 _rally_point_total_count;
AP_Float _rally_limit_km;
uint32_t _last_change_time_ms;
};