diff --git a/Buildscripts/release-sdk-modules.txt b/Buildscripts/release-sdk-modules.txt index 57dcb0c66..be5570f9c 100644 --- a/Buildscripts/release-sdk-modules.txt +++ b/Buildscripts/release-sdk-modules.txt @@ -5,6 +5,7 @@ cjson-module crypt-module gps-module http-module +location-module lvgl-module lvgl-window-manager-module service-module diff --git a/TactilityKernel/include/tactility/drivers/compass.h b/TactilityKernel/include/tactility/drivers/compass.h new file mode 100644 index 000000000..e7be563f1 --- /dev/null +++ b/TactilityKernel/include/tactility/drivers/compass.h @@ -0,0 +1,62 @@ +// SPDX-License-Identifier: Apache-2.0 +#pragma once + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * Magnetic field strength in µT, in the device frame (right-handed): + * +x points to the front of the device, +y to its left and +z up. + * Drivers transform the sensor axes into this frame. + * A level device facing magnetic north reads +x, and facing east reads +y. + */ +struct CompassData { + float x, y, z; +}; + +/** + * @brief API for compass (magnetometer) drivers. + * + * @note read_field() is required. read_heading() is optional: a driver that can't service it + * leaves it NULL, and compass_read_heading() derives the heading from the x and y axes. + */ +struct CompassApi { + /** + * @brief Reads the current magnetic field. + * @param[in] device the compass device + * @param[out] data pointer to store the current reading + * @retval ERROR_NONE on success + */ + error_t (*read_field)(struct Device* device, struct CompassData* data); + + /** + * @brief Reads the current heading. + * @param[in] device the compass device + * @param[out] heading_deg pointer to store the heading, in degrees clockwise from magnetic north [0, 360) + * @retval ERROR_NONE on success + */ + error_t (*read_heading)(struct Device* device, float* heading_deg); +}; + +/** + * @brief Reads the current magnetic field using the specified compass device, in µT. + */ +error_t compass_read_field(struct Device* device, struct CompassData* data); + +/** + * @brief Reads the current heading using the specified compass device. + * Assumes the device is held level when the driver has no heading of its own. + * @retval ERROR_INVALID_STATE when the driver has no heading of its own and the horizontal field is zero + * @param[out] heading_deg the heading in degrees clockwise from magnetic north [0, 360) + */ +error_t compass_read_heading(struct Device* device, float* heading_deg); + +extern const struct DeviceType COMPASS_TYPE; + +#ifdef __cplusplus +} +#endif diff --git a/TactilityKernel/source/drivers/compass.cpp b/TactilityKernel/source/drivers/compass.cpp new file mode 100644 index 000000000..407c3abf2 --- /dev/null +++ b/TactilityKernel/source/drivers/compass.cpp @@ -0,0 +1,39 @@ +// SPDX-License-Identifier: Apache-2.0 +#include +#include + +#include + +#define COMPASS_DRIVER_API(driver) ((struct CompassApi*)driver->api) + +extern "C" { + +error_t compass_read_field(Device* device, CompassData* data) { + const auto* driver = device_get_driver(device); + return COMPASS_DRIVER_API(driver)->read_field(device, data); +} + +error_t compass_read_heading(Device* device, float* heading_deg) { + const auto* driver = device_get_driver(device); + auto* api = COMPASS_DRIVER_API(driver); + if (api->read_heading) return api->read_heading(device, heading_deg); + + CompassData data; + error_t error = api->read_field(device, &data); + if (error != ERROR_NONE) return error; + if (data.x == 0.0f && data.y == 0.0f) return ERROR_INVALID_STATE; + + float heading = std::atan2(data.y, data.x) * (180.0f / static_cast(M_PI)); + if (heading < 0.0f) { + heading += 360.0f; + if (heading >= 360.0f) heading = 0.0f; + } + *heading_deg = heading; + return ERROR_NONE; +} + +const DeviceType COMPASS_TYPE { + .name = "compass" +}; + +} diff --git a/TactilityKernel/source/symbols.c b/TactilityKernel/source/symbols.c index 9cef92f4c..9cd2b5cff 100644 --- a/TactilityKernel/source/symbols.c +++ b/TactilityKernel/source/symbols.c @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -261,6 +262,10 @@ const struct ModuleSymbol KERNEL_SYMBOLS[] = { DEFINE_MODULE_SYMBOL(I2S_CONTROLLER_TYPE), // drivers/i8080_controller DEFINE_MODULE_SYMBOL(I8080_CONTROLLER_TYPE), + // drivers/compass + DEFINE_MODULE_SYMBOL(compass_read_field), + DEFINE_MODULE_SYMBOL(compass_read_heading), + DEFINE_MODULE_SYMBOL(COMPASS_TYPE), // drivers/imu DEFINE_MODULE_SYMBOL(imu_read_accel), DEFINE_MODULE_SYMBOL(imu_read_gyro), diff --git a/TactilityKernel/tests/source/compass_test.cpp b/TactilityKernel/tests/source/compass_test.cpp new file mode 100644 index 000000000..e19f1f5f9 --- /dev/null +++ b/TactilityKernel/tests/source/compass_test.cpp @@ -0,0 +1,161 @@ +#include "doctest.h" + +#include + +#include +#include +#include + +namespace { + +CompassData mock_field = {}; +error_t mock_field_error = ERROR_NONE; +float mock_heading = 0.0f; + +int start(Device*) { return ERROR_NONE; } +int stop(Device*) { return ERROR_NONE; } + +error_t read_field(Device*, CompassData* data) { + if (mock_field_error != ERROR_NONE) return mock_field_error; + *data = mock_field; + return ERROR_NONE; +} + +error_t read_heading(Device*, float* heading_deg) { + *heading_deg = mock_heading; + return ERROR_NONE; +} + +const CompassApi derived_api = { .read_field = read_field, .read_heading = nullptr }; +const CompassApi native_api = { .read_field = read_field, .read_heading = read_heading }; + +Driver derived_driver = { + .name = "compass_test_derived", + .compatible = (const char*[]) { "compass_test_derived", nullptr }, + .start_device = start, + .stop_device = stop, + .api = &derived_api, + .device_type = &COMPASS_TYPE, + .internal = nullptr, +}; + +Driver native_driver = { + .name = "compass_test_native", + .compatible = (const char*[]) { "compass_test_native", nullptr }, + .start_device = start, + .stop_device = stop, + .api = &native_api, + .device_type = &COMPASS_TYPE, + .internal = nullptr, +}; + +/** Runs a body with a device bound to the driver */ +template +void with_device(Driver* driver, Body body) { + Device device = { .name = "compass_test_device", .config = nullptr, .parent = nullptr }; + REQUIRE_EQ(driver_construct_add(driver), ERROR_NONE); + REQUIRE_EQ(device_construct(&device), ERROR_NONE); + device_set_driver(&device, driver); + REQUIRE_EQ(device_add(&device), ERROR_NONE); + + body(&device); + + CHECK_EQ(device_remove(&device), ERROR_NONE); + CHECK_EQ(device_destruct(&device), ERROR_NONE); + CHECK_EQ(driver_remove_destruct(driver), ERROR_NONE); +} + +float derived_heading(float x, float y) { + mock_field = { x, y, 0.0f }; + float heading = -1.0f; + with_device(&derived_driver, [&](Device* device) { + CHECK_EQ(compass_read_heading(device, &heading), ERROR_NONE); + }); + return heading; +} + +} // namespace + +TEST_CASE("compass_read_heading derives the cardinal directions from the field") { + mock_field_error = ERROR_NONE; + CHECK(derived_heading(1.0f, 0.0f) == doctest::Approx(0.0f)); + CHECK(derived_heading(0.0f, 1.0f) == doctest::Approx(90.0f)); + CHECK(derived_heading(-1.0f, 0.0f) == doctest::Approx(180.0f)); + CHECK(derived_heading(0.0f, -1.0f) == doctest::Approx(270.0f)); +} + +TEST_CASE("compass_read_heading derives headings in the range [0, 360)") { + mock_field_error = ERROR_NONE; + CHECK(derived_heading(1.0f, 1.0f) == doctest::Approx(45.0f)); + CHECK(derived_heading(1.0f, -1.0f) == doctest::Approx(315.0f)); + CHECK(derived_heading(-1.0f, -1.0f) == doctest::Approx(225.0f)); + + // Slightly below the x axis must wrap to just under 360, not go negative + const float heading = derived_heading(1.0f, -0.001f); + CHECK(heading >= 0.0f); + CHECK(heading < 360.0f); + CHECK(heading == doctest::Approx(359.94f).epsilon(0.001)); +} + +TEST_CASE("compass_read_heading keeps a tiny negative angle below 360") { + mock_field_error = ERROR_NONE; + // Adding 360 to such a small negative angle rounds to exactly 360.0f in float + const float heading = derived_heading(1.0f, -1e-9f); + CHECK(heading >= 0.0f); + CHECK(heading < 360.0f); +} + +TEST_CASE("compass_read_heading ignores field strength and the z axis") { + mock_field_error = ERROR_NONE; + CHECK(derived_heading(40.0f, 40.0f) == doctest::Approx(45.0f)); + + mock_field = { 1.0f, 0.0f, 500.0f }; + float heading = -1.0f; + with_device(&derived_driver, [&](Device* device) { + CHECK_EQ(compass_read_heading(device, &heading), ERROR_NONE); + }); + CHECK(heading == doctest::Approx(0.0f)); +} + +TEST_CASE("compass_read_heading rejects a zero horizontal field and leaves the output untouched") { + mock_field_error = ERROR_NONE; + mock_field = { 0.0f, 0.0f, 50.0f }; + float heading = 123.0f; + with_device(&derived_driver, [&](Device* device) { + CHECK_EQ(compass_read_heading(device, &heading), ERROR_INVALID_STATE); + }); + CHECK_EQ(heading, 123.0f); +} + +TEST_CASE("compass_read_heading propagates the field read error and leaves the output untouched") { + mock_field_error = ERROR_TIMEOUT; + float heading = 123.0f; + with_device(&derived_driver, [&](Device* device) { + CHECK_EQ(compass_read_heading(device, &heading), ERROR_TIMEOUT); + }); + CHECK_EQ(heading, 123.0f); + mock_field_error = ERROR_NONE; +} + +TEST_CASE("compass_read_heading prefers the driver's own heading") { + mock_field_error = ERROR_NONE; + mock_field = { 1.0f, 0.0f, 0.0f }; + mock_heading = 200.0f; + float heading = -1.0f; + with_device(&native_driver, [&](Device* device) { + CHECK_EQ(compass_read_heading(device, &heading), ERROR_NONE); + }); + CHECK(heading == doctest::Approx(200.0f)); +} + +TEST_CASE("compass_read_field returns the driver's reading") { + mock_field_error = ERROR_NONE; + mock_field = { 1.5f, -2.5f, 3.5f }; + CompassData data = {}; + with_device(&derived_driver, [&](Device* device) { + CHECK_EQ(compass_read_field(device, &data), ERROR_NONE); + }); + CHECK(data.x == doctest::Approx(1.5f)); + CHECK(data.y == doctest::Approx(-2.5f)); + CHECK(data.z == doctest::Approx(3.5f)); +}