Currently, I am attempting to send data via SPI to the gyro based off of a simplified version of ardupilot's implementation of this: https://github.com/ArduPilot/ardupilot/ ... #L238-L266. This function attempts multiple times to send the data to the register and then reads the register to make sure that the register has the value that was sent.
Here is the main function code:
Code: Select all
SPIConfig bmi270_spi_config = {
.circular = false,
.end_cb = NULL,
.ssport = GPIOB,
.sspad = GPIOB_PIN2_GYRO1_CS,
.cr1 = SPI_CR1_BR_1 | SPI_CR1_CPOL | SPI_CR1_CPHA,
.cr2 = 0,
};
static void internal_read_register(SPIDriver* spip, uint8_t reg, uint8_t *buf, int8_t len) {
reg |= 0x80;
spiSend(spip, 1, ®);
spiReceive(spip, len, buf);
}
static bool write_register(SPIDriver* spip, uint8_t reg, uint8_t val) {
bool worked = false;
// spiAcquireBus(spip);
spiStart(spip, &bmi270_spi_config);
uint8_t buf[] = {reg, val};
spiSelect(spip);
for (int i = 0; i < 1000; i++) {
spiSend(spip, 2, buf);
uint8_t v2 = 0;
internal_read_register(spip, reg, &v2, 1);
if (v2 == val) {
worked = true;
break;
}
}
spiUnselect(spip);
// spiReleaseBus(spip);
return worked;
}
static void read_register(SPIDriver* spip, uint8_t reg, uint8_t *buf, int8_t len) {
// spiAcquireBus(spip);
spiStart(spip, &bmi270_spi_config);
spiSelect(spip);
internal_read_register(spip, reg, buf, len);
spiUnselect(spip);
// spiReleaseBus(spip);
}
Register Setup:
Code: Select all
palSetPadMode(GPIOB, GPIOB_PIN2_GYRO1_CS, PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(GPIOA, GPIOA_PIN5_SPI_GYRO_SCK, PAL_MODE_ALTERNATE(5)| PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOA, GPIOA_PIN6_SPI_GYRO_MISO, PAL_MODE_ALTERNATE(5)| PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOA, GPIOA_PIN7_SPI_GYRO_MOSI, PAL_MODE_ALTERNATE(5)| PAL_STM32_OSPEED_HIGHEST);
When I run a basic read to activate the SPI, followed by a write to soft reset the device the SPI it seems like write function fails to actually update the register it is writing to. Note that this is based off of ardupilot's implementation (https://github.com/ArduPilot/ardupilot/ ... #L523-L532) of this setup:
Code: Select all
uint8_t chip_id = 0;
bool worked = false;
read_register(spiDriver, 0x00, &chip_id, 1);
chThdSleepMilliseconds(1);
worked = write_register(spiDriver, 0x7E, 0xB6);
chThdSleepMilliseconds(5);
log_info("WORKED: %d", worked);
Based off of reading other forum posts regarding SPI and looking at examples, I can't find anything wrong with my implementation of this write function (the log outputs zero, which means that the write never matched the data). I have tested the read function and it seems to be able to read different values in other contexts and I have verified with other software (betaflight) that the gyro can be interfaced with, so that rules out hardware issues and indicates something must be wrong with my write code. Is there something wrong with the way I am invoking the spiSend function which causes it to not update the register?