mirror of https://github.com/ArduPilot/ardupilot
92 lines
1.7 KiB
C++
92 lines
1.7 KiB
C++
/*
|
|
test of HAL_BinarySemaphore
|
|
*/
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_HAL/Semaphores.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
void setup();
|
|
void loop();
|
|
|
|
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
|
|
|
|
class BinarySemTest {
|
|
public:
|
|
HAL_BinarySemaphore sem1{1};
|
|
HAL_BinarySemaphore sem2{0};
|
|
|
|
void setup(void);
|
|
void thread1(void);
|
|
void thread2(void);
|
|
void update(bool ok);
|
|
|
|
uint32_t ops, timeouts;
|
|
uint32_t last_print_us;
|
|
HAL_Semaphore mtx;
|
|
};
|
|
|
|
void BinarySemTest::setup(void)
|
|
{
|
|
hal.scheduler->thread_create(
|
|
FUNCTOR_BIND_MEMBER(&BinarySemTest::thread1, void), "thd1", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0);
|
|
hal.scheduler->thread_create(
|
|
FUNCTOR_BIND_MEMBER(&BinarySemTest::thread2, void), "thd2", 2048, AP_HAL::Scheduler::PRIORITY_IO, 0);
|
|
::printf("Setup threads\n");
|
|
}
|
|
|
|
void BinarySemTest::thread2(void)
|
|
{
|
|
while (true) {
|
|
bool ok = sem2.wait(50000);
|
|
sem1.signal();
|
|
update(ok);
|
|
}
|
|
}
|
|
|
|
void BinarySemTest::thread1(void)
|
|
{
|
|
while (true) {
|
|
bool ok = sem1.wait(50000);
|
|
sem2.signal();
|
|
update(ok);
|
|
}
|
|
}
|
|
|
|
void BinarySemTest::update(bool ok)
|
|
{
|
|
WITH_SEMAPHORE(mtx);
|
|
if (ok) {
|
|
ops++;
|
|
} else {
|
|
timeouts++;
|
|
}
|
|
uint32_t now_us = AP_HAL::micros();
|
|
float dt = (now_us - last_print_us)*1.0e-6;
|
|
if (dt >= 1.0) {
|
|
last_print_us = now_us;
|
|
::printf("tick %u %.3f ops/s %.3f timeouts/s\n",
|
|
unsigned(AP_HAL::millis()),
|
|
ops/dt,
|
|
timeouts/dt);
|
|
ops = 0;
|
|
timeouts = 0;
|
|
}
|
|
}
|
|
|
|
static BinarySemTest *ct;
|
|
|
|
void setup(void)
|
|
{
|
|
ct = new BinarySemTest;
|
|
ct->setup();
|
|
}
|
|
|
|
void loop(void)
|
|
{
|
|
hal.scheduler->delay(1000);
|
|
}
|
|
|
|
AP_HAL_MAIN();
|