diff --git a/Makefile b/Makefile index 7e35ae7039..f44cdf92c4 100644 --- a/Makefile +++ b/Makefile @@ -167,7 +167,7 @@ PROJ_OBJ_CF2 += platformservice.o sound_cf2.o extrx.o sysload.o # Stabilizer modules PROJ_OBJ += commander.o crtp_commander.o crtp_commander_rpyt.o -PROJ_OBJ += crtp_commander_generic.o ext_position.o +PROJ_OBJ += crtp_commander_generic.o crtp_localization_service.o PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o PROJ_OBJ += estimator_$(ESTIMATOR).o controller_$(CONTROLLER).o diff --git a/src/deck/drivers/src/lpsTwrTag.c b/src/deck/drivers/src/lpsTwrTag.c index 2ff039faa8..f9547df7c9 100644 --- a/src/deck/drivers/src/lpsTwrTag.c +++ b/src/deck/drivers/src/lpsTwrTag.c @@ -26,6 +26,7 @@ #include +#include #include "lpsTwrTag.h" @@ -33,6 +34,7 @@ #include "task.h" #include "log.h" +#include "crtp_localization_service.h" #include "stabilizer_types.h" #ifdef ESTIMATOR_TYPE_kalman @@ -235,11 +237,13 @@ static uint32_t twrTagOnEvent(dwDevice_t *dev, uwbEvent_t event) options->rangingState |= (1<rangingState |= (1<failedRanging[current_anchor] = 0; + locSrvSendRangeFloat(current_anchor, options->distance[current_anchor]); succededRanging[current_anchor]++; } diff --git a/src/modules/interface/crtp.h b/src/modules/interface/crtp.h index 1ff9947932..18c63b7dfb 100644 --- a/src/modules/interface/crtp.h +++ b/src/modules/interface/crtp.h @@ -42,7 +42,7 @@ typedef enum { CRTP_PORT_SETPOINT = 0x03, CRTP_PORT_MEM = 0x04, CRTP_PORT_LOG = 0x05, - CRTP_PORT_POSITION = 0x06, + CRTP_PORT_LOCALIZATION = 0x06, CRTP_PORT_SETPOINT_GENERIC = 0x07, CRTP_PORT_PLATFORM = 0x0D, CRTP_PORT_LINK = 0x0F, @@ -50,16 +50,16 @@ typedef enum { typedef struct _CRTPPacket { - uint8_t size; + uint8_t size; //< Size of data union { struct { union { - uint8_t header; + uint8_t header; //< Header selecting channel and port struct { #ifndef CRTP_HEADER_COMPAT - uint8_t channel : 2; + uint8_t channel : 2; //< Selected channel within port uint8_t reserved : 2; - uint8_t port : 4; + uint8_t port : 4; //< Selected port #else uint8_t channel : 2; uint8_t port : 4; @@ -67,9 +67,9 @@ typedef struct _CRTPPacket #endif }; }; - uint8_t data[CRTP_MAX_DATA_SIZE]; + uint8_t data[CRTP_MAX_DATA_SIZE]; //< Data }; - uint8_t raw[CRTP_MAX_DATA_SIZE+1]; + uint8_t raw[CRTP_MAX_DATA_SIZE+1]; //< The full packet "raw" }; } __attribute__((packed)) CRTPPacket; diff --git a/src/modules/interface/ext_position.h b/src/modules/interface/crtp_localization_service.h similarity index 75% rename from src/modules/interface/ext_position.h rename to src/modules/interface/crtp_localization_service.h index 23c8f59390..591921b689 100644 --- a/src/modules/interface/ext_position.h +++ b/src/modules/interface/crtp_localization_service.h @@ -23,8 +23,8 @@ * */ -#ifndef _EXT_POSITION_H_ -#define _EXT_POSITION_H_ +#ifndef _CRTP_LOCALIZATION_SERVICE_H_ +#define _CRTP_LOCALIZATION_SERVICE_H_ #include "stabilizer_types.h" @@ -38,10 +38,19 @@ struct CrtpExtPosition float z; // in m } __attribute__((packed)); -// Set up the callback for the CRTP_PORT_POSITION -void extPositionInit(void); +typedef enum +{ + RANGE_STREAM_FLOAT = 0, + RANGE_STREAM_FP16 = 1, +} locsrv_t; + +// Set up the callback for the CRTP_PORT_LOCALIZATION +void locSrvInit(void); // Get the current position from the cache bool getExtPosition(state_t *state); -#endif /* _EXT_POSITION_H_ */ +// Send range in float. After 5 ranges it will send the packet. +void locSrvSendRangeFloat(uint8_t id, float range); + +#endif /* _CRTP_LOCALIZATION_SERVICE_H_ */ diff --git a/src/modules/src/comm.c b/src/modules/src/comm.c index d1f60efeb3..3fe3fd6945 100644 --- a/src/modules/src/comm.c +++ b/src/modules/src/comm.c @@ -41,6 +41,7 @@ #include "usblink.h" #include "platformservice.h" #include "syslink.h" +#include "crtp_localization_service.h" static bool isInit; @@ -78,7 +79,8 @@ void commInit(void) #endif logInit(); paramInit(); - + locSrvInit(); + //setup CRTP communication channel //TODO: check for USB first and prefer USB over radio //if (usbTest()) diff --git a/src/modules/src/ext_position.c b/src/modules/src/crtp_localization_service.c similarity index 56% rename from src/modules/src/ext_position.c rename to src/modules/src/crtp_localization_service.c index 7677075624..0028d5e772 100644 --- a/src/modules/src/ext_position.c +++ b/src/modules/src/crtp_localization_service.c @@ -23,18 +23,39 @@ * * */ +#include +#include #include "FreeRTOS.h" #include "task.h" -#include "ext_position.h" #include "crtp.h" +#include "crtp_localization_service.h" #include "log.h" +#include "param.h" #ifdef ESTIMATOR_TYPE_kalman #include "estimator_kalman.h" #endif +#define NBR_OF_RANGES_IN_PACKET 5 + +typedef enum +{ + EXT_POSITION = 0, + GENERIC_TYPE = 1, +} locsrvChannels_t; + +typedef struct +{ + uint8_t type; + struct + { + uint8_t id; + float range; + } __attribute__((packed)) ranges[NBR_OF_RANGES_IN_PACKET]; +} __attribute__((packed)) rangePacket; + /** * Position data cache */ @@ -48,21 +69,39 @@ typedef struct // Struct for logging position information static positionMeasurement_t ext_pos; static ExtPositionCache crtpExtPosCache; -static void extPositionCrtpCB(CRTPPacket* pk); - +static CRTPPacket pkRange; +static uint8_t rangeIndex; +static bool enableRangeStreamFloat = false; static bool isInit = false; -void extPositionInit() +static void locSrvCrtpCB(CRTPPacket* pk); +static void extPositionHandler(CRTPPacket* pk); + +void locSrvInit() { if (isInit) { return; } - crtpRegisterPortCB(CRTP_PORT_POSITION, extPositionCrtpCB); + crtpRegisterPortCB(CRTP_PORT_LOCALIZATION, locSrvCrtpCB); isInit = true; } -static void extPositionCrtpCB(CRTPPacket* pk) +static void locSrvCrtpCB(CRTPPacket* pk) +{ + switch (pk->channel) + { + case EXT_POSITION: + extPositionHandler(pk); + break; + case GENERIC_TYPE: + // TODO: Implement + default: + break; + } +} + +static void extPositionHandler(CRTPPacket* pk) { crtpExtPosCache.targetVal[!crtpExtPosCache.activeSide] = *((struct CrtpExtPosition*)pk->data); crtpExtPosCache.activeSide = !crtpExtPosCache.activeSide; @@ -87,9 +126,48 @@ bool getExtPosition(state_t *state) return false; } +void locSrvSendPacket(locsrv_t type, uint8_t *data, uint8_t length) +{ + CRTPPacket pk; + + ASSERT(length < CRTP_MAX_DATA_SIZE); + + pk.port = CRTP_PORT_LOCALIZATION; + pk.channel = GENERIC_TYPE; + memcpy(pk.data, data, length); + crtpSendPacket(&pk); +} + +void locSrvSendRangeFloat(uint8_t id, float range) +{ + rangePacket *rp = (rangePacket *)pkRange.data; + + ASSERT(rangeIndex <= NBR_OF_RANGES_IN_PACKET); + + if (enableRangeStreamFloat) + { + rp->ranges[rangeIndex].id = id; + rp->ranges[rangeIndex].range = range; + rangeIndex++; + + if (rangeIndex >= 5) + { + rp->type = RANGE_STREAM_FLOAT; + pkRange.port = CRTP_PORT_LOCALIZATION; + pkRange.channel = GENERIC_TYPE; + pkRange.size = sizeof(rangePacket); + crtpSendPacket(&pkRange); + rangeIndex = 0; + } + } +} + LOG_GROUP_START(ext_pos) LOG_ADD(LOG_FLOAT, X, &ext_pos.x) LOG_ADD(LOG_FLOAT, Y, &ext_pos.y) LOG_ADD(LOG_FLOAT, Z, &ext_pos.z) LOG_GROUP_STOP(ext_pos) +PARAM_GROUP_START(locSrv) +PARAM_ADD(PARAM_UINT8, enRangeStreamFP32, &enableRangeStreamFloat) +PARAM_GROUP_STOP(locSrv) diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index e1d280a78b..89bd2c1e80 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -36,7 +36,7 @@ #include "sensors.h" #include "commander.h" -#include "ext_position.h" +#include "crtp_localization_service.h" #include "sitaw.h" #include "controller.h" #include "power_distribution.h" diff --git a/src/modules/src/system.c b/src/modules/src/system.c index 7ff44bcdfb..7513edc62c 100644 --- a/src/modules/src/system.c +++ b/src/modules/src/system.c @@ -60,7 +60,6 @@ #include "buzzer.h" #include "sound.h" #include "sysload.h" -#include "ext_position.h" #ifdef PLATFORM_CF1 #include "uart_cf1.h" @@ -171,7 +170,6 @@ void systemTask(void *arg) systemInit(); commInit(); commanderInit(); - extPositionInit(); // Set callback for CRTP_PORT_POSITION stabilizerInit(); #ifdef PLATFORM_CF2 deckInit(); diff --git a/test/deck/drivers/src/TestLpsTwrTag.c b/test/deck/drivers/src/TestLpsTwrTag.c index 5c3b75c012..b23007b7f1 100644 --- a/test/deck/drivers/src/TestLpsTwrTag.c +++ b/test/deck/drivers/src/TestLpsTwrTag.c @@ -7,6 +7,7 @@ #include "unity.h" #include "mock_libdw1000.h" #include "mock_cfassert.h" +#include "crtp_localization_serviceMocks.h" #include "dw1000Mocks.h" #ifdef ESTIMATOR_TYPE_kalman diff --git a/test/testSupport/crtp_localization_serviceMocks.c b/test/testSupport/crtp_localization_serviceMocks.c new file mode 100644 index 0000000000..509239d4da --- /dev/null +++ b/test/testSupport/crtp_localization_serviceMocks.c @@ -0,0 +1,6 @@ +#include + +void locSrvSendRangeFloat(uint8_t id, float range) +{ + // Do nothing +} diff --git a/test/testSupport/crtp_localization_serviceMocks.h b/test/testSupport/crtp_localization_serviceMocks.h new file mode 100644 index 0000000000..e69de29bb2