From 810dbbd25b5a1e3a27918d1cd51f3c1059335cc4 Mon Sep 17 00:00:00 2001 From: alexklimaj Date: Sun, 15 Aug 2021 19:07:06 -0600 Subject: [PATCH] AFBR mode switching, rate switching, and test --- .../broadcom/afbrs50/AFBRS50.cpp | 212 ++++++-- .../broadcom/afbrs50/AFBRS50.hpp | 14 +- .../broadcom/afbrs50/API/Src/timer.c | 27 +- .../broadcom/afbrs50/argus_hal_test.c | 476 +++++++++++++----- .../broadcom/afbrs50/argus_hal_test.h | 175 ++++--- 5 files changed, 654 insertions(+), 250 deletions(-) diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index 72ea05172b..22c3105c89 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -77,6 +77,9 @@ status_t AFBRS50::measurement_ready_callback(status_t status, void *data) if (g_dev) { g_dev->ProcessMeasurement(data); } + + } else { + PX4_ERR("Measurement Ready Callback received error!: %i", (int)status); } } @@ -94,15 +97,23 @@ void AFBRS50::ProcessMeasurement(void *data) if ((evaluate_status == STATUS_OK) && (res.Status == 0)) { uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000); float result_m = static_cast(result_mm) / 1000.f; + //int8_t quality = res.Bin.SignalQuality; int8_t quality = 100; + // Signal quality indicates 100% for good signals, 50% and lower for weak signals. + // 1% is an errored signal (not reliable). Signal Quality of 0% is unknown. + //if (quality == 1) { + // quality = 0; + //} + // distance quality check - if (result_m < _min_distance || result_m > _max_distance) { + if (result_m > _max_distance) { result_m = 0.0; quality = 0; } _current_distance = result_m; + _current_quality = quality; _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); } } @@ -135,8 +146,7 @@ int AFBRS50::init() uint8_t a = (value >> 24) & 0xFFU; uint8_t b = (value >> 16) & 0xFFU; uint8_t c = value & 0xFFFFU; - PX4_INFO_RAW("AFBR-S50 Chip ID: %" PRId32 ", API Version: %" PRId32 " v%" PRId8 ".%" PRId8 ".%" PRId8 "\n", id, - value, a, b, c); + PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c); argus_module_version_t mv = Argus_GetModuleVersion(_hnd); @@ -157,10 +167,8 @@ int AFBRS50::init() break; case AFBR_S50LV85D_V1: - // Start in short range mode - set_mode(ARGUS_MODE_B); // Long: ARGUS_MODE_A, Short: ARGUS_MODE_B _min_distance = 0.08f; - _max_distance = 80.f; // Long: 80m, Short: 30m + _max_distance = 30.f; _px4_rangefinder.set_min_distance(_min_distance); _px4_rangefinder.set_max_distance(_max_distance); _px4_rangefinder.set_fov(math::radians(6.f)); @@ -198,7 +206,13 @@ int AFBRS50::init() break; } - _state = STATE::CONFIGURE; + if (_testing) { + _state = STATE::TEST; + + } else { + _state = STATE::CONFIGURE; + } + ScheduleDelayed(_measure_interval); return PX4_OK; } @@ -210,20 +224,46 @@ void AFBRS50::Run() { switch (_state) { case STATE::TEST: { - Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd)); + if (_testing) { + Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd)); + _testing = false; - _state = STATE::CONFIGURE; - ScheduleDelayed(100_ms); + } else { + _state = STATE::CONFIGURE; + } } break; case STATE::CONFIGURE: { - Argus_SetConfigurationFrameTime(_hnd, _measure_interval); - - status_t status = Argus_StartMeasurementTimer(_hnd, measurement_ready_callback); + //status_t status = Argus_SetConfigurationFrameTime(_hnd, _measure_interval); + status_t status = set_rate(SHORT_RANGE_MODE_HZ); if (status != STATUS_OK) { - PX4_ERR("CONFIGURE status not okay: %" PRIi32, status); + PX4_ERR("CONFIGURE status not okay: %i", (int)status); + _state = STATE::STOP; + ScheduleNow(); + } + + status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_B, DFM_MODE_8X); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); + } + + status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_A, DFM_MODE_8X); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); + } + + // start in short range mode + _mode = ARGUS_MODE_B; + set_mode(_mode); + + status = Argus_StartMeasurementTimer(_hnd, measurement_ready_callback); + + if (status != STATUS_OK) { + PX4_ERR("CONFIGURE status not okay: %i", (int)status); _state = STATE::STOP; ScheduleNow(); @@ -236,6 +276,8 @@ void AFBRS50::Run() case STATE::COLLECT: { // currently handeled by measurement_ready_callback + + UpdateMode(); } break; @@ -250,8 +292,6 @@ void AFBRS50::Run() break; } - UpdateMode(); - // backup schedule ScheduleDelayed(100_ms); } @@ -259,22 +299,40 @@ void AFBRS50::Run() void AFBRS50::UpdateMode() { // only update mode if _current_distance is a valid measurement - if (_current_distance > 0) { + if ((_current_distance > 0) && (_current_quality > 0)) { if ((_current_distance >= _long_range_threshold) && (_mode != ARGUS_MODE_A)) { // change to long range mode - _mode = ARGUS_MODE_A; - set_mode(_mode); - _measure_interval = (1000000 / LONG_RANGE_MODE_HZ); - ScheduleDelayed(1000_ms); // don't switch again for at least 1 second + argus_mode_t mode = ARGUS_MODE_A; + status_t status = set_mode(mode); + + if (status != STATUS_OK) { + PX4_ERR("set_mode status not okay: %i", (int)status); + } + + status = set_rate(LONG_RANGE_MODE_HZ); + + if (status != STATUS_OK) { + PX4_ERR("set_rate status not okay: %i", (int)status); + } } else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) { // change to short range mode - _mode = ARGUS_MODE_B; - set_mode(_mode); - _measure_interval = (1000000 / SHORT_RANGE_MODE_HZ); - ScheduleDelayed(1000_ms); // don't switch again for at least 1 second + argus_mode_t mode = ARGUS_MODE_B; + status_t status = set_mode(mode); + + if (status != STATUS_OK) { + PX4_ERR("set_mode status not okay: %i", (int)status); + } + + status = set_rate(SHORT_RANGE_MODE_HZ); + + if (status != STATUS_OK) { + PX4_ERR("set_rate status not okay: %i", (int)status); + } } + + ScheduleDelayed(1000_ms); // don't switch again for at least 1 second } } @@ -284,19 +342,76 @@ void AFBRS50::stop() ScheduleNow(); } +int AFBRS50::test() +{ + _testing = true; + + init(); + + return PX4_OK; +} + void AFBRS50::print_info() { perf_print_counter(_sample_perf); - get_mode(); + get_info(); } -void AFBRS50::set_mode(argus_mode_t mode) +status_t AFBRS50::set_mode(argus_mode_t mode) { - Argus_SetConfigurationMeasurementMode(_hnd, mode); - Argus_SetConfigurationDFMMode(_hnd, mode, DFM_MODE_8X); + while (Argus_GetStatus(_hnd) != STATUS_IDLE) { + px4_usleep(1_ms); + } + + status_t status = Argus_SetConfigurationMeasurementMode(_hnd, mode); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationMeasurementMode status not okay: %i", (int)status); + return status; + } + + argus_mode_t current_mode; + status = Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); + + if (status != STATUS_OK) { + PX4_ERR("Argus_GetConfigurationMeasurementMode status not okay: %i", (int)status); + return status; + + } else { + _mode = current_mode; + } + + return status; } -void AFBRS50::get_mode() +status_t AFBRS50::set_rate(uint32_t rate_hz) +{ + while (Argus_GetStatus(_hnd) != STATUS_IDLE) { + px4_usleep(1_ms); + } + + status_t status = Argus_SetConfigurationFrameTime(_hnd, (1000000 / rate_hz)); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationFrameTime status not okay: %i", (int)status); + return status; + } + + uint32_t current_rate; + status = Argus_GetConfigurationFrameTime(_hnd, ¤t_rate); + + if (status != STATUS_OK) { + PX4_ERR("Argus_GetConfigurationFrameTime status not okay: %i", (int)status); + return status; + + } else { + _measure_interval = current_rate; + } + + return status; +} + +void AFBRS50::get_info() { argus_mode_t current_mode; argus_dfm_mode_t dfm_mode; @@ -306,7 +421,7 @@ void AFBRS50::get_mode() PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance); PX4_INFO_RAW("mode: %d\n", current_mode); PX4_INFO_RAW("dfm mode: %d\n", dfm_mode); - PX4_INFO_RAW("rate: %d Hz\n", (1000000 / _measure_interval)); + PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval)); } namespace afbrs50 @@ -351,16 +466,44 @@ static int status() static int stop() { + if (g_dev == nullptr) { + PX4_ERR("driver not running"); + return PX4_ERROR; + } + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; - } PX4_INFO("driver stopped"); return PX4_OK; } +static int test(const uint8_t rotation) +{ + if (g_dev != nullptr) { + PX4_ERR("already started"); + return PX4_ERROR; + } + + g_dev = new AFBRS50(rotation); + + if (g_dev == nullptr) { + PX4_ERR("object instantiate failed"); + return PX4_ERROR; + } + + if (g_dev->test() != PX4_OK) { + PX4_ERR("driver test failed"); + delete g_dev; + g_dev = nullptr; + return PX4_ERROR; + } + + return PX4_OK; +} + static int usage() { PRINT_MODULE_DESCRIPTION( @@ -382,6 +525,7 @@ $ afbrs50 stop PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); PRINT_MODULE_USAGE_PARAM_INT('r', 25, 0, 25, "Sensor rotation - downward facing by default", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test driver"); PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); return PX4_OK; } @@ -421,6 +565,10 @@ extern "C" __EXPORT int afbrs50_main(int argc, char *argv[]) } else if (!strcmp(argv[myoptind], "stop")) { return afbrs50::stop(); + + } else if (!strcmp(argv[myoptind], "test")) { + return afbrs50::test(rotation); + } return afbrs50::usage(); diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp index 65af6c04d1..f7503b321b 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -63,11 +63,15 @@ public: */ void print_info(); - /** + /**50 * Stop the automatic measurement state machine. */ void stop(); + int test(); + + bool _testing = false; + private: void Run() override; @@ -77,8 +81,9 @@ private: static status_t measurement_ready_callback(status_t status, void *data); - void get_mode(); - void set_mode(argus_mode_t mode); + void get_info(); + status_t set_mode(argus_mode_t mode); + status_t set_rate(uint32_t rate_hz); argus_hnd_t *_hnd{nullptr}; argus_mode_t _mode{ARGUS_MODE_B}; // Short-Range @@ -96,8 +101,9 @@ private: perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")}; - int _measure_interval{1000000 / 50}; // 50Hz + uint32_t _measure_interval{1000000 / 50}; // 50Hz float _current_distance{0}; + int8_t _current_quality{0}; const float _short_range_threshold = 4.0; //meters const float _long_range_threshold = 6.0; //meters float _max_distance; diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/timer.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/timer.c index d627f0490b..8ace56ea92 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/timer.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/timer.c @@ -9,25 +9,25 @@ static struct hrt_call broadcom_hrt_call = {}; -static timer_cb_t timer_callback_; /*! Callback function for PIT timer */ +static timer_cb_t timer_callback_ = 0; /*! Callback function for PIT timer */ -static uint32_t period_us_; +static uint32_t period_us_ = 1000000 / 1000; // 1000Hz + +static bool isInitialized_ = false; /*! Storage for the callback parameter */ static void *callback_param_; static void broadcom_hrt_callout(void *arg) { - if (timer_callback_ != 0) { + if ((timer_callback_ != 0) && (period_us_ != 0) && (isInitialized_ == true)) { //timer_callback_(arg); timer_callback_(callback_param_); hrt_call_after(&broadcom_hrt_call, period_us_, broadcom_hrt_callout, callback_param_); - } -} -void Timer_Init(void) -{ - hrt_cancel(&broadcom_hrt_call); + } else { + hrt_cancel(&broadcom_hrt_call); + } } /*!*************************************************************************** @@ -72,7 +72,8 @@ status_t Timer_Start(uint32_t period, void *param) period_us_ = period; if (period != 0) { - hrt_call_after(&broadcom_hrt_call, period, broadcom_hrt_callout, param); + hrt_call_after(&broadcom_hrt_call, period_us_, broadcom_hrt_callout, callback_param_); + isInitialized_ = true; } else { hrt_cancel(&broadcom_hrt_call); @@ -91,6 +92,7 @@ status_t Timer_Stop(void *param) { period_us_ = 0; callback_param_ = 0; + isInitialized_ = false; hrt_cancel(&broadcom_hrt_call); return STATUS_OK; } @@ -111,10 +113,15 @@ status_t Timer_SetInterval(uint32_t dt_microseconds, void *param) { if (dt_microseconds != 0) { period_us_ = dt_microseconds; - hrt_call_after(&broadcom_hrt_call, dt_microseconds, broadcom_hrt_callout, param); + callback_param_ = param; + isInitialized_ = true; + hrt_call_after(&broadcom_hrt_call, period_us_, broadcom_hrt_callout, callback_param_); } else { hrt_cancel(&broadcom_hrt_call); + callback_param_ = 0; + period_us_ = 0; + isInitialized_ = false; } return STATUS_OK; diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c index 0a6a9f956f..8b877fa1d4 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c @@ -46,7 +46,8 @@ #include "platform/argus_nvm.h" #include "platform/argus_irq.h" -#include +#include +#include /******************************************************************************* * Definitions @@ -64,6 +65,7 @@ static status_t SpiConnectionTest(s2pi_slave_t slave); static status_t SpiInterruptTest(s2pi_slave_t slave); static status_t GpioModeTest(s2pi_slave_t slave); static status_t TimerTest(s2pi_slave_t slave); +static status_t PITTest(void); static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct); static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size); @@ -73,13 +75,17 @@ static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms); static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom); static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *RcoTrim); static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples); +static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n); +static void PIT_Callback(void *param); static void DataReadyCallback(void *param); +/// @cond EXTERN extern uint32_t EEPROM_ReadChipId(uint8_t const *eeprom); extern argus_module_version_t EEPROM_ReadModule(uint8_t const *eeprom); extern status_t EEPROM_Read(s2pi_slave_t slave, uint8_t address, uint8_t *data); extern uint8_t hamming_decode(uint8_t const *code, uint8_t *data); +/// @endcond /****************************************************************************** * Variables @@ -93,63 +99,76 @@ status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave) { status_t status = STATUS_OK; - print("########################################################\n"); - print("# Running HAL Verification Test - " HAL_TEST_VERSION "\n"); - print("########################################################\n\n"); + PX4_INFO_RAW("########################################################\n"); + PX4_INFO_RAW("# Running HAL Verification Test - " HAL_TEST_VERSION "\n"); + PX4_INFO_RAW("########################################################\n\n"); - print("1 > Timer Plausibility Test\n"); + PX4_INFO_RAW("1 > Timer Plausibility Test\n"); status = TimerPlausibilityTest(); if (status != STATUS_OK) { goto summary; } - print("1 > PASS\n\n"); + PX4_INFO_RAW("1 > PASS\n\n"); - print("2 > Timer Wraparound Test\n"); + PX4_INFO_RAW("2 > Timer Wraparound Test\n"); status = TimerWraparoundTest(); if (status != STATUS_OK) { goto summary; } - print("2 > PASS\n\n"); + PX4_INFO_RAW("2 > PASS\n\n"); - print("3 > SPI Connection Test\n"); + PX4_INFO_RAW("3 > SPI Connection Test\n"); status = SpiConnectionTest(spi_slave); if (status != STATUS_OK) { goto summary; } - print("3 > PASS\n\n"); + PX4_INFO_RAW("3 > PASS\n\n"); - print("4 > SPI Interrupt Test\n"); + PX4_INFO_RAW("4 > SPI Interrupt Test\n"); status = SpiInterruptTest(spi_slave); if (status != STATUS_OK) { goto summary; } - print("4 > PASS\n\n"); + PX4_INFO_RAW("4 > PASS\n\n"); - print("5 > GPIO Mode Test\n"); + PX4_INFO_RAW("5 > GPIO Mode Test\n"); status = GpioModeTest(spi_slave); if (status != STATUS_OK) { goto summary; } - print("5 > PASS\n\n"); + PX4_INFO_RAW("5 > PASS\n\n"); - print("6 > Timer Test\n"); + PX4_INFO_RAW("6 > Lifetime Counter Timer (LTC) Test\n"); status = TimerTest(spi_slave); if (status != STATUS_OK) { goto summary; } - print("6 > PASS\n\n"); + PX4_INFO_RAW("6 > PASS\n\n"); -summary: - print("########################################################\n"); + PX4_INFO_RAW("7 > Periodic Interrupt Timer (PIT) Test\n"); + status = PITTest(); - if (status != STATUS_OK) { - print("# FAIL: HAL Verification Test finished with error %d!\n", status); + if (status == ERROR_NOT_IMPLEMENTED) { + PX4_INFO_RAW("7 > SKIPPED (PIT is not implemented)\n\n"); } else { - print("# PASS: HAL Verification Test finished successfully!\n"); + if (status != STATUS_OK) { goto summary; } + + PX4_INFO_RAW("7 > PASS\n\n"); } - print("########################################################\n\n"); + +summary: + PX4_INFO_RAW("########################################################\n"); + + if (status != STATUS_OK) { + PX4_INFO_RAW("# FAIL: HAL Verification Test finished with error %d!\n", (int)status); + + } else { + PX4_INFO_RAW("# PASS: HAL Verification Test finished successfully!\n"); + } + + PX4_INFO_RAW("########################################################\n\n"); return status; } @@ -158,7 +177,7 @@ summary: * * @details This verifies that the counter values returned from the * #Timer_GetCounterValue function are valid. This means, the low - * counter value \p lct is within 0 and 999999 μs. + * counter value \p lct is within 0 and 999999 µs. * * @return Returns the \link #status_t status\endlink: * - #STATUS_OK on success. @@ -167,10 +186,10 @@ summary: static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct) { if (lct > 999999) { - error_log("Timer plausibility check:\n" - "The parameter \"lct\" of Timer_GetCounterValue() must always " - "be within 0 and 999999.\n" - "Current Values: hct = %d, lct = %d", hct, lct); + PX4_INFO_RAW("Timer plausibility check:\n" + "The parameter \"lct\" of Timer_GetCounterValue() must always " + "be within 0 and 999999.\n" + "Current Values: hct = %d, lct = %d", (uint)hct, (uint)lct); return ERROR_FAIL; } @@ -227,13 +246,13 @@ static status_t TimerPlausibilityTest(void) /* Either the hct value must have been increased or the lct value if the hct * value is still the same. */ if (!((hct1 > hct0) || ((hct1 == hct0) && (lct1 > lct0)))) { - error_log("Timer plausibility check: the elapsed time could not be " - "measured with the Timer_GetCounterValue() function; no time " - "has elapsed!\n" - "The delay was induced by the following code:\n" - "for (volatile uint32_t i = 0; i < 100000; ++i) __asm(\"nop\");\n", - "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", - hct0, lct0, hct1, lct1); + PX4_INFO_RAW("Timer plausibility check: the elapsed time could not be " + "measured with the Timer_GetCounterValue() function; no time " + "has elapsed!\n" + "The delay was induced by the following code:\n" + "for (volatile uint32_t i = 0; i < 100000; ++i) __asm(\"nop\");\n" + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); return ERROR_FAIL; } @@ -243,7 +262,7 @@ static status_t TimerPlausibilityTest(void) /*!*************************************************************************** * @brief Wraparound Test for the Timer HAL Implementation. * - * @details The LTC values must wrap from 999999 μs to 0 μs and increase the + * @details The LTC values must wrap from 999999 µs to 0 µs and increase the * seconds counter accordingly. This test verifies the correct wrapping * by consecutively calling the #Timer_GetCounterValue function until * at least 2 wraparound events have been occurred. @@ -283,6 +302,8 @@ static status_t TimerWraparoundTest(void) uint32_t hct2 = hct0 + n; uint32_t lct2 = lct0; + px4_usleep(20000); + /* Periodically read timer values. From previous tests we * already know the timer value is increasing. */ while (hct0 < hct2 || lct0 < lct2) { @@ -302,16 +323,18 @@ static status_t TimerWraparoundTest(void) * than previous one. */ if (!(((hct1 == hct0 + 1) && (lct1 < lct0)) || ((hct1 == hct0) && (lct1 >= lct0)))) { - error_log("Timer plausibility check: the wraparound of \"lct\" or " - "\"hct\" parameters of the Timer_GetCounterValue() " - "function was not handled correctly!\n" - "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", - hct0, lct0, hct1, lct1); + PX4_INFO_RAW("Timer plausibility check: the wraparound of \"lct\" or " + "\"hct\" parameters of the Timer_GetCounterValue() " + "function was not handled correctly!\n" + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); return ERROR_FAIL; } hct0 = hct1; lct0 = lct1; + + px4_usleep(20000); } return STATUS_OK; @@ -352,8 +375,8 @@ static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size) status_t status = S2PI_TransferFrame(slave, data, data, size, 0, 0); if (status < STATUS_OK) { - error_log("SPI transfer failed! The call to S2PI_TransferFrame " - "yielded error code: %d", status); + PX4_INFO_RAW("SPI transfer failed! The call to S2PI_TransferFrame " + "yielded error code: %d", (int)status); return status; } @@ -367,16 +390,16 @@ static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size) status = S2PI_GetStatus(); if (status < STATUS_OK) { - error_log("SPI transfer failed! The call to S2PI_GetStatus " - "yielded error code: %d", status); + PX4_INFO_RAW("SPI transfer failed! The call to S2PI_GetStatus " + "yielded error code: %d", (int)status); S2PI_Abort(); return status; } if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - error_log("SPI transfer failed! The operation did not finished " - "within %d ms. This may also be caused by an invalid " - "timer implementation!", timeout_ms); + PX4_INFO_RAW("SPI transfer failed! The operation did not finished " + "within %u ms. This may also be caused by an invalid " + "timer implementation!", (uint)timeout_ms); return ERROR_TIMEOUT; } } while (status == STATUS_BUSY); @@ -425,7 +448,7 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) status = SPITransferSync(slave, data, 17U); if (status < STATUS_OK) { - error_log("SPI connection test failed!"); + PX4_INFO_RAW("SPI connection test failed!"); return status; } @@ -437,17 +460,17 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) status = SPITransferSync(slave, data, 17U); if (status < STATUS_OK) { - error_log("SPI connection test failed!"); + PX4_INFO_RAW("SPI connection test failed!"); return status; } /* Verify the read pattern. */ for (uint8_t i = 1; i < 17U; ++i) { if (data[i] != i) { - error_log("SPI connection test failed!\n" - "Verification of read data is invalid!\n" - "read_data[%d] = %d, but expected was %d", - i, data[i], i); + PX4_INFO_RAW("SPI connection test failed!\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data[i], i); return ERROR_FAIL; } } @@ -468,9 +491,9 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) *****************************************************************************/ static void DataReadyCallback(void *param) { - irqstate_t irqstate_flags = px4_enter_critical_section(); + IRQ_LOCK(); *((bool *) param) = true; - px4_leave_critical_section(irqstate_flags); + IRQ_UNLOCK(); } /*!*************************************************************************** @@ -510,7 +533,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status_t status = SPITransferSync(slave, d1, sizeof(d1)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -518,7 +541,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d2, sizeof(d2)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -526,7 +549,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d3, sizeof(d3)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -534,7 +557,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d4, sizeof(d4)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -542,7 +565,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d5, sizeof(d5)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -550,7 +573,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d6, sizeof(d6)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -558,7 +581,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d7, sizeof(d7)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -566,7 +589,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d8, sizeof(d8)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -574,7 +597,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d9, sizeof(d9)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -582,7 +605,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d10, sizeof(d10)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -590,7 +613,7 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) status = SPITransferSync(slave, d11, sizeof(d11)); if (status < STATUS_OK) { - error_log("Device configuration failed!"); + PX4_INFO_RAW("Device configuration failed!"); return status; } @@ -631,7 +654,7 @@ static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples) status_t status = SPITransferSync(slave, d, sizeof(d)); if (status < STATUS_OK) { - error_log("Trigger measurement failed!"); + PX4_INFO_RAW("Trigger measurement failed!"); return status; } @@ -672,8 +695,8 @@ static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms) while (S2PI_ReadIrqPin(slave)) { if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - error_log("SPI interrupt test failed! The S2PI_ReadIrqPin did not " - "determine an pending interrupt within %d ms.", timeout_ms); + PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did not " + "determine an pending interrupt within %u ms.", (uint)timeout_ms); return ERROR_TIMEOUT; } } @@ -740,16 +763,16 @@ static status_t SpiInterruptTest(s2pi_slave_t slave) status_t status = S2PI_SetIrqCallback(slave, DataReadyCallback, (void *)&isDataReady); if (status < STATUS_OK) { - error_log("SPI interrupt test failed! The call to S2PI_SetIrqCallback " - "yielded error code: %d", status); + PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " + "yielded error code: %d", (int)status); return status; } /* Check if IRQ is not yet pending. */ if (S2PI_ReadIrqPin(slave) == 0) { - error_log("SPI interrupt test failed! The S2PI_ReadIrqPin did " - "return 0 but no interrupt is pending since no " - "measurements are executed yet!"); + PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did " + "return 0 but no interrupt is pending since no " + "measurements are executed yet!"); return ERROR_FAIL; }; @@ -757,7 +780,7 @@ static status_t SpiInterruptTest(s2pi_slave_t slave) status = ConfigureDevice(slave, 0); if (status < STATUS_OK) { - error_log("SPI interrupt test failed!"); + PX4_INFO_RAW("SPI interrupt test failed!"); return status; } @@ -765,7 +788,7 @@ static status_t SpiInterruptTest(s2pi_slave_t slave) status = TriggerMeasurement(slave, 0); if (status < STATUS_OK) { - error_log("SPI interrupt test failed!"); + PX4_INFO_RAW("SPI interrupt test failed!"); return status; } @@ -776,15 +799,15 @@ static status_t SpiInterruptTest(s2pi_slave_t slave) status = AwaitDataReady(slave, timeout_ms); if (status < STATUS_OK) { - error_log("SPI interrupt test failed!"); + PX4_INFO_RAW("SPI interrupt test failed!"); return status; } /* Wait for Interrupt using the callback method. */ while (!isDataReady) { if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - error_log("SPI interrupt test failed! The IRQ callback was not " - "invoked within %d ms.", timeout_ms); + PX4_INFO_RAW("SPI interrupt test failed! The IRQ callback was not " + "invoked within %u ms.", (uint)timeout_ms); return ERROR_TIMEOUT; } } @@ -793,8 +816,8 @@ static status_t SpiInterruptTest(s2pi_slave_t slave) status = S2PI_SetIrqCallback(slave, 0, 0); if (status < STATUS_OK) { - error_log("SPI interrupt test failed! The call to S2PI_SetIrqCallback " - "with null pointers yielded error code: %d", status); + PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " + "with null pointers yielded error code: %d", (int)status); return status; } @@ -832,8 +855,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) status_t status = SPITransferSync(slave, d1, sizeof(d1)); if (status < STATUS_OK) { - error_log("EEPROM readout failed (enable EEPROM), " - "error code: %d", status); + PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " + "error code: %d", (int)status); return status; } @@ -844,8 +867,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) status = EEPROM_Read(slave, address, &data[address]); if (status != STATUS_OK) { - error_log("EEPROM readout failed @ address 0x%02x, " - "error code: %d!", address, status); + PX4_INFO_RAW("EEPROM readout failed @ address 0x%02x, " + "error code: %d!", address, (int)status); return status; } } @@ -855,8 +878,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) status = SPITransferSync(slave, d2, sizeof(d2)); if (status < STATUS_OK) { - error_log("EEPROM readout failed (enable EEPROM), " - "error code: %d", status); + PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " + "error code: %d", (int)status); return status; } @@ -864,8 +887,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) uint8_t err = hamming_decode(data, eeprom); if (err != 0) { - error_log("EEPROM readout failed! Failed to decoding " - "Hamming weight (error: %d)!", err); + PX4_INFO_RAW("EEPROM readout failed! Failed to decoding " + "Hamming weight (error: %d)!", err); return STATUS_ARGUS_EEPROM_BIT_ERROR; } @@ -911,29 +934,29 @@ static status_t GpioModeTest(s2pi_slave_t slave) status_t status = ReadEEPROM(slave, eeprom1); if (status < STATUS_OK) { - error_log("GPIO mode test failed (1st attempt)!"); + PX4_INFO_RAW("GPIO mode test failed (1st attempt)!"); return status; } status = ReadEEPROM(slave, eeprom2); if (status < STATUS_OK) { - error_log("GPIO mode test failed (2nd attempt)!"); + PX4_INFO_RAW("GPIO mode test failed (2nd attempt)!"); return status; } status = ReadEEPROM(slave, eeprom3); if (status < STATUS_OK) { - error_log("GPIO mode test failed (3rd attempt)!"); + PX4_INFO_RAW("GPIO mode test failed (3rd attempt)!"); return status; } /* Verify EEPROM data. */ if ((memcmp(eeprom1, eeprom2, 16) != 0) || (memcmp(eeprom1, eeprom3, 16) != 0)) { - error_log("GPIO Mode test failed (data comparison)!\n" - "The data from 3 distinct EEPROM readout does not match!"); + PX4_INFO_RAW("GPIO Mode test failed (data comparison)!\n" + "The data from 3 distinct EEPROM readout does not match!"); return ERROR_FAIL; } @@ -942,14 +965,14 @@ static status_t GpioModeTest(s2pi_slave_t slave) argus_module_version_t module = EEPROM_ReadModule(eeprom1); if (chipID == 0 || module == 0) { - error_log("GPIO Mode test failed (data verification)!\n" - "Invalid EEPROM data: Module = %d; Chip ID = %d!", module, chipID); + PX4_INFO_RAW("GPIO Mode test failed (data verification)!\n" + "Invalid EEPROM data: Module = %d; Chip ID = %u!", module, (uint)chipID); return ERROR_FAIL; } - print("EEPROM Readout succeeded!\n"); - print("- Module: %d\n", module); - print("- Device ID: %d\n", chipID); + PX4_INFO_RAW("EEPROM Readout succeeded!\n"); + PX4_INFO_RAW("- Module: %d\n", module); + PX4_INFO_RAW("- Device ID: %u\n", (uint)chipID); return STATUS_OK; } @@ -997,7 +1020,7 @@ static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) case MODULE_NONE: /* Uncalibrated module; use all 0 data. */ default: - error_log("EEPROM Readout failed! Unknown module number: %d", module); + PX4_INFO_RAW("EEPROM Readout failed! Unknown module number: %d", module); return ERROR_ARGUS_UNKNOWN_MODULE; } @@ -1036,9 +1059,9 @@ static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples) status_t status = TriggerMeasurement(slave, samples); if (status < STATUS_OK) { - error_log("Speed test failed!\n" - "Call to TransferFrame returned code: %d", - status); + PX4_INFO_RAW("Speed test failed!\n" + "Call to TransferFrame returned code: %d", + (int)status); return status; } @@ -1046,9 +1069,9 @@ static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples) status = AwaitDataReady(slave, 300); if (status < STATUS_OK) { - error_log("Speed test failed!\n" - "SPI Read IRQ pin didn't raised, timeout activated at 200ms, error code: %d", - status); + PX4_INFO_RAW("Speed test failed!\n" + "SPI Read IRQ pin didn't raised, timeout activated at 200ms, error code: %d", + (int)status); return status; } @@ -1089,7 +1112,7 @@ static status_t TimerTest(s2pi_slave_t slave) /* Test parameter configuration: *****************************************/ const int8_t n = 10; // The number of measurements. const uint32_t ds = 100; // The step size in averaging samples. - const float exp_slope = 102.4; // Expected slope is 102.4 μs / phase / sample + const float exp_slope = 102.4; // Expected slope is 102.4 µs / phase / sample const float rel_slope_error = 3e-2; // Relative slope tolerance is 3%. /*************************************************************************/ @@ -1098,19 +1121,19 @@ static status_t TimerTest(s2pi_slave_t slave) status_t status = ReadRcoTrim(slave, &RcoTrim); if (status < STATUS_OK) { - error_log("Timer test failed!\n" - "EEPROM Read test returned code: %d", status); + PX4_INFO_RAW("Timer test failed!\n" + "EEPROM Read test returned code: %d", (int)status); return status; } - print("RCOTrim = %d\n", RcoTrim); + PX4_INFO_RAW("RCOTrim = %d\n", RcoTrim); /* Configure the device with calibrated RCO to 24MHz. */ status = ConfigureDevice(slave, RcoTrim); if (status < STATUS_OK) { - error_log("Timer test failed!\n" - "Configuration test returned code: %d", status); + PX4_INFO_RAW("Timer test failed!\n" + "Configuration test returned code: %d", (int)status); return status; } @@ -1122,9 +1145,9 @@ static status_t TimerTest(s2pi_slave_t slave) float x2sum = 0; float xysum = 0; - print("+-------+---------+------------+\n"); - print("| count | samples | elapsed us |\n"); - print("+-------+---------+------------+\n"); + PX4_INFO_RAW("+-------+---------+------------+\n"); + PX4_INFO_RAW("| count | samples | elapsed us |\n"); + PX4_INFO_RAW("+-------+---------+------------+\n"); for (uint8_t i = 1; i <= n; ++i) { ltc_t start; @@ -1134,9 +1157,9 @@ static status_t TimerTest(s2pi_slave_t slave) status = RunMeasurement(slave, samples); if (status < STATUS_OK) { - error_log("Timer test failed!\n" - "Run measurement returned code: %d", - status); + PX4_INFO_RAW("Timer test failed!\n" + "Run measurement returned code: %d", + (int)status); return status; } @@ -1147,30 +1170,229 @@ static status_t TimerTest(s2pi_slave_t slave) x2sum += (float) samples * samples; xysum += (float) samples * elapsed_usec; - print("| %5d | %7d | %10d |\n", i, samples, elapsed_usec); + PX4_INFO_RAW("| %5d | %7d | %10d |\n", i, samples, (uint)elapsed_usec); } - print("+-------+---------+------------+\n"); + PX4_INFO_RAW("+-------+---------+------------+\n"); const float slope = (n * xysum - xsum * ysum) / (n * x2sum - xsum * xsum); const float intercept = (ysum * x2sum - xsum * xysum) / (n * x2sum - xsum * xsum); - print("Linear Regression: y(x) = %dE-7 sec * x + %dE-7 sec\n", - (int)(10 * slope), (int)(10 * intercept)); + PX4_INFO_RAW("Linear Regression: y(x) = %dE-7 sec * x + %dE-7 sec\n", + (int)(10 * slope), (int)(10 * intercept)); /* Check the error of the slope. */ const float max_slope = exp_slope * (1.f + rel_slope_error); const float min_slope = exp_slope * (1.f - rel_slope_error); if (slope > max_slope || slope < min_slope) { - error_log("Time test failed!\n" - "The measured time slope does not match the expected value! " - "(actual: %dE-7, expected: %dE-7, min: %dE-7, max: %dE-7)\n", - (int)(10 * slope), (int)(10 * exp_slope), - (int)(10 * min_slope), (int)(10 * max_slope)); + PX4_INFO_RAW("Time test failed!\n" + "The measured time slope does not match the expected value! " + "(actual: %dE-7, expected: %dE-7, min: %dE-7, max: %dE-7)\n", + (int)(10 * slope), (int)(10 * exp_slope), + (int)(10 * min_slope), (int)(10 * max_slope)); return ERROR_FAIL; } return STATUS_OK; } + +/*!*************************************************************************** + * @brief Data structure for the PIT test. + * + * @details Contains data that is required by the PIT timer test. + *****************************************************************************/ +typedef struct { + /*! The number of PIT callback events. */ + volatile uint32_t n; + + /*! The time stamp of the first callback event. */ + ltc_t t_first; + + /*! The time stamp of the last callback event. */ + ltc_t t_last; + +} pit_data_t; + + + +/*!*************************************************************************** + * @brief Callback function invoked by the PIT. + * + * @details The function that is invoked every time a specified interval elapses. + * An abstract parameter is passed to the function whenever it is called. + * + * This implementation collects callback time stamps and counts the + * number of callback events using the abstract parameter. + * + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. + *****************************************************************************/ +static void PIT_Callback(void *param) +{ + pit_data_t *data = (pit_data_t *) param; + + if (data->n == 0) { + Time_GetNow(&data->t_first); + data->t_last = data->t_first; + + } else { + Time_GetNow(&data->t_last); + } + + data->n++; +} + +/*!*************************************************************************** + * @brief Executes a PIT measurement and verifies the callback interval. + * + * @details The function configures the PIT with a given interval and waits + * several callback events to happen. In each callback event, the + * elapsed time is measured and the number of calls are counted. + * Finally, the average interrupt period is compared with the + * lifetime timer that has been already verified in a previous test + * (see #Timer_Test). + * + * @param exp_dt_us The expected timer interval in microseconds. + * @param n The number of PIT events to await. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval return any + * negative status. + *****************************************************************************/ +static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n) +{ + /* Test parameter configuration: *****************************************/ + const float rel_dt_error = 1e-3; // Relative timer interval tolerance is 0.1%. + const float abs_dt_error = 1.0; // Absolute timer interval tolerance is 1us. + /*************************************************************************/ + float dt = exp_dt_us * rel_dt_error; + + if (dt < abs_dt_error) { dt = abs_dt_error; } + + const float max_dt = exp_dt_us + dt; + const float min_dt = exp_dt_us - dt; + /*************************************************************************/ + + /* Setup the PIT callback with specified interval. */ + pit_data_t data = { 0 }; + status_t status = Timer_SetInterval(exp_dt_us, &data); + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", (int)status); + return status; + } + + /* Wait until n PIT callback have been happened. */ + uint32_t timeout_us = (n + 1) * exp_dt_us; + ltc_t start; + Time_GetNow(&start); + + while (data.n < n) { + if (Time_CheckTimeoutUSec(&start, timeout_us)) { + PX4_INFO_RAW("PIT test failed!\n" + "Waiting for the PIT interrupt events yielded a timeout."); + status = ERROR_TIMEOUT; + break; + } + } + + if (status == STATUS_OK) { + /* Disable the PIT timer callback. */ + status = Timer_SetInterval(0, &data); + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", (int)status); + } + } + + if (status == STATUS_OK) { + /* Check if PIT callback is not invoked any more. */ + timeout_us = 2 * exp_dt_us; + Time_GetNow(&start); + + while (!Time_CheckTimeoutUSec(&start, timeout_us)) { __asm("nop"); } + + if (data.n > n) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetInterval has been called after it was disabled."); + status = ERROR_FAIL; + } + } + + /* Verify the measured average timer interval. */ + const float act_dt_us = Time_DiffUSec(&data.t_first, &data.t_last) / (n - 1); + + if (status == STATUS_OK && (act_dt_us > max_dt || act_dt_us < min_dt)) { + PX4_INFO_RAW("PIT test failed!\n" + "The measured timer interval does not match the expected value!\n"); + status = ERROR_FAIL; + } + + PX4_INFO_RAW("PIT Test Results:\n" + " - event count: %u\n" + " - actual interval: %d us\n" + " - expected interval: %d us, min: %d us, max: %d us\n", + (uint)data.n, (int)act_dt_us, (uint)exp_dt_us, (int)min_dt, (int)max_dt); + + return status; +} + +/*!*************************************************************************** + * @brief Test for PIT HAL Implementation by comparing timings to the device. + * + * @details The test verifies the timer HAL implementation by comparing the + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not + * implemented and the test is skipped. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval or + * #Timer_SetCallback return any negative status. + *****************************************************************************/ +static status_t PITTest(void) +{ + status_t status = Timer_SetCallback(PIT_Callback); + + if (status == ERROR_NOT_IMPLEMENTED) { return status; } + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetCallback returned status code: %d", (int)status); + return status; + } + + status = RunPITTest(10000, 10); + + if (status < STATUS_OK) { return status; } + + status = RunPITTest(333, 1000); + + if (status < STATUS_OK) { return status; } + + status = RunPITTest(100000, 5); + + if (status < STATUS_OK) { return status; } + + status = Timer_SetCallback(0); + + if (status < STATUS_OK) { + PX4_INFO_RAW("PIT test failed!\n" + "Timer_SetCallback to 0 returned status code: %d", (int)status); + return status; + } + + return STATUS_OK; +} diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h index 1ac16fcaa5..5af6660dfa 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h @@ -37,9 +37,20 @@ #ifndef ARGUS_HAL_TEST_H #define ARGUS_HAL_TEST_H -__BEGIN_DECLS +#ifdef __cplusplus +extern "C" { +#endif + /*!*************************************************************************** + * @defgroup argustest HAL Self Test + * + * @brief A test module to verify implementation of the HAL. + * + * @details A series of automated tests that can be executed on the target + * platform in order to verify the implementation of the HAL that + * are required by the API. + * * @addtogroup argustest * @{ *****************************************************************************/ @@ -50,117 +61,127 @@ __BEGIN_DECLS * @brief Version number of the HAL Self Test. * * @details Changes: - * - * - v1.0: + * * v1.0: * - Initial release. - * . - * - v1.1: + * * v1.1: * - Added additional print output. * - Increased tolerance for timer test to 3%. * - Fixed callback issue by disabling it after IRQ test. - * . + * * v1.1: + * - Added PIT test cases. *****************************************************************************/ -#define HAL_TEST_VERSION "v1.1" +#define HAL_TEST_VERSION "v1.2" /*!*************************************************************************** * @brief Executes a series of tests in order to verify the HAL implementation. * - * @details A series of automated tests are executed in order to verify the - * implementation of the HAL required by the API. + * @details A series of automated tests are executed on the target platform in + * order to verify the implementation of the HAL that are required by + * the API. * - * The following tests are executed: + * Each test will write an error description via the print (i.e. UART) + * function that shows what went wrong. Also an corresponding status is + * returned in case no print functionality is available. * - * 1) Timer Plausibility Test: + * The following tests are executed: * - * Rudimentary tests of the lifetime counter (LTC) implementation. - * This verifies that the LTC is running by checking if the returned - * values of two consecutive calls to the #Timer_GetCounterValue - * function are ascending. An artificial delay using the NOP operation - * is induced such that the timer is not read to fast. + * **1) Timer Plausibility Test:** * - * 2) Timer Wraparound Test: + * Rudimentary tests of the lifetime counter (LTC) implementation. + * This verifies that the LTC is running by checking if the returned + * values of two consecutive calls to the #Timer_GetCounterValue + * function are ascending. An artificial delay using the NOP operation + * is induced such that the timer is not read to fast. * - * The LTC values must wrap from 999999 μs to 0 μs and increase the - * seconds counter accordingly. This test verifies the correct wrapping - * by consecutively calling the #Timer_GetCounterValue function until - * at least 2 wraparound events have been occurred. + * **2) Timer Wraparound Test:** * - * 3) SPI Connection Test: + * The LTC values must wrap from 999999 µs to 0 µs and increase the + * seconds counter accordingly. This test verifies the correct wrapping + * by consecutively calling the #Timer_GetCounterValue function until + * at least 2 wraparound events have been occurred. * - * This test verifies the basic functionality of the SPI interface. - * The test utilizes the devices laser pattern register, which can - * be freely programmed by any 128-bit pattern. Thus, it writes a byte - * sequence and reads back the written values on the consecutive SPI - * access. + * **3) SPI Connection Test:** * - * 4) SPI Interrupt Test: + * This test verifies the basic functionality of the SPI interface. + * The test utilizes the devices laser pattern register, which can + * be freely programmed by any 128-bit pattern. Thus, it writes a byte + * sequence and reads back the written values on the consecutive SPI + * access. * - * This test verifies the correct implementation of the device - * integration finished interrupt callback. Therefore it configures - * the device with a minimal setup to run a pseudo measurement that - * does not emit any laser light. + * **4) SPI Interrupt Test:** * - * Note that this test does verify the GPIO interrupt that occurs - * whenever the device has finished the integration/measurement and - * new data is waiting to be read from the device. This does not test - * the interrupt that is triggered when the SPI transfer has finished. + * This test verifies the correct implementation of the device + * integration finished interrupt callback. Therefore it configures + * the device with a minimal setup to run a pseudo measurement that + * does not emit any laser light. * - * The data ready interrupt implies two S2PI layer functions that - * are tested in this test: The #S2PI_SetIrqCallback function installs - * a callback function that is invoked whenever the IRQ occurs. - * The IRQ can be delayed due to higher priority task, e.g. from the - * user code. It is essential for the laser safety timeout algorithm - * to determine the device ready signal as fast as possible, another - * method is implemented to read if the IRQ is pending but the - * callback has not been reset yet. This is what the #S2PI_ReadIrqPin - * function is for. + * Note that this test does verify the GPIO interrupt that occurs + * whenever the device has finished the integration/measurement and + * new data is waiting to be read from the device. This does not test + * the interrupt that is triggered when the SPI transfer has finished. * - * 5) GPIO Mode Test: + * The data ready interrupt implies two S2PI layer functions that + * are tested in this test: The #S2PI_SetIrqCallback function installs + * a callback function that is invoked whenever the IRQ occurs. + * The IRQ can be delayed due to higher priority task, e.g. from the + * user code. It is essential for the laser safety timeout algorithm + * to determine the device ready signal as fast as possible, another + * method is implemented to read if the IRQ is pending but the + * callback has not been reset yet. This is what the #S2PI_ReadIrqPin + * function is for. * - * This test verifies the GPIO mode of the S2PI HAL module. This is - * done by leveraging the EEPROM readout sequence that accesses the - * devices EEPROM via a software protocol that depends on the GPIO - * mode. + * **5) GPIO Mode Test:** * - * This the requires several steps, most of them are already verified - * in previous tests: - * - Basic device configuration and enable EEPROM. - * - Read EERPOM via GPIO mode and apply Hamming weight - * - Repeat several times (to eliminate random readout issues). - * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c) - * - Check if Module Number and Chip ID is not 0 + * This test verifies the GPIO mode of the S2PI HAL module. This is + * done by leveraging the EEPROM readout sequence that accesses the + * devices EEPROM via a software protocol that depends on the GPIO + * mode. * - * 6) Timer Test: + * This the requires several steps, most of them are already verified + * in previous tests: * - * The test verifies the timer HAL implementation by comparing the - * timings to the AFBR-S50 device as a reference. - * Therefore several measurement are executed on the device, each with - * a different averaging sample count. The elapsed time increases - * linearly with the number of averaging samples. In order to remove - * the time for software/setup, a linear regression fit is applied to - * the measurement results and only the slope is considered for the - * result. A delta of 102.4 microseconds per sample is expected. - * If the measured delta per sample is within an specified error range, - * the timer implementation is considered correct. + * - Basic device configuration and enable EEPROM. + * - Read EERPOM via GPIO mode and apply Hamming weight. + * - Repeat several times (to eliminate random readout issues). + * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c). + * - Check if Module Number and Chip ID is not 0. * - * ------------------------------------------------------------------- + * **6) Timer Test for Lifetime Counter:** * - * Each test will write an error description via the print (i.e. UART) - * function that shows what went wrong. Also an corresponding status is - * returned in case no print functionality is available. + * The test verifies the lifetime counter timer HAL implementation by + * comparing the timings to the AFBR-S50 device as a reference. + * Therefore several measurement are executed on the device, each with + * a different averaging sample count. The elapsed time increases + * linearly with the number of averaging samples. In order to remove + * the time for software/setup, a linear regression fit is applied to + * the measurement results and only the slope is considered for the + * result. A delta of 102.4 microseconds per sample is expected. + * If the measured delta per sample is within an specified error range, + * the timer implementation is considered correct. + * + * **7) Timer Test for Periodic Interrupt Timer:** + * + * The test verifies the correct implementation of the periodic + * interrupt timer (PIT). It sets different intervals and waits for + * a certain number of interrupts to happen. Each interrupt event + * is counted and the time between the first and the last interrupt + * is measured. Finally, the measured interval is compared to the + * expectations. * * * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ - * lines. This is actually just a number that is passed - * to the SPI interface to distinct for multiple SPI slave - * devices. Note that the slave must be not equal to 0, - * since is reserved for error handling. + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. * * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave); -__END_DECLS +#ifdef __cplusplus +} +#endif /*! @} */ #endif /* ARGUS_CAL_API_H */