Skip to content

Commit

Permalink
Merge pull request #1382 from bitcraze/toverumar/add_bl_test_deck
Browse files Browse the repository at this point in the history
Add RPM deck test for Brushless
  • Loading branch information
tobbeanton authored Jun 11, 2024
2 parents b15eebc + e87e2ba commit fe48d59
Show file tree
Hide file tree
Showing 5 changed files with 297 additions and 14 deletions.
10 changes: 0 additions & 10 deletions .vscode/settings.json

This file was deleted.

2 changes: 1 addition & 1 deletion src/deck/drivers/src/Kconfig
Original file line number Diff line number Diff line change
Expand Up @@ -485,7 +485,7 @@ config DECK_ACS37800

config DECK_RPM
bool "Support the rpm deck"
default n
default y
help
Enables the rpm deck that can be usfull in testing.

Expand Down
6 changes: 3 additions & 3 deletions src/deck/drivers/src/rpm.c
Original file line number Diff line number Diff line change
Expand Up @@ -81,8 +81,7 @@ static uint16_t m2rpm;
static uint16_t m3rpm;
static uint16_t m4rpm;


static void rpmInit(DeckInfo *info)
static void rpmTestInit(DeckInfo *info)
{
int i;
isInit = true;
Expand Down Expand Up @@ -153,6 +152,7 @@ static void rpmInit(DeckInfo *info)
TIM_ITConfig(TIM3, TIM_IT_CC1, ENABLE);
TIM_ITConfig(TIM3, TIM_IT_CC2, ENABLE);
TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);

}

static uint16_t calcRPM(uint32_t t1, uint32_t t2)
Expand Down Expand Up @@ -313,7 +313,7 @@ static const DeckDriver rpm_deck = {

.usedGpio = DECK_USING_IO_2 | DECK_USING_IO_3 | DECK_USING_PA2 | DECK_USING_PA3,

.init = rpmInit,
.init = rpmTestInit
};

DECK_DRIVER(rpm_deck);
Expand Down
1 change: 1 addition & 0 deletions src/deck/drivers/src/test/Kbuild
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
obj-y += exptest.o
obj-y += exptestCfBl.o
obj-y += exptestRR.o
obj-y += exptestBolt11.o
292 changes: 292 additions & 0 deletions src/deck/drivers/src/test/exptestCfBl.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,292 @@
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2021 Bitcraze AB
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, in version 3.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* exptest.c - Testing of expansion port.
*/
#define DEBUG_MODULE "ET"

#include <stdint.h>

#include "stm32fxxx.h"
#include "config.h"
#include "debug.h"
#include "deck.h"
#include "deck_test.h"
#include "motors.h"
#include "platform.h"

//FreeRTOS includes
#include "FreeRTOS.h"
#include "task.h"

#include "log.h"
#include "param.h"
#include "sensors.h"

//Hardware configuration
#define ET_GPIO_PERIF (RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB | RCC_AHB1Periph_GPIOC)

#define ET_GPIO_PORT_TX1 GPIOC
#define ET_GPIO_PIN_TX1 GPIO_Pin_10
#define ET_GPIO_PORT_RX1 GPIOC
#define ET_GPIO_PIN_RX1 GPIO_Pin_11

#define ET_GPIO_PORT_SCK GPIOA
#define ET_GPIO_PIN_SCK GPIO_Pin_5
#define ET_GPIO_PORT_MOSI GPIOA
#define ET_GPIO_PIN_MOSI GPIO_Pin_6
#define ET_GPIO_PORT_MISO GPIOA
#define ET_GPIO_PIN_MISO GPIO_Pin_7

#define ET_GPIO_PORT_SDA GPIOB
#define ET_GPIO_PIN_SDA GPIO_Pin_7
#define ET_GPIO_PORT_SCL GPIOB
#define ET_GPIO_PIN_SCL GPIO_Pin_6

#define ET_GPIO_PORT_IO1 GPIOB
#define ET_GPIO_PIN_IO1 GPIO_Pin_8
#define ET_GPIO_PORT_IO4 GPIOC
#define ET_GPIO_PIN_IO4 GPIO_Pin_12

#define ET_NBR_PINS 7

#define RPM_TEST_LOWER_LIMIT 12000
#define MOTOR_TEST_PWM (UINT16_MAX/2)
#define MOTOR_TEST_TIME_MILLIS 2000
#define MOTOR_FEED_SIGNAL_INTVL 1
#define MOTOR_RPM_NBR_SAMPLES (MOTOR_TEST_TIME_MILLIS/MOTOR_FEED_SIGNAL_INTVL)

typedef struct _etGpio {
GPIO_TypeDef *port;
uint16_t pin;
char name[6];
} EtGpio;

typedef struct {
logVarId_t m1;
logVarId_t m2;
logVarId_t m3;
logVarId_t m4;
} motorRpmParams_t;

static EtGpio etGpioIn[ET_NBR_PINS] = {
{ET_GPIO_PORT_TX1, ET_GPIO_PIN_TX1, "TX1"},
{ET_GPIO_PORT_RX1, ET_GPIO_PIN_RX1, "RX1"},
{ET_GPIO_PORT_SCK, ET_GPIO_PIN_SCK, "SCK"},
{ET_GPIO_PORT_MOSI, ET_GPIO_PIN_MOSI, "MOSI"},
{ET_GPIO_PORT_MISO, ET_GPIO_PIN_MISO, "MISO"},
{ET_GPIO_PORT_IO1, ET_GPIO_PIN_IO1, "IO1"},
{ET_GPIO_PORT_IO4, ET_GPIO_PIN_IO4, "IO4"}
};

static EtGpio etGpioSDA = {ET_GPIO_PORT_SDA, ET_GPIO_PIN_SDA, "SDA"};
static EtGpio etGpioSCL = {ET_GPIO_PORT_SCL, ET_GPIO_PIN_SCL, "SCL"};

static bool isInit;
const DeckDriver *bcRpm = NULL;
static motorRpmParams_t motorRpm = {0};

static void expCfBlTestInit(DeckInfo *info)
{
(void)info;
if (isInit) {
return;
}

// Initialize the VL53L0 sensor using the zRanger deck driver
bcRpm = deckFindDriverByName("bcRpm");
motorsInit(platformConfigGetMotorMapping());
motorRpm.m1 = logGetVarId("rpm", "m1");
motorRpm.m2 = logGetVarId("rpm", "m2");
motorRpm.m3 = logGetVarId("rpm", "m3");
motorRpm.m4 = logGetVarId("rpm", "m4");
isInit = true;
}

static int getMotorRpm(uint16_t motorIdx)
{
uint16_t ret = 0xFF;
switch (motorIdx) {
case MOTOR_M1:
ret = logGetInt(motorRpm.m1);
break;

case MOTOR_M2:
ret = logGetInt(motorRpm.m2);
break;

case MOTOR_M3:
ret = logGetInt(motorRpm.m3);
break;

case MOTOR_M4:
ret = logGetInt(motorRpm.m4);
break;

default:
break;
}
return ret;
}

static void setMotorsPwm(uint32_t pwm)
{
for (int i = 0; i<NBR_OF_MOTORS; i++) {
motorsSetRatio(i, pwm);
}
}

static void runMotors()
{
#ifdef CONFIG_MOTORS_ESC_PROTOCOL_DSHOT
motorsBurstDshot();
#endif
vTaskDelay(MOTOR_FEED_SIGNAL_INTVL);
}



static bool rpmTestRun(void)
{
bool passed = true;
uint16_t testTime = MOTOR_TEST_TIME_MILLIS;
int32_t waitTime = MOTOR_TEST_TIME_MILLIS;
int32_t rpmSamples[] = {0,0,0,0};
setMotorsPwm(0);
while (waitTime) { //We need to wait until all ESCs are started. We need to feed a signal continuosly so they dont go to sleep
runMotors();
waitTime -= MOTOR_FEED_SIGNAL_INTVL;
}

setMotorsPwm(MOTOR_TEST_PWM);
while(testTime) {
runMotors();
testTime -= MOTOR_FEED_SIGNAL_INTVL;
for (int i = 0; i<NBR_OF_MOTORS; i++) {
rpmSamples[i] += getMotorRpm(i);
}
}

setMotorsPwm(0);
for (int i = 0; i<NBR_OF_MOTORS; i++) {
int rpmAvg = rpmSamples[i] / MOTOR_RPM_NBR_SAMPLES;
DEBUG_PRINT("Motor; %d RPM; %d\n", i, rpmAvg);
passed &= (rpmAvg > RPM_TEST_LOWER_LIMIT);
}
return passed;
}

static bool expCfBlTestRun(void)
{
int i;
volatile int delay;
bool status = true;
GPIO_InitTypeDef GPIO_InitStructure;
GpioRegBuf gpioSaved;

isInit = true;

status &= sensorsManufacturingTest();


// Enable GPIOs
RCC_AHB1PeriphClockCmd(ET_GPIO_PERIF, ENABLE);

decktestSaveGPIOStatesABC(&gpioSaved);

for (i = 0; i < ET_NBR_PINS; i++) {
//Initialize the pins as inputs
GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
}

for (i = 0; i < ET_NBR_PINS && status; i++) {
// Configure pin as output to poke others
GPIO_InitStructure.GPIO_Pin = etGpioIn[i].pin;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);

// Test high
GPIO_SetBits(etGpioIn[i].port, etGpioIn[i].pin);
for (delay = 0; delay < 1000; delay++);


// Test low
GPIO_ResetBits(etGpioIn[i].port, etGpioIn[i].pin);
for (delay = 0; delay < 1000; delay++);


// Restore
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;
GPIO_Init(etGpioIn[i].port, &GPIO_InitStructure);
}

decktestRestoreGPIOStatesABC(&gpioSaved);

//Run rpm test
if(status) {
if(bcRpm->init) {
bcRpm->init(NULL);
}

rpmTestRun();
}

if (status) {
// Configure SDA & SCL to turn on OK leds
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Low_Speed;
GPIO_InitStructure.GPIO_Pin = etGpioSDA.pin;
GPIO_Init(etGpioSDA.port, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = etGpioSCL.pin;
GPIO_Init(etGpioSCL.port, &GPIO_InitStructure);
// Turn on OK LEDs.
GPIO_ResetBits(etGpioSDA.port, etGpioSDA.pin);
GPIO_ResetBits(etGpioSCL.port, etGpioSCL.pin);
}

return status;
}




static const DeckDriver expCfBltest_deck = {
.vid = 0xFF,
.pid = 0xFF,
.name = "bcExpTestCfBl",

.usedGpio = 0xFFFFFFFF,

.init = expCfBlTestInit,
.test = expCfBlTestRun
};

DECK_DRIVER(expCfBltest_deck);

0 comments on commit fe48d59

Please sign in to comment.