Skip to content

Commit

Permalink
Merge pull request #1213 from bitcraze/krichardsson/rename-any
Browse files Browse the repository at this point in the history
Rename "any" controller and estimator
  • Loading branch information
knmcguire committed Feb 1, 2023
2 parents bc58d44 + c6eaa98 commit 7f1d1fc
Show file tree
Hide file tree
Showing 16 changed files with 54 additions and 54 deletions.
4 changes: 2 additions & 2 deletions configs/bolt_defconfig
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
CONFIG_PLATFORM_BOLT=y

CONFIG_ESTIMATOR_ANY=y
CONFIG_CONTROLLER_ANY=y
CONFIG_ESTIMATOR_AUTO_SELECT=y
CONFIG_CONTROLLER_AUTO_SELECT=y
4 changes: 2 additions & 2 deletions configs/cfbl_defconfig
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
CONFIG_MOTORS_ESC_PROTOCOL_DSHOT=y

CONFIG_ESTIMATOR_ANY=y
CONFIG_CONTROLLER_ANY=y
CONFIG_ESTIMATOR_AUTO_SELECT=y
CONFIG_CONTROLLER_AUTO_SELECT=y
2 changes: 1 addition & 1 deletion configs/flapper_defconfig
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
CONFIG_PLATFORM_FLAPPER=y

CONFIG_ESTIMATOR_ANY=y
CONFIG_ESTIMATOR_AUTO_SELECT=y
CONFIG_CONTROLLER_PID=y

CONFIG_MOTORS_ESC_PROTOCOL_STANDARD_PWM=y
Expand Down
4 changes: 2 additions & 2 deletions configs/tag_defconfig
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
CONFIG_PLATFORM_TAG=y

CONFIG_ESTIMATOR_ANY=y
CONFIG_CONTROLLER_ANY=y
CONFIG_ESTIMATOR_AUTO_SELECT=y
CONFIG_CONTROLLER_AUTO_SELECT=y

CONFIG_DECK_FORCE="bcLoco"
CONFIG_SENSORS_IGNORE_BAROMETER_FAIL=y
8 changes: 4 additions & 4 deletions docs/development/create_platform.md
Original file line number Diff line number Diff line change
Expand Up @@ -195,17 +195,17 @@ To make it easier for people to build for `RINCEWIND` we can add a `defconfig` f
```Makefile
CONFIG_PLATFORM_BOLT=y

CONFIG_ESTIMATOR_ANY=y
CONFIG_CONTROLLER_ANY=y
CONFIG_ESTIMATOR_AUTO_SELECT=y
CONFIG_CONTROLLER_AUTO_SELECT=y
```

Based on this a start of `rincewind_defconfig` could be:

```Makefile
CONFIG_PLATFORM_RINCEWIND=y

CONFIG_ESTIMATOR_ANY=y
CONFIG_CONTROLLER_ANY=y
CONFIG_ESTIMATOR_AUTO_SELECT=y
CONFIG_CONTROLLER_AUTO_SELECT=y
```

Then `RINCEWIND` platform could be built by:
Expand Down
6 changes: 3 additions & 3 deletions src/deck/core/deck_info.c
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ static void enumerateDecks(void);
static void checkPeriphAndGpioConflicts(void);

static void scanRequiredSystemProperties(void);
static StateEstimatorType requiredEstimator = anyEstimator;
static StateEstimatorType requiredEstimator = StateEstimatorTypeAutoSelect;
static bool registerRequiredEstimator(StateEstimatorType estimator);
static bool requiredLowInterferenceRadioMode = false;

Expand Down Expand Up @@ -370,9 +370,9 @@ static bool registerRequiredEstimator(StateEstimatorType estimator)
{
bool isError = false;

if (anyEstimator != estimator)
if (StateEstimatorTypeAutoSelect != estimator)
{
if (anyEstimator == requiredEstimator)
if (StateEstimatorTypeAutoSelect == requiredEstimator)
{
requiredEstimator = estimator;
}
Expand Down
12 changes: 6 additions & 6 deletions src/deck/drivers/src/flowdeck_v1v2.c
Original file line number Diff line number Diff line change
Expand Up @@ -121,12 +121,12 @@ static void flowdeckTask(void *param)
flowData.stdDevX = stdFlow;
flowData.stdDevY = stdFlow;
flowData.dt = (float)(usecTimestamp()-lastTime)/1000000.0f;
// we do want to update dt every measurement and not only in the ones with detected motion,
// as we work with instantaneous gyro and velocity values in the update function
// we do want to update dt every measurement and not only in the ones with detected motion,
// as we work with instantaneous gyro and velocity values in the update function
// (meaning assuming the current measurements over all of dt)
lastTime = usecTimestamp();
lastTime = usecTimestamp();




#if defined(USE_MA_SMOOTHING)
// Use MA Smoothing
Expand Down Expand Up @@ -203,7 +203,7 @@ static const DeckDriver flowdeck1_deck = {
.name = "bcFlow",
.usedGpio = DECK_USING_IO_3,
.usedPeriph = DECK_USING_I2C | DECK_USING_SPI,
.requiredEstimator = kalmanEstimator,
.requiredEstimator = StateEstimatorTypeKalman,

.init = flowdeck1Init,
.test = flowdeck1Test,
Expand Down Expand Up @@ -250,7 +250,7 @@ static const DeckDriver flowdeck2_deck = {

.usedGpio = DECK_USING_IO_3,
.usedPeriph = DECK_USING_I2C | DECK_USING_SPI,
.requiredEstimator = kalmanEstimator,
.requiredEstimator = StateEstimatorTypeKalman,

.init = flowdeck2Init,
.test = flowdeck2Test,
Expand Down
2 changes: 1 addition & 1 deletion src/deck/drivers/src/lighthouse.c
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,7 @@ static const DeckDriver lighthouse_deck = {

.usedGpio = 0,
.usedPeriph = DECK_USING_UART1,
.requiredEstimator = kalmanEstimator,
.requiredEstimator = StateEstimatorTypeKalman,

.memoryDef = &memoryDef,

Expand Down
2 changes: 1 addition & 1 deletion src/deck/drivers/src/locodeck.c
Original file line number Diff line number Diff line change
Expand Up @@ -585,7 +585,7 @@ static const DeckDriver dwm1000_deck = {
.usedGpio = DECK_USING_IO_1 | DECK_USING_PC10 | DECK_USING_PC11,
#endif
.usedPeriph = DECK_USING_SPI,
.requiredEstimator = kalmanEstimator,
.requiredEstimator = StateEstimatorTypeKalman,
#ifdef LOCODECK_NO_LOW_INTERFERENCE
.requiredLowInterferenceRadioMode = false,
#else
Expand Down
2 changes: 1 addition & 1 deletion src/modules/interface/controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@
#include "stabilizer_types.h"

typedef enum {
ControllerTypeAny,
ControllerTypeAutoSelect,
ControllerTypePID,
ControllerTypeMellinger,
ControllerTypeINDI,
Expand Down
14 changes: 7 additions & 7 deletions src/modules/interface/estimator.h
Original file line number Diff line number Diff line change
Expand Up @@ -29,18 +29,18 @@
#include "stabilizer_types.h"

typedef enum {
anyEstimator = 0,
complementaryEstimator,
StateEstimatorTypeAutoSelect = 0,
StateEstimatorTypeComplementary,
#ifdef CONFIG_ESTIMATOR_KALMAN_ENABLE
kalmanEstimator,
StateEstimatorTypeKalman,
#endif
#ifdef CONFIG_ESTIMATOR_UKF_ENABLE
ukfEstimator,
StateEstimatorTypeUkf,
#endif
#ifdef CONFIG_ESTIMATOR_OOT
OutOfTreeEstimator,
StateEstimatorTypeOutOfTree,
#endif
StateEstimatorTypeCount,
StateEstimatorType_COUNT,
} StateEstimatorType;

typedef enum {
Expand Down Expand Up @@ -168,4 +168,4 @@ bool estimatorDequeue(measurement_t *measurement);
void estimatorOutOfTreeInit(void);
bool estimatorOutOfTreeTest(void);
void estimatorOutOfTree(state_t *state, const uint32_t tick);
#endif
#endif
12 changes: 6 additions & 6 deletions src/modules/src/Kconfig
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,10 @@ menu "Controllers and Estimators"

choice
prompt "Default controller"
default CONFIG_CONTROLLER_ANY
default CONFIG_CONTROLLER_AUTO_SELECT

config CONTROLLER_ANY
bool "Any Controller"
config CONTROLLER_AUTO_SELECT
bool "Auto select Controller"
help
Do not care which controller is used, if any

Expand Down Expand Up @@ -61,10 +61,10 @@ config ESTIMATOR_UKF_ENABLE

choice
prompt "Default estimator"
default CONFIG_ESTIMATOR_ANY
default CONFIG_ESTIMATOR_AUTO_SELECT

config ESTIMATOR_ANY
bool "Any Estimator"
config ESTIMATOR_AUTO_SELECT
bool "Auto select Estimator"
help
Do not care which estimator is used, if any

Expand Down
8 changes: 4 additions & 4 deletions src/modules/src/controller.c
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
#include "autoconf.h"

#define DEFAULT_CONTROLLER ControllerTypePID
static ControllerType currentController = ControllerTypeAny;
static ControllerType currentController = ControllerTypeAutoSelect;

static void initController();

Expand Down Expand Up @@ -41,7 +41,7 @@ void controllerInit(ControllerType controller) {

currentController = controller;

if (ControllerTypeAny == currentController) {
if (ControllerTypeAutoSelect == currentController) {
currentController = DEFAULT_CONTROLLER;
}

Expand All @@ -54,11 +54,11 @@ void controllerInit(ControllerType controller) {
#elif defined(CONFIG_CONTROLLER_BRESCIANINI)
#define CONTROLLER ControllerTypeBrescianini
#else
#define CONTROLLER ControllerTypeAny
#define CONTROLLER ControllerTypeAutoSelect
#endif

ControllerType forcedController = CONTROLLER;
if (forcedController != ControllerTypeAny) {
if (forcedController != ControllerTypeAutoSelect) {
DEBUG_PRINT("Controller type forced\n");
currentController = forcedController;
}
Expand Down
18 changes: 9 additions & 9 deletions src/modules/src/estimator.c
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@
#include "eventtrigger.h"
#include "quatcompress.h"

#define DEFAULT_ESTIMATOR complementaryEstimator
static StateEstimatorType currentEstimator = anyEstimator;
#define DEFAULT_ESTIMATOR StateEstimatorTypeComplementary
static StateEstimatorType currentEstimator = StateEstimatorTypeAutoSelect;


#define MEASUREMENTS_QUEUE_SIZE (20)
Expand Down Expand Up @@ -106,28 +106,28 @@ void stateEstimatorInit(StateEstimatorType estimator) {
}

void stateEstimatorSwitchTo(StateEstimatorType estimator) {
if (estimator < 0 || estimator >= StateEstimatorTypeCount) {
if (estimator < 0 || estimator >= StateEstimatorType_COUNT) {
return;
}

StateEstimatorType newEstimator = estimator;

if (anyEstimator == newEstimator) {
if (StateEstimatorTypeAutoSelect == newEstimator) {
newEstimator = DEFAULT_ESTIMATOR;
}

#if defined(CONFIG_ESTIMATOR_KALMAN)
#define ESTIMATOR kalmanEstimator
#define ESTIMATOR StateEstimatorTypeKalman
#elif defined(CONFIG_UKF_KALMAN)
#define ESTIMATOR ukfEstimator
#define ESTIMATOR StateEstimatorTypeUkf
#elif defined(CONFIG_ESTIMATOR_COMPLEMENTARY)
#define ESTIMATOR complementaryEstimator
#define ESTIMATOR StateEstimatorTypeComplementary
#else
#define ESTIMATOR anyEstimator
#define ESTIMATOR StateEstimatorTypeAutoSelect
#endif

StateEstimatorType forcedEstimator = ESTIMATOR;
if (forcedEstimator != anyEstimator) {
if (forcedEstimator != StateEstimatorTypeAutoSelect) {
DEBUG_PRINT("Estimator type forced\n");
newEstimator = forcedEstimator;
}
Expand Down
8 changes: 4 additions & 4 deletions src/modules/src/stabilizer.c
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,7 @@ void stabilizerInit(StateEstimatorType estimator)

sensorsInit();
stateEstimatorInit(estimator);
controllerInit(ControllerTypeAny);
controllerInit(ControllerTypeAutoSelect);
powerDistributionInit();
motorsInit(platformConfigGetMotorMapping());
collisionAvoidanceInit();
Expand Down Expand Up @@ -359,13 +359,13 @@ void stabilizerSetEmergencyStopTimeout(int timeout)
*/
PARAM_GROUP_START(stabilizer)
/**
* @brief Estimator type Any(0), complementary(1), extended kalman(2), **unscented kalman(3) (Default: 0)
*
* @brief Estimator type Auto select(0), complementary(1), extended kalman(2), **unscented kalman(3) (Default: 0)
*
* ** Experimental, needs to be enabled in kbuild
*/
PARAM_ADD_CORE(PARAM_UINT8, estimator, &estimatorType)
/**
* @brief Controller type Any(0), PID(1), Mellinger(2), INDI(3), Brescianini(4) (Default: 0)
* @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4) (Default: 0)
*/
PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType)
/**
Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/system.c
Original file line number Diff line number Diff line change
Expand Up @@ -194,7 +194,7 @@ void systemTask(void *arg)
commInit();
commanderInit();

StateEstimatorType estimator = anyEstimator;
StateEstimatorType estimator = StateEstimatorTypeAutoSelect;

#ifdef CONFIG_ESTIMATOR_KALMAN_ENABLE
estimatorKalmanTaskInit();
Expand Down

0 comments on commit 7f1d1fc

Please sign in to comment.