From 171a4a210bf1de2969541d39de60b439dc4d6fd3 Mon Sep 17 00:00:00 2001 From: Aleksandr Khromykh Date: Mon, 6 Jan 2025 13:14:38 +0100 Subject: [PATCH] Bluetooth: Mesh: fix defaults for generic location srv Commit fixes default values for: - Global Latitude - Global Longitude - Local North - Local East Adds intermediate default for double WGS84 latitude\longitude. Signed-off-by: Aleksandr Khromykh --- include/bluetooth/mesh/gen_loc.h | 38 +++++++++++--------- subsys/bluetooth/mesh/gen_loc_internal.h | 39 ++++++++++++-------- subsys/bluetooth/mesh/gen_loc_srv.c | 21 +++++------ subsys/bluetooth/mesh/shell/shell_loc_cli.c | 40 ++++++++++++++++++--- 4 files changed, 93 insertions(+), 45 deletions(-) diff --git a/include/bluetooth/mesh/gen_loc.h b/include/bluetooth/mesh/gen_loc.h index 3af0a19edf5b..fe12ba80a1be 100644 --- a/include/bluetooth/mesh/gen_loc.h +++ b/include/bluetooth/mesh/gen_loc.h @@ -14,6 +14,7 @@ #ifndef BT_MESH_GEN_LOC_H__ #define BT_MESH_GEN_LOC_H__ +#include #include #ifdef __cplusplus @@ -21,16 +22,24 @@ extern "C" { #endif /** - * @defgroup bt_mesh_loc_altitude_defines Defines for altitude. - * Boundaries and special values for the altitude. + * @defgroup bt_mesh_loc_location_defines Defines for locations. + * Boundaries and special values for the latitude, longitude, altitude and local North/East. * @{ */ -/** Highest altitude that can be represented */ -#define BT_MESH_LOC_ALTITUDE_MAX 32765 -/** Altitude is unknown or not configured. */ -#define BT_MESH_LOC_ALTITUDE_UNKNOWN 0x7fff -/** Altitude is out of bounds. */ -#define BT_MESH_LOC_ALTITUDE_TOO_LARGE 0x7ffe +/** The Global Longitude is unknown or not configured. */ +#define BT_MESH_LOC_GLOBAL_LONGITUDE_UNKNOWN DBL_MAX +/** The Global Latitude is unknown or not configured. */ +#define BT_MESH_LOC_GLOBAL_LATITUDE_UNKNOWN DBL_MAX +/** The Local North is unknown or not configured. */ +#define BT_MESH_LOC_LOCAL_NORTH_UNKNOWN ((int16_t)0x8000) +/** The Local East is unknown or not configured. */ +#define BT_MESH_LOC_LOCAL_EAST_UNKNOWN ((int16_t)0x8000) +/** The highest Altitude (Local and Global) that can be represented */ +#define BT_MESH_LOC_ALTITUDE_MAX ((int16_t)32765) +/** The Altitude (Local and Global) is unknown or not configured. */ +#define BT_MESH_LOC_ALTITUDE_UNKNOWN ((int16_t)0x7fff) +/** The Altitude (Local and Global) is out of bounds. */ +#define BT_MESH_LOC_ALTITUDE_TOO_LARGE ((int16_t)0x7ffe) /** @} */ /** @@ -50,26 +59,23 @@ extern "C" { #define BT_MESH_LOC_FLOOR_NUMBER_UNKNOWN 0xff /** @} */ -/** Global location parameters. */ +/** Global location parameters. @sa bt_mesh_loc_location_defines */ struct bt_mesh_loc_global { /** Global WGS84 North coordinate in degrees. */ double latitude; /** Global WGS84 East coordinate in degrees. */ double longitude; - /** - * Global altitude above the WGS84 datum in meters. - * @sa bt_mesh_loc_altitude_defines - */ + /** Global altitude above the WGS84 datum in meters. */ int16_t altitude; }; /** Local location parameters. */ struct bt_mesh_loc_local { - /** Local north position in decimeters. */ + /** The Local North position in decimeters. @sa bt_mesh_loc_location_defines */ int16_t north; - /** Local east position in decimeters. */ + /** The Local East position in decimeters. @sa bt_mesh_loc_location_defines */ int16_t east; - /** Local altitude in decimeters. @sa bt_mesh_loc_altitude_defines */ + /** The Local Altitude in decimeters. @sa bt_mesh_loc_location_defines */ int16_t altitude; /** Floor number. @sa bt_mesh_loc_floor_number_defines */ int16_t floor_number; diff --git a/subsys/bluetooth/mesh/gen_loc_internal.h b/subsys/bluetooth/mesh/gen_loc_internal.h index 574dcacda636..2f35a40b0ea5 100644 --- a/subsys/bluetooth/mesh/gen_loc_internal.h +++ b/subsys/bluetooth/mesh/gen_loc_internal.h @@ -13,21 +13,25 @@ #ifndef BT_MESH_GEN_LOC_INTERNAL_H__ #define BT_MESH_GEN_LOC_INTERNAL_H__ +#include #include -#define BT_MESH_LOC_LATITUDE_FACTOR \ - (23860929.411111) /* ((INT32_MAX - 1) / 90) */ -#define BT_MESH_LOC_LONGITUDE_FACTOR \ - (11930464.705556) /* ((INT32_MAX - 1) / 180) \ - */ +#define BT_MESH_LOC_LATITUDE_NOT_CONFIGURED 0x80000000 +#define BT_MESH_LOC_LONGITUDE_NOT_CONFIGURED 0x80000000 -static inline void -bt_mesh_loc_global_encode(struct net_buf_simple *buf, - const struct bt_mesh_loc_global *loc) +static inline void bt_mesh_loc_global_encode(struct net_buf_simple *buf, + const struct bt_mesh_loc_global *loc) { - uint32_t latitude = (uint32_t)(loc->latitude * BT_MESH_LOC_LATITUDE_FACTOR); - uint32_t longitude = - (uint32_t)(loc->longitude * BT_MESH_LOC_LONGITUDE_FACTOR); + int32_t latitude = BT_MESH_LOC_LATITUDE_NOT_CONFIGURED; + int32_t longitude = BT_MESH_LOC_LONGITUDE_NOT_CONFIGURED; + + if (loc->latitude >= -90.0 && loc->latitude <= 90.0) { + latitude = floor(loc->latitude / 90 * (INT32_MAX - 1)); + } + + if (loc->longitude >= -180.0 && loc->longitude <= 180.0) { + longitude = floor(loc->longitude / 180 * (INT32_MAX - 1)); + } net_buf_simple_add_le32(buf, latitude); net_buf_simple_add_le32(buf, longitude); @@ -37,10 +41,15 @@ bt_mesh_loc_global_encode(struct net_buf_simple *buf, static inline void bt_mesh_loc_global_decode(struct net_buf_simple *buf, struct bt_mesh_loc_global *loc) { - loc->latitude = - (net_buf_simple_pull_le32(buf) / BT_MESH_LOC_LATITUDE_FACTOR); - loc->longitude = - (net_buf_simple_pull_le32(buf) / BT_MESH_LOC_LONGITUDE_FACTOR); + int32_t latitude = net_buf_simple_pull_le32(buf); + int32_t longitude = net_buf_simple_pull_le32(buf); + + loc->latitude = latitude == BT_MESH_LOC_LATITUDE_NOT_CONFIGURED + ? BT_MESH_LOC_GLOBAL_LATITUDE_UNKNOWN + : ((double)latitude * 90) / (INT32_MAX - 1); + loc->longitude = longitude == BT_MESH_LOC_LONGITUDE_NOT_CONFIGURED + ? BT_MESH_LOC_GLOBAL_LONGITUDE_UNKNOWN + : ((double)longitude * 180) / (INT32_MAX - 1); loc->altitude = net_buf_simple_pull_le16(buf); } diff --git a/subsys/bluetooth/mesh/gen_loc_srv.c b/subsys/bluetooth/mesh/gen_loc_srv.c index 7da6aa95f1ae..279722607a2b 100644 --- a/subsys/bluetooth/mesh/gen_loc_srv.c +++ b/subsys/bluetooth/mesh/gen_loc_srv.c @@ -10,18 +10,19 @@ #include "model_utils.h" #include "gen_loc_internal.h" -#define LOC_GLOBAL_DEFAULT \ - { \ - .latitude = 0.0, .longitude = 0.0, \ - .altitude = BT_MESH_LOC_ALTITUDE_UNKNOWN, \ +#define LOC_GLOBAL_DEFAULT \ + { \ + .latitude = BT_MESH_LOC_GLOBAL_LATITUDE_UNKNOWN, \ + .longitude = BT_MESH_LOC_GLOBAL_LONGITUDE_UNKNOWN, \ + .altitude = BT_MESH_LOC_ALTITUDE_UNKNOWN, \ } -#define LOC_LOCAL_DEFAULT \ - { \ - .north = 0, .east = 0, \ - .altitude = BT_MESH_LOC_ALTITUDE_UNKNOWN, \ - .floor_number = BT_MESH_LOC_FLOOR_NUMBER_UNKNOWN, \ - .is_mobile = false, .time_delta = 0, .precision_mm = 4096000, \ +#define LOC_LOCAL_DEFAULT \ + { \ + .north = BT_MESH_LOC_LOCAL_NORTH_UNKNOWN, .east = BT_MESH_LOC_LOCAL_EAST_UNKNOWN, \ + .altitude = BT_MESH_LOC_ALTITUDE_UNKNOWN, \ + .floor_number = BT_MESH_LOC_FLOOR_NUMBER_UNKNOWN, .is_mobile = false, \ + .time_delta = 0, .precision_mm = 4096000, \ } /* Global location */ diff --git a/subsys/bluetooth/mesh/shell/shell_loc_cli.c b/subsys/bluetooth/mesh/shell/shell_loc_cli.c index e3518feff75c..d09e83080e6f 100644 --- a/subsys/bluetooth/mesh/shell/shell_loc_cli.c +++ b/subsys/bluetooth/mesh/shell/shell_loc_cli.c @@ -17,8 +17,23 @@ static const struct bt_mesh_model *mod; static void global_loc_print(const struct shell *shell, int err, struct bt_mesh_loc_global *rsp) { if (!err) { - shell_print(shell, "Latitude: %f, longitude: %f, altitude: %d", rsp->latitude, - rsp->longitude, rsp->altitude); + if (rsp->latitude == BT_MESH_LOC_GLOBAL_LATITUDE_UNKNOWN) { + shell_print(shell, "Latitude: unknown"); + } else { + shell_print(shell, "Latitude: %f", rsp->latitude); + } + + if (rsp->longitude == BT_MESH_LOC_GLOBAL_LONGITUDE_UNKNOWN) { + shell_print(shell, "Longitude: unknown"); + } else { + shell_print(shell, "Longitude: %f", rsp->longitude); + } + + if (rsp->altitude == BT_MESH_LOC_ALTITUDE_UNKNOWN) { + shell_print(shell, "Altitude: unknown"); + } else { + shell_print(shell, "Altitude: %d", rsp->altitude); + } } } @@ -84,11 +99,28 @@ static int cmd_loc_global_set_unack(const struct shell *shell, size_t argc, char static void local_loc_print(const struct shell *shell, int err, struct bt_mesh_loc_local *rsp) { if (!err) { + if (rsp->north == BT_MESH_LOC_LOCAL_NORTH_UNKNOWN) { + shell_print(shell, "North: unknown"); + } else { + shell_print(shell, "North: %d", rsp->north); + } + + if (rsp->east == BT_MESH_LOC_LOCAL_EAST_UNKNOWN) { + shell_print(shell, "East: unknown"); + } else { + shell_print(shell, "East: %d", rsp->east); + } + + if (rsp->altitude == BT_MESH_LOC_ALTITUDE_UNKNOWN) { + shell_print(shell, "Altitude: unknown"); + } else { + shell_print(shell, "Altitude: %d", rsp->altitude); + } + shell_print(shell, - "North: %d, east: %d, altitude: %d, " "floor_number: %d, is_mobile: %d, " "time_delta: %d, precision_mm: %d", - rsp->north, rsp->east, rsp->altitude, rsp->floor_number, rsp->is_mobile, + rsp->floor_number, rsp->is_mobile, rsp->time_delta, rsp->precision_mm); } }