Skip to content

Commit

Permalink
#546 Moved another ~20k to CCM.
Browse files Browse the repository at this point in the history
  • Loading branch information
tobbeanton committed May 18, 2020
1 parent f824316 commit 9886b37
Show file tree
Hide file tree
Showing 22 changed files with 91 additions and 58 deletions.
3 changes: 3 additions & 0 deletions src/deck/api/deck_spi.c
Original file line number Diff line number Diff line change
Expand Up @@ -223,6 +223,9 @@ bool spiTest(void)

bool spiExchange(size_t length, const uint8_t * data_tx, uint8_t * data_rx)
{
ASSERT_DMA_SAFE(data_tx);
ASSERT_DMA_SAFE(data_rx);

// DMA already configured, just need to set memory addresses
SPI_TX_DMA_STREAM->M0AR = (uint32_t)data_tx;
SPI_TX_DMA_STREAM->NDTR = length;
Expand Down
3 changes: 3 additions & 0 deletions src/deck/api/deck_spi3.c
Original file line number Diff line number Diff line change
Expand Up @@ -215,6 +215,9 @@ bool spi3Test(void)

bool spi3Exchange(size_t length, const uint8_t * data_tx, uint8_t * data_rx)
{
ASSERT_DMA_SAFE(data_tx);
ASSERT_DMA_SAFE(data_rx);

// DMA already configured, just need to set memory addresses
SPI_TX_DMA_STREAM->M0AR = (uint32_t)data_tx;
SPI_TX_DMA_STREAM->NDTR = length;
Expand Down
6 changes: 3 additions & 3 deletions src/deck/drivers/src/aideck.c
Original file line number Diff line number Diff line change
Expand Up @@ -130,9 +130,9 @@ static bool aideckTest()
}

static const DeckDriver aideck_deck = {
.vid = 0,
.pid = 0,
.name = "bcAIDeck",
.vid = 0xBC,
.pid = 0x12,
.name = "bcAI",

.usedPeriph = 0,
.usedGpio = 0, // FIXME: Edit the used GPIOs
Expand Down
2 changes: 1 addition & 1 deletion src/deck/drivers/src/lhtesterdeck.c
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ static bool isInit;

#define TIM_PERIF RCC_APB1Periph_TIM5
#define TIM TIM5
#define TIM_DBG DBGMCU_TIM2_STOP
#define TIM_DBG DBGMCU_TIM5_STOP
#define TIM_SETCOMPARE TIM_SetCompare2
#define TIM_GETCAPTURE TIM_GetCapture2

Expand Down
11 changes: 6 additions & 5 deletions src/deck/drivers/src/multiranger.c
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include "pca95x4.h"
#include "vl53l1x.h"
#include "range.h"
#include "static_mem.h"

#include "i2cdev.h"

Expand All @@ -52,11 +53,11 @@ static bool isPassed = false;
#define MR_PIN_LEFT PCA95X4_P6
#define MR_PIN_RIGHT PCA95X4_P2

static VL53L1_Dev_t devFront;
static VL53L1_Dev_t devBack;
static VL53L1_Dev_t devUp;
static VL53L1_Dev_t devLeft;
static VL53L1_Dev_t devRight;
NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devFront;
NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devBack;
NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devUp;
NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devLeft;
NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t devRight;

static uint16_t mrGetMeasurementAndRestart(VL53L1_Dev_t *dev)
{
Expand Down
4 changes: 4 additions & 0 deletions src/deck/drivers/src/test/aidecktest.c
Original file line number Diff line number Diff line change
Expand Up @@ -403,6 +403,10 @@ static bool aitdecktestTest()
DEBUG_PRINT("NINA->GAP8 reset [FAILED]\r\n");
}
}
else
{
DEBUG_PRINT("NINA->GAP8 reset, NINA not responding. [FAILED]\r\n");
}

//Test RST of both GAP8 and NINA by pulling reset
pinMode(DECK_GPIO_IO4, OUTPUT);
Expand Down
7 changes: 4 additions & 3 deletions src/deck/drivers/src/usddeck.c
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@
#include "log.h"
#include "param.h"
#include "crc_bosch.h"
#include "static_mem.h"

// Hardware defines
#ifdef USDDECK_USE_ALT_PINS_AND_SPI
Expand Down Expand Up @@ -112,7 +113,7 @@ static void csLow(void);
static void usdLogTask(void* prm);
static void usdWriteTask(void* prm);

static crc crcTable[256];
NO_DMA_CCM_SAFE_ZERO_INIT static crc crcTable[256];

static usdLogConfig_t usdLogConfig;

Expand All @@ -124,9 +125,9 @@ DWORD workBuff[512]; /* 2048 byte working buffer */
#endif

//Fatfs object
static FATFS FatFs;
NO_DMA_CCM_SAFE_ZERO_INIT static FATFS FatFs;
//File object
static FIL logFile;
NO_DMA_CCM_SAFE_ZERO_INIT static FIL logFile;
static SemaphoreHandle_t logFileMutex;

static QueueHandle_t usdLogQueue;
Expand Down
3 changes: 2 additions & 1 deletion src/deck/drivers/src/zranger.c
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include "log.h"
#include "param.h"
#include "range.h"
#include "static_mem.h"

#include "i2cdev.h"
#include "zranger.h"
Expand All @@ -55,7 +56,7 @@ static uint16_t range_last = 0;

static bool isInit;

static VL53L0xDev dev;
NO_DMA_CCM_SAFE_ZERO_INIT static VL53L0xDev dev;

void zRangerInit(DeckInfo* info)
{
Expand Down
3 changes: 2 additions & 1 deletion src/deck/drivers/src/zranger2.c
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
#include "log.h"
#include "param.h"
#include "range.h"
#include "static_mem.h"

#include "i2cdev.h"
#include "zranger2.h"
Expand All @@ -56,7 +57,7 @@ static uint16_t range_last = 0;

static bool isInit;

static VL53L1_Dev_t dev;
NO_DMA_CCM_SAFE_ZERO_INIT static VL53L1_Dev_t dev;

static uint16_t zRanger2GetMeasurementAndRestart(VL53L1_Dev_t *dev)
{
Expand Down
6 changes: 6 additions & 0 deletions src/drivers/src/i2c_drv.c
Original file line number Diff line number Diff line change
Expand Up @@ -208,6 +208,8 @@ static inline void i2cdrvRoughLoopDelay(uint32_t us)

static void i2cdrvStartTransfer(I2cDrv *i2c)
{
ASSERT_DMA_SAFE(i2c->txMessage.buffer);

if (i2c->txMessage.direction == i2cRead)
{
i2c->DMAStruct.DMA_BufferSize = i2c->txMessage.messageLength;
Expand Down Expand Up @@ -380,6 +382,8 @@ void i2cdrvCreateMessage(I2cMessage *message,
uint32_t length,
uint8_t *buffer)
{
ASSERT_DMA_SAFE(buffer);

message->slaveAddress = slaveAddress;
message->direction = direction;
message->isInternal16bit = false;
Expand All @@ -398,6 +402,8 @@ void i2cdrvCreateMessageIntAddr(I2cMessage *message,
uint32_t length,
uint8_t *buffer)
{
ASSERT_DMA_SAFE(buffer);

message->slaveAddress = slaveAddress;
message->direction = direction;
message->isInternal16bit = IsInternal16;
Expand Down
2 changes: 1 addition & 1 deletion src/drivers/src/piezo.c
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@
// HW defines
#define PIEZO_TIM_PERIF RCC_APB1Periph_TIM5
#define PIEZO_TIM TIM5
#define PIEZO_TIM_DBG DBGMCU_TIM2_STOP
#define PIEZO_TIM_DBG DBGMCU_TIM5_STOP
#define PIEZO_TIM_SETCOMPARE TIM_SetCompare2
#define PIEZO_TIM_GETCAPTURE TIM_GetCapture2

Expand Down
26 changes: 16 additions & 10 deletions src/drivers/src/vl53l1x.c
Original file line number Diff line number Diff line change
Expand Up @@ -194,14 +194,16 @@ VL53L1_Error VL53L1_RdByte(
uint16_t index,
uint8_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_Error status = VL53L1_ERROR_NONE;
static uint8_t r8data;

if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 1, pdata))
if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 1, &r8data))
{
status = VL53L1_ERROR_CONTROL_INTERFACE;
}
*pdata = r8data;

return status;
return status;
}


Expand All @@ -210,14 +212,16 @@ VL53L1_Error VL53L1_RdWord(
uint16_t index,
uint16_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_Error status = VL53L1_ERROR_NONE;
static uint16_t r16data;

if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)pdata))
if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 2, (uint8_t *)&r16data))
{
status = VL53L1_ERROR_CONTROL_INTERFACE;
}

return status;
*pdata = r16data;

return status;
}


Expand All @@ -226,14 +230,16 @@ VL53L1_Error VL53L1_RdDWord(
uint16_t index,
uint32_t *pdata)
{
VL53L1_Error status = VL53L1_ERROR_NONE;
VL53L1_Error status = VL53L1_ERROR_NONE;
static uint32_t r32data;

if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)pdata))
if (!i2cdevRead16(pdev->I2Cx, pdev->devAddr, index, 4, (uint8_t *)&r32data))
{
status = VL53L1_ERROR_CONTROL_INTERFACE;
}
*pdata = r32data;

return status;
return status;
}

/*
Expand Down
1 change: 1 addition & 0 deletions src/drivers/src/ws2812_cf2.c
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,7 @@ void ws2812Init(void)
/* DMA1 Channel5 Config TM */
DMA_DeInit(DMA1_Stream5);

ASSERT_DMA_SAFE(led_dma.buffer);
// USART TX DMA Channel Config
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR2;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)led_dma.buffer; // this is the buffer memory
Expand Down
9 changes: 5 additions & 4 deletions src/hal/src/ledseq.c
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@
#include "FreeRTOS.h"
#include "timers.h"
#include "semphr.h"
#include "static_mem.h"

#include "led.h"

Expand Down Expand Up @@ -167,12 +168,12 @@ static void updateActive(led_t led);

//State of every sequence for every led: LEDSEQ_STOP if stopped or the current
//step
static int state[LED_NUM][SEQ_NUM];
NO_DMA_CCM_SAFE_ZERO_INIT static int state[LED_NUM][SEQ_NUM];
//Active sequence for each led
static int activeSeq[LED_NUM];
NO_DMA_CCM_SAFE_ZERO_INIT static int activeSeq[LED_NUM];

static xTimerHandle timer[LED_NUM];
static StaticTimer_t timerBuffer[LED_NUM];
NO_DMA_CCM_SAFE_ZERO_INIT static xTimerHandle timer[LED_NUM];
NO_DMA_CCM_SAFE_ZERO_INIT static StaticTimer_t timerBuffer[LED_NUM];

static xSemaphoreHandle ledseqSem;

Expand Down
1 change: 1 addition & 0 deletions src/hal/src/sensors_bmi088_spi_bmp388.c
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,7 @@
#define GYR_DIS_CS() GPIO_SetBits(BMI088_GYR_GPIO_CS_PORT, BMI088_GYR_GPIO_CS)

/* Defines and buffers for full duplex SPI DMA transactions */
/* The buffers must not be placed in CCM */
#define SPI_MAX_DMA_TRANSACTION_SIZE 15
static uint8_t spiTxBuffer[SPI_MAX_DMA_TRANSACTION_SIZE + 1];
static uint8_t spiRxBuffer[SPI_MAX_DMA_TRANSACTION_SIZE + 1];
Expand Down
2 changes: 1 addition & 1 deletion src/hal/src/usb.c
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@
#include "static_mem.h"


__ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END ;
NO_DMA_CCM_SAFE_ZERO_INIT __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END ;

static bool isInit = false;
static bool doingTransfer = false;
Expand Down
3 changes: 2 additions & 1 deletion src/lib/STM32_USB_Device_Library/Core/src/usbd_req.c
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include "usbd_req.h"
#include "usbd_ioreq.h"
#include "usbd_desc.h"
#include "static_mem.h"


/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
Expand Down Expand Up @@ -99,7 +100,7 @@ __ALIGN_BEGIN uint32_t USBD_cfg_status __ALIGN_END = 0;
#pragma data_alignment=4
#endif
#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
__ALIGN_BEGIN uint8_t USBD_StrDesc[USB_MAX_STR_DESC_SIZ] __ALIGN_END ;
NO_DMA_CCM_SAFE_ZERO_INIT __ALIGN_BEGIN uint8_t USBD_StrDesc[USB_MAX_STR_DESC_SIZ] __ALIGN_END ;
/**
* @}
*/
Expand Down
25 changes: 13 additions & 12 deletions src/modules/src/kalman_core.c
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@
#include "param.h"
#include "math3d.h"
#include "debug.h"
#include "static_mem.h"

// #define DEBUG_STATE_CHECK

Expand Down Expand Up @@ -225,23 +226,23 @@ void kalmanCoreInit(kalmanCoreData_t* this) {
static void scalarUpdate(kalmanCoreData_t* this, arm_matrix_instance_f32 *Hm, float error, float stdMeasNoise)
{
// The Kalman gain as a column vector
static float K[KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float K[KC_STATE_DIM];
static arm_matrix_instance_f32 Km = {KC_STATE_DIM, 1, (float *)K};

// Temporary matrices for the covariance updates
static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};

static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
static arm_matrix_instance_f32 tmpNN2m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};

static float tmpNN3d[KC_STATE_DIM * KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN3d[KC_STATE_DIM * KC_STATE_DIM];
static arm_matrix_instance_f32 tmpNN3m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN3d};

static float HTd[KC_STATE_DIM * 1];
NO_DMA_CCM_SAFE_ZERO_INIT static float HTd[KC_STATE_DIM * 1];
static arm_matrix_instance_f32 HTm = {KC_STATE_DIM, 1, HTd};

static float PHTd[KC_STATE_DIM * 1];
NO_DMA_CCM_SAFE_ZERO_INIT static float PHTd[KC_STATE_DIM * 1];
static arm_matrix_instance_f32 PHTm = {KC_STATE_DIM, 1, PHTd};

ASSERT(Hm->numRows == 1);
Expand Down Expand Up @@ -670,14 +671,14 @@ void kalmanCorePredict(kalmanCoreData_t* this, float cmdThrust, Axis3f *acc, Axi
*/

// The linearized update matrix
static float A[KC_STATE_DIM][KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float A[KC_STATE_DIM][KC_STATE_DIM];
static arm_matrix_instance_f32 Am = { KC_STATE_DIM, KC_STATE_DIM, (float *)A}; // linearized dynamics for covariance update;

// Temporary matrices for the covariance updates
static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
static arm_matrix_instance_f32 tmpNN1m = { KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};

static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
static arm_matrix_instance_f32 tmpNN2m = { KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};

float dt2 = dt*dt;
Expand Down Expand Up @@ -923,14 +924,14 @@ void kalmanCoreAddProcessNoise(kalmanCoreData_t* this, float dt)
void kalmanCoreFinalize(kalmanCoreData_t* this, uint32_t tick)
{
// Matrix to rotate the attitude covariances once updated
static float A[KC_STATE_DIM][KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float A[KC_STATE_DIM][KC_STATE_DIM];
static arm_matrix_instance_f32 Am = {KC_STATE_DIM, KC_STATE_DIM, (float *)A};

// Temporary matrices for the covariance updates
static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN1d[KC_STATE_DIM * KC_STATE_DIM];
static arm_matrix_instance_f32 tmpNN1m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN1d};

static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
NO_DMA_CCM_SAFE_ZERO_INIT static float tmpNN2d[KC_STATE_DIM * KC_STATE_DIM];
static arm_matrix_instance_f32 tmpNN2m = {KC_STATE_DIM, KC_STATE_DIM, tmpNN2d};

// Incorporate the attitude error (Kalman filter state) with the attitude
Expand Down
2 changes: 1 addition & 1 deletion src/modules/src/log.c
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ struct log_block {
struct log_ops * ops;
};

static struct log_ops logOps[LOG_MAX_OPS];
NO_DMA_CCM_SAFE_ZERO_INIT static struct log_ops logOps[LOG_MAX_OPS];
NO_DMA_CCM_SAFE_ZERO_INIT static struct log_block logBlocks[LOG_MAX_BLOCKS];
static xSemaphoreHandle logLock;
static StaticSemaphore_t logLockBuffer;
Expand Down
Loading

0 comments on commit 9886b37

Please sign in to comment.