Skip to content

Commit

Permalink
Bluetooth: Mesh: fix defaults for generic location srv
Browse files Browse the repository at this point in the history 
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 <[email protected]>
  • Loading branch information
@alxelax
alxelax committed Jan 6, 2025
1 parent 8b79d37 commit 171a4a2
Show file tree
Hide file tree
Showing 4 changed files with 93 additions and 45 deletions.
38 changes: 22 additions & 16 deletions include/bluetooth/mesh/gen_loc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,23 +14,32 @@
#ifndef BT_MESH_GEN_LOC_H__
#define BT_MESH_GEN_LOC_H__

#include <float.h>
#include <zephyr/bluetooth/mesh.h>

#ifdef __cplusplus
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)
/** @} */

/**
Expand All @@ -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;
Expand Down
39 changes: 24 additions & 15 deletions subsys/bluetooth/mesh/gen_loc_internal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,21 +13,25 @@
#ifndef BT_MESH_GEN_LOC_INTERNAL_H__
#define BT_MESH_GEN_LOC_INTERNAL_H__

#include <math.h>
#include <bluetooth/mesh/gen_loc.h>

#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);
Expand All @@ -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);
}

Expand Down
21 changes: 11 additions & 10 deletions subsys/bluetooth/mesh/gen_loc_srv.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
Expand Down
40 changes: 36 additions & 4 deletions subsys/bluetooth/mesh/shell/shell_loc_cli.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}

Expand Down Expand Up @@ -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);
}
}
Expand Down

0 comments on commit 171a4a2

Please sign in to comment.