Browse Source

Plane: use scheduler ticks in place of mainloop_count

master
Peter Barker 7 years ago committed by Andrew Tridgell
parent
commit
73092c3c30
  1. 3
      ArduPlane/ArduPlane.cpp
  2. 3
      ArduPlane/Plane.h
  3. 7
      ArduPlane/failsafe.cpp

3
ArduPlane/ArduPlane.cpp

@ -126,9 +126,6 @@ void Plane::loop() @@ -126,9 +126,6 @@ void Plane::loop()
G_Dt = (timer - perf.fast_loopTimer_us) * 1.0e-6f;
perf.fast_loopTimer_us = timer;
// for mainloop failure monitoring
perf.mainLoop_count++;
// tell the scheduler one tick has passed
scheduler.tick();

3
ArduPlane/Plane.h

@ -741,9 +741,6 @@ private: @@ -741,9 +741,6 @@ private:
// System Timers
// Time in microseconds of start of main control loop
uint32_t fast_loopTimer_us;
// Counter of main loop executions. Used for performance monitoring and failsafe processing
uint16_t mainLoop_count;
} perf;
struct {

7
ArduPlane/failsafe.cpp

@ -16,14 +16,15 @@ @@ -16,14 +16,15 @@
*/
void Plane::failsafe_check(void)
{
static uint16_t last_mainLoop_count;
static uint16_t last_ticks;
static uint32_t last_timestamp;
static bool in_failsafe;
uint32_t tnow = micros();
if (perf_info.get_num_loops() != last_mainLoop_count) {
const uint16_t ticks = scheduler.ticks();
if (ticks != last_ticks) {
// the main loop is running, all is OK
last_mainLoop_count = perf_info.get_num_loops();
last_ticks = ticks;
last_timestamp = tnow;
in_failsafe = false;
return;

Loading…
Cancel
Save