static uint16_t adis16405_read_u16(uint8_t addr_in) {
// All transfers are 16 bits
// Addresses are 7 bits
// We therefore address using the following system for reading
// MSB --- LSB
// 0 write bit, 7 bit address, 8 bits of 0
// For example, 0x0A00 would read address 0x0A
// Clear write bit
// Shift into position
spiAcquireBus(&ADIS16405_SPID);
spiStart(&ADIS16405_SPID, &spi_cfg);
uint16_t data_rx = internal_adis16405_read_u16(addr_in);
spiReleaseBus(&ADIS16405_SPID);
return data_rx;
}
msg_t Mtd25::wait_op_complete(systime_t timeout) {
systime_t start = chVTGetSystemTimeX();
systime_t end = start + timeout;
osalThreadSleepMilliseconds(2);
while (chVTIsSystemTimeWithinX(start, end)) {
uint8_t tmp;
#if SPI_USE_MUTUAL_EXCLUSION
spiAcquireBus(this->spip);
#endif
spiSelect(spip);
spiPolledExchange(spip, S25_CMD_RDSR1);
tmp = spiPolledExchange(spip, 0);
spiUnselect(spip);
#if SPI_USE_MUTUAL_EXCLUSION
spiReleaseBus(this->spip);
#endif
if (tmp & S25_SR1_WIP) {
continue;
}
else {
if (tmp & (S25_SR1_PERR | S25_SR1_EERR))
return MSG_RESET;
else
return MSG_OK;
}
osalThreadSleepMilliseconds(10);
}
/* time is out */
return MSG_RESET;
}
msg_t Mtd25::spi_write(const uint8_t *txdata, size_t len,
uint8_t *writebuf, size_t preamble_len) {
memcpy(&writebuf[preamble_len], txdata, len);
if (MSG_OK != spi_write_enable())
return MSG_RESET;
#if SPI_USE_MUTUAL_EXCLUSION
spiAcquireBus(this->spip);
#endif
spiSelect(spip);
spiSend(spip, preamble_len + len, writebuf);
spiUnselect(spip);
#if SPI_USE_MUTUAL_EXCLUSION
spiReleaseBus(this->spip);
#endif
return wait_op_complete(cfg.programtime);
}
/*! \brief Process information from a adis_spi_cb event
*
*/
void adis_spi_cb_txdone_handler(eventid_t id) {
(void) id;
switch(adis_driver.reg) {
case ADIS_PRODUCT_ID:
spiUnselect(adis_driver.spi_instance);
spiReleaseBus(adis_driver.spi_instance);
adis_tstall_delay();
spiAcquireBus(adis_driver.spi_instance);
spiSelect(adis_driver.spi_instance);
spiStartReceive(adis_driver.spi_instance, adis_driver.rx_numbytes, adis_driver.adis_rxbuf);
adis_driver.state = ADIS_RX_PEND;
break;
case ADIS_GLOB_CMD:
spiStartReceive(adis_driver.spi_instance, adis_driver.rx_numbytes, adis_driver.adis_rxbuf);
adis_driver.state = ADIS_RX_PEND;
break;
default:
spiUnselect(adis_driver.spi_instance);
spiReleaseBus(adis_driver.spi_instance);
adis_driver.state = ADIS_IDLE;
break;
}
}
void sc_spi_send(uint8_t spin, uint8_t *data, size_t bytes)
{
SPIConfig spi_cfg;
chDbgAssert(spin < SC_SPI_MAX_CLIENTS, "SPI n outside range", "#4");
chDbgAssert(spi_conf[spin].spip != NULL, "SPI n not initialized", "#3");
/* spiStart always to set CS per SPI client. */
spi_cfg.end_cb = NULL;
spi_cfg.ssport = spi_conf[spin].cs_port;
spi_cfg.sspad = spi_conf[spin].cs_pin;
spi_cfg.cr1 = spi_conf[spin].cr1;
spiStart(spi_conf[spin].spip, &spi_cfg);
spiAcquireBus(spi_conf[spin].spip);
spiSelect(spi_conf[spin].spip);
spiSend(spi_conf[spin].spip, bytes, data);
spiUnselect(spi_conf[spin].spip);
spiReleaseBus(spi_conf[spin].spip);
spiStop(spi_conf[spin].spip);
}
void sc_spi_exchange(uint8_t spin, uint8_t *tx, uint8_t *rx, size_t bytes)
{
SPIConfig spi_cfg;
chDbgAssert(spin < SC_SPI_MAX_CLIENTS, "SPI n outside range", "#3");
chDbgAssert(spi_conf[spin].spip != NULL, "SPI n not initialized", "#2");
/* spiStart always to set CS for every call according to SPI client. */
spi_cfg.end_cb = NULL;
spi_cfg.ssport = spi_conf[spin].cs_port;
spi_cfg.sspad = spi_conf[spin].cs_pin;
spi_cfg.cr1 = spi_conf[spin].cr1;
spiStart(spi_conf[spin].spip, &spi_cfg);
spiAcquireBus(spi_conf[spin].spip);
spiSelect(spi_conf[spin].spip);
spiExchange(spi_conf[spin].spip, bytes, tx, rx);
spiUnselect(spi_conf[spin].spip);
spiReleaseBus(spi_conf[spin].spip);
spiStop(spi_conf[spin].spip);
}
/**
* @brief Retrieves raw data from the BaseGyroscope.
* @note This data is retrieved from MEMS register without any algebraical
* manipulation.
* @note The axes array must be at least the same size of the
* BaseGyroscope axes number.
*
* @param[in] ip pointer to @p BaseGyroscope interface.
* @param[out] axes a buffer which would be filled with raw data.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*/
static msg_t gyro_read_raw(void *ip, int32_t axes[L3GD20_GYRO_NUMBER_OF_AXES]) {
L3GD20Driver* devp;
int16_t tmp;
uint8_t i, buff [2 * L3GD20_GYRO_NUMBER_OF_AXES];
msg_t msg = MSG_OK;
osalDbgCheck((ip != NULL) && (axes != NULL));
/* Getting parent instance pointer.*/
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
osalDbgAssert((devp->state == L3GD20_READY),
"gyro_read_raw(), invalid state");
#if L3GD20_USE_SPI
osalDbgAssert((devp->config->spip->state == SPI_READY),
"gyro_read_raw(), channel not ready");
#if L3GD20_SHARED_SPI
spiAcquireBus(devp->config->spip);
spiStart(devp->config->spip,
devp->config->spicfg);
#endif /* L3GD20_SHARED_SPI */
l3gd20SPIReadRegister(devp->config->spip, L3GD20_AD_OUT_X_L,
L3GD20_GYRO_NUMBER_OF_AXES * 2, buff);
#if L3GD20_SHARED_SPI
spiReleaseBus(devp->config->spip);
#endif /* L3GD20_SHARED_SPI */
#endif /* L3GD20_USE_SPI */
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
axes[i] = (int32_t)tmp;
}
return msg;
}
static uint32_t getLuxStr(char *luxString)
{
uint16_t lux = 0;
spiAcquireBus(&CC3000_SPI_DRIVER);
i2cAcquireBus(&I2C_DRIVER);
i2cStart(&I2C_DRIVER, &i2cConfig);
if (RDY_OK != (tslReadLuxConvertSleep(&I2C_DRIVER,
TSL2561_ADDR_FLOAT,
&lux)))
{
PRINT("Error", NULL);
}
i2cStop(&I2C_DRIVER);
i2cReleaseBus(&I2C_DRIVER);
spiReleaseBus(&CC3000_SPI_DRIVER);
snprintf(luxString, LUX_STRING_SIZE, "%d lx", lux);
luxString[LUX_STRING_SIZE-1] = 0;
return 0;
}
/**
* @brief Deactivates the LIS302DL Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LIS302DLDriver object
*
* @api
*/
void lis302dlStop(LIS302DLDriver *devp) {
uint8_t cr1;
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LIS302DL_STOP) || (devp->state == LIS302DL_READY),
"lis302dlStop(), invalid state");
if (devp->state == LIS302DL_READY) {
#if LIS302DL_USE_SPI
#if LIS302DL_SHARED_SPI
spiAcquireBus((devp)->config->spip);
spiStart((devp)->config->spip,
(devp)->config->spicfg);
#endif /* LIS302DL_SHARED_SPI */
cr1 = 0;
lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG1, 1, &cr1);
spiStop((devp)->config->spip);
#if LIS302DL_SHARED_SPI
spiReleaseBus((devp)->config->spip);
#endif /* LIS302DL_SHARED_SPI */
#endif /* LIS302DL_USE_SPI */
}
devp->state = LIS302DL_STOP;
}
static flash_error_t program(void *instance, flash_address_t addr,
const uint8_t *pp, size_t n) {
N25Q128Driver *devp = (N25Q128Driver *)instance;
SPIDriver *spip = devp->config->spip;
flash_error_t err;
osalDbgAssert(devp->state == FLASH_READY, "invalid state");
#if N25Q128_SHARED_SPI == TRUE
spiAcquireBus(spip);
spiStart(spip, devp->config->spicfg);
#endif
devp->state = FLASH_ACTIVE;
while (n > 0U) {
uint8_t sts;
/* Data size that can be written in a single program page operation.*/
size_t chunk = (size_t)(((addr | PAGE_MASK) + 1U) - addr);
if (chunk > n) {
chunk = n;
}
/* Enabling write operation.*/
spiSelect(spip);
spi_send_cmd(devp, N25Q128_CMD_WRITE_ENABLE);
spiUnselect(spip);
(void) spiPolledExchange(spip, 0xFF); /* One frame delay.*/
/* Page program command.*/
spiSelect(spip);
spi_send_cmd_addr(devp, N25Q128_CMD_PAGE_PROGRAM, addr);
spiSend(spip, chunk, pp);
spiUnselect(spip);
(void) spiPolledExchange(spip, 0xFF); /* One frame delay.*/
/* Operation end waiting.*/
do {
#if N25Q128_NICE_WAITING == TRUE
osalThreadSleepMilliseconds(1);
#endif
/* Read status command.*/
spiSelect(spip);
spi_send_cmd(devp, N25Q128_CMD_READ_STATUS_REGISTER);
spiReceive(spip, 1, &sts);
spiUnselect(spip);
} while ((sts & N25Q128_STS_BUSY) != 0U);
/* Checking for errors.*/
if ((sts & N25Q128_STS_ALL_ERRORS) != 0U) {
/* Clearing status register.*/
(void) spiPolledExchange(spip, 0xFF); /* One frame delay.*/
spiSelect(spip);
spi_send_cmd(devp, N25Q128_CMD_CLEAR_FLAG_STATUS_REGISTER);
spiUnselect(spip);
/* Program operation failed.*/
err = FLASH_PROGRAM_FAILURE;
goto exit_error;
}
/* Next page.*/
addr += chunk;
pp += chunk;
n -= chunk;
}
/* Program operation succeeded.*/
err = FLASH_NO_ERROR;
/* Common exit path for this function.*/
exit_error:
devp->state = FLASH_READY;
#if N25Q128_SHARED_SPI == TRUE
spiReleaseBus(spip);
#endif
return err;
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
uint32_t tempR, tempP, tempY;
float Roll,Pitch,Yaw;
halInit();
chSysInit();
i2c_setup();
palSetPadMode(IOPORT1, 8, PAL_MODE_INPUT); // PA8 - RADIO INPUT
palSetPadMode(IOPORT2, 13, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* SCK. */
palSetPadMode(IOPORT2, 14, PAL_MODE_STM32_ALTERNATE_PUSHPULL); /* MISO.*/
palSetPadMode(IOPORT2, 15, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); /* MOSI.*/
palSetPadMode(IOPORT2, 12, PAL_MODE_STM32_ALTERNATE_OPENDRAIN);
palSetPadMode(IOPORT3, 11, PAL_MODE_OUTPUT_PUSHPULL);
chMtxInit(&mtx_imu);
chMtxInit(&mutex_motors);
palClearPad(IOPORT3,10);
//palClearPad(IOPORT3,11);
/*
* Activates the USB driver and then the USB bus pull-up on D+.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbConnectBus(serusbcfg.usbp);
//palClearPad(GPIOC, GPIOC_USB_DISC);
icuStart(&ICUD1, &icucfg);
icuEnable(&ICUD1);
chThdSleepSeconds(1);
chThdCreateStatic(waThread1, sizeof(waThread1), HIGHPRIO, Thread1, NULL);
//chThdCreateStatic(waMotorsThread, sizeof(waMotorsThread), NORMALPRIO, MotorsThread, NULL);
spiAcquireBus(&SPID2); /* Acquire ownership of the bus. */
spiStart(&SPID2, &ls_spicfg); /* Setup transfer parameters. */
spiSelect(&SPID2);
KP = 1;
KI = 1;
KD = 0;
while (TRUE) {
while(!(SPID2.spi->SR & SPI_SR_RXNE));
spiReceive(&SPID2,24,rxbuf_16bit);
// Floating point calculation of Roll/Pitch/Yaw inside the microcontroller. Decomment next 6 lines if you want to implement
// the control Law.
tempR = (uint32_t)((rxbuf_16bit[0] << 31) | (rxbuf_16bit[1] << 23) | (rxbuf_16bit[2] << 11) | rxbuf_16bit[3]);
tempP = (uint32_t)((rxbuf_16bit[4] << 31) | (rxbuf_16bit[5] << 23) | (rxbuf_16bit[6] << 11) | rxbuf_16bit[7]);
tempY = (uint32_t)((rxbuf_16bit[8] << 31) | (rxbuf_16bit[9] << 23) | (rxbuf_16bit[10] << 11) | rxbuf_16bit[11]);
Roll = (*(float*)&tempR);
Pitch = (*(float*)&tempP);
Yaw = (*(float*)&tempY);
// CONTROL LAW HERE
//++ (ROLL,PITCH,YAW ERRORS) -----> [CONTROLLER] -----> (MOTORS INPUT)
// PID Control Law
roll_error = 0 - Roll;
pitch_error = 0 - Pitch;
yaw_error = 0 - Yaw;
roll_I = roll_I + roll_error*0.01;
pitch_I = pitch_I + pitch_error*0.01;
roll_D = (roll_error - roll_prev_error)/0.01;
pitch_D = (pitch_error - pitch_prev_error)/0.01;
roll_controller_output = (int8_t)(KP*roll_error + KI*roll_I + KD*roll_D);
pitch_controller_output = (int8_t)(KP*pitch_error + KI*pitch_I + KD*pitch_D);
//roll_controller_output = (int8_t)roll_error;
//pitch_controller_output = (int8_t)pitch_error;
roll_prev_error = roll_error;
pitch_prev_error = pitch_error;
//-------------------------------------------------------------------
blctrl20_set_velocity();
palSetPad(IOPORT3,11);
/* SPI FLOATING POINT TEST PACKET (sending 5.6F)
rxbuf_16bit[0] = 0;
rxbuf_16bit[1] = 129;
rxbuf_16bit[2] = 1638;
rxbuf_16bit[3] = 819;
rxbuf_16bit[4] = 0;
rxbuf_16bit[5] = 129;
rxbuf_16bit[6] = 1638;
rxbuf_16bit[7] = 819;
rxbuf_16bit[8] = 1;
rxbuf_16bit[9] = 129;
rxbuf_16bit[10] = 1638;
rxbuf_16bit[11] = 819;
*/
if(SDU1.config->usbp->state==USB_ACTIVE)
{
// chprintf((BaseChannel *)&SDU1,"S:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:%6d:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11]);
chprintf((BaseChannel *)&SDU1, "S:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:%6D:E\r\n",rxbuf_16bit[0],rxbuf_16bit[1],rxbuf_16bit[2],rxbuf_16bit[3],rxbuf_16bit[4],rxbuf_16bit[5],rxbuf_16bit[6],rxbuf_16bit[7],rxbuf_16bit[8],rxbuf_16bit[9],rxbuf_16bit[10],rxbuf_16bit[11],(int8_t)Roll,(int8_t)Pitch,(int8_t)Yaw,icu_ch[3],icu_ch[4],roll_controller_output,pitch_controller_output,yaw_controller_output);
}
chThdSleepMilliseconds(10);
}
spiUnselect(&SPID2);
spiReleaseBus(&SPID2); /* Ownership release. */
}
void eeprom_select(void){
spiAcquireBus(&EEPROM_SPI_BUS); /* Acquire ownership of the bus.*/
spiSelect(&EEPROM_SPI_BUS); /* Slave Select assertion. */
}
void frontend_configure(void)
{
spiStart(&FRONTEND_SPI, &spi_config);
spiAcquireBus(&FRONTEND_SPI);
spiSelect(&FRONTEND_SPI);
for (u8 i = 0; i < 2; ++i) {
spi_write(2, 0x03);
spi_write(3, 0x01);
spi_write(4, 0x03);
spi_write(5, 0x00);
spi_write(6, 0x1F);
spi_write(9, 0x00);
spi_write(11, 0x08);
spi_write(12, 0x1C);
spi_write(13, 0x03);
spi_write(14, 0x69);
spi_write(15, 0x2B);
spi_write(16, 0x74);
spi_write(17, 0xF1);
spi_write(18, 0xEA);
spi_write(19, 0x0B);
spi_write(20, 0x01);
spi_write(21, 0x28);
spi_write(22, 0x2B);
spi_write(23, 0x74);
spi_write(24, 0xF1);
spi_write(25, 0xEA);
spi_write(26, 0x0B);
spi_write(27, 0x01);
spi_write(28, 0x28);
spi_write(29, 0x2B);
spi_write(30, 0x74);
spi_write(31, 0xF1);
spi_write(32, 0xEA);
spi_write(33, 0x0B);
spi_write(34, 0x03);
spi_write(35, 0x7F);
spi_write(36, 0x2B);
spi_write(37, 0x74);
spi_write(38, 0xF1);
spi_write(39, 0xEA);
spi_write(40, 0x0B);
spi_write(41, 0x03);
spi_write(42, 0x4F);
spi_write(43, 0x89);
spi_write(45, 0x01);
spi_write(46, 0x7B);
spi_write(47, 0x91);
spi_write(49, 0xBD);
spi_write(50, 0x0C);
spi_write(51, 0x64);
spi_write(52, 0xA8);
spi_write(53, 0x5B);
spi_write(54, 0xE5);
spi_write(55, 0x57);
spi_write(56, 0xE4);
spi_write(57, 0xDD);
spi_write(58, 0x0C);
spi_write(59, 0x64);
spi_write(60, 0xA8);
spi_write(61, 0xC5);
spi_write(62, 0x6D);
spi_write(63, 0x57);
spi_write(64, 0x71);
spi_write(65, 0x7F);
spi_write(66, 0x00);
spi_write(67, 0xC9);
spi_write(68, 0x86);
spi_write(69, 0x7E);
spi_write(70, 0x0F);
spi_write(71, 0x11);
spi_write(72, 0x00);
spi_write(73, 0x00);
spi_write(74, 0xFF);
spi_write(75, 0x1C);
spi_write(76, 0xCD);
spi_write(77, 0x99);
spi_write(78, 0x0B);
spi_write(79, 0x40);
spi_write(80, 0x08);
spi_write(81, 0x30);
spi_write(82, 0xFF);
spi_write(83, 0x1C);
spi_write(84, 0xCD);
spi_write(85, 0x99);
spi_write(86, 0x0B);
spi_write(87, 0x40);
spi_write(88, 0x08);
spi_write(89, 0x30);
spi_write(90, 0xFF);
spi_write(91, 0x0C);
spi_write(92, 0xCD);
spi_write(93, 0x99);
spi_write(94, 0x0B);
spi_write(95, 0x40);
spi_write(96, 0x08);
spi_write(97, 0x30);
spi_write(98, 0xFF);
spi_write(99, 0x0C);
spi_write(100, 0xCD);
spi_write(101, 0x99);
spi_write(102, 0x0B);
spi_write(103, 0x40);
spi_write(104, 0x08);
spi_write(105, 0x30);
}
spi_write(15, 0x0B);
spi_write(22, 0x0B);
spi_write(29, 0x0B);
spi_write(36, 0x0B);
spiUnselect(&FRONTEND_SPI);
spiReleaseBus(&FRONTEND_SPI);
}
/**
* @brief Configures and activates LIS302DL Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LIS302DLDriver object
* @param[in] config pointer to the @p LIS302DLConfig object
*
* @api
*/
void lis302dlStart(LIS302DLDriver *devp, const LIS302DLConfig *config) {
uint32_t i;
uint8_t cr[2] = {0, 0};
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == LIS302DL_STOP) || (devp->state == LIS302DL_READY),
"lis302dlStart(), invalid state");
devp->config = config;
/* Control register 1 configuration block.*/
{
cr[0] = LIS302DL_CTRL_REG1_XEN | LIS302DL_CTRL_REG1_YEN |
LIS302DL_CTRL_REG1_ZEN | LIS302DL_CTRL_REG1_PD |
devp->config->outputdatarate |
devp->config->fullscale;
}
/* Control register 2 configuration block.*/
{
#if LIS302DL_USE_ADVANCED || defined(__DOXYGEN__)
if(devp->config->hpmode != LIS302DL_HPM_BYPASSED)
cr[1] = devp->config->highpass;
#endif
}
#if LIS302DL_USE_SPI
#if LIS302DL_SHARED_SPI
spiAcquireBus((devp)->config->spip);
#endif /* LIS302DL_SHARED_SPI */
spiStart((devp)->config->spip, (devp)->config->spicfg);
lis302dlSPIWriteRegister(devp->config->spip, LIS302DL_AD_CTRL_REG1,
2, cr);
#if LIS302DL_SHARED_SPI
spiReleaseBus((devp)->config->spip);
#endif /* LIS302DL_SHARED_SPI */
#endif /* LIS302DL_USE_SPI */
/* Storing sensitivity information according to full scale value */
if(devp->config->fullscale == LIS302DL_FS_2G) {
devp->fullscale = LIS302DL_2G;
if(devp->config->sensitivity == NULL)
for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
devp->sensitivity[i] = LIS302DL_SENS_2G;
else
for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
devp->sensitivity[i] = devp->config->sensitivity[i];
}
else if(devp->config->fullscale == LIS302DL_FS_8G) {
devp->fullscale = LIS302DL_8G;
if(devp->config->sensitivity == NULL)
for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
devp->sensitivity[i] = LIS302DL_SENS_8G;
else
for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
devp->sensitivity[i] = devp->config->sensitivity[i];
}
else {
osalDbgAssert(FALSE, "lis302dlStart(), accelerometer full scale issue");
}
if(devp->config->bias != NULL)
for(i = 0; i < LIS302DL_NUMBER_OF_AXES; i++)
devp->bias[i] = devp->config->bias[i];
/* This is the Accelerometer transient recovery time */
osalThreadSleepMilliseconds(10);
devp->state = LIS302DL_READY;
}
/**
* @brief Changes the L3GD20Driver gyroscope fullscale value.
* @note This function also rescale sensitivities and biases based on
* previous and next fullscale value.
* @note A recalibration is highly suggested after calling this function.
*
* @param[in] devp pointer to @p BaseGyroscope interface.
* @param[in] fs new fullscale value.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET otherwise.
*/
static msg_t gyro_set_full_scale(L3GD20Driver *devp, l3gd20_fs_t fs) {
float newfs, scale;
uint8_t i, cr;
msg_t msg = MSG_OK;
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == L3GD20_READY),
"gyro_set_full_scale(), invalid state");
#if L3GD20_USE_SPI
osalDbgAssert((devp->config->spip->state == SPI_READY),
"gyro_set_full_scale(), channel not ready");
#endif
if(fs == L3GD20_FS_250DPS) {
newfs = L3GD20_250DPS;
}
else if(fs == L3GD20_FS_500DPS) {
newfs = L3GD20_500DPS;
}
else if(fs == L3GD20_FS_2000DPS) {
newfs = L3GD20_2000DPS;
}
else {
return MSG_RESET;
}
if(newfs != devp->gyrofullscale) {
scale = newfs / devp->gyrofullscale;
devp->gyrofullscale = newfs;
#if L3GD20_USE_SPI
#if L3GD20_SHARED_SPI
spiAcquireBus(devp->config->spip);
spiStart(devp->config->spip,
devp->config->spicfg);
#endif /* L3GD20_SHARED_SPI */
/* Updating register.*/
l3gd20SPIReadRegister(devp->config->spip,
L3GD20_AD_CTRL_REG4, 1, &cr);
#if L3GD20_SHARED_SPI
spiReleaseBus(devp->config->spip);
#endif /* L3GD20_SHARED_SPI */
#endif /* L3GD20_USE_SPI */
cr &= ~(L3GD20_CTRL_REG4_FS_MASK);
cr |= fs;
#if L3GD20_USE_SPI
#if L3GD20_SHARED_SPI
spiAcquireBus(devp->config->spip);
spiStart(devp->config->spip,
devp->config->spicfg);
#endif /* L3GD20_SHARED_SPI */
l3gd20SPIWriteRegister(devp->config->spip,
L3GD20_AD_CTRL_REG4, 1, &cr);
#if L3GD20_SHARED_SPI
spiReleaseBus(devp->config->spip);
#endif /* L3GD20_SHARED_SPI */
#endif /* L3GD20_USE_SPI */
/* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
devp->gyrosensitivity[i] *= scale;
devp->gyrobias[i] *= scale;
}
}
return msg;
}
/// called every 10ms from clock.c - check all temp sensors that are ready for checking
void temp_sensor_tick() {
temp_sensor_t i = 0;
for (; i < NUM_TEMP_SENSORS; i++) {
if (temp_sensors_runtime[i].next_read_time) {
temp_sensors_runtime[i].next_read_time--;
}
else {
uint16_t temp = 0;
#ifdef TEMP_MAX31855
uint32_t value = 0;
struct max31855_plat_data *max31855;
#endif
//time to deal with this temp sensor
switch(temp_sensors[i].temp_type) {
#ifdef TEMP_MAX31855
case TT_MAX31855:
#ifdef __arm__
max31855 = (struct max31855_plat_data*)temp_sensors[i].plat_data;
spiAcquireBus(max31855->bus);
spiStart(max31855->bus, max31855->config);
spiSelect(max31855->bus);
spiReceive(max31855->bus, 4, &value);
spiUnselect(max31855->bus);
spiReleaseBus(max31855->bus);
value = SWAP_UINT32(value);
temp_sensors_runtime[i].temp_flags = 0;
if (value & (1 << 16)) {
#ifdef HALT_ON_TERMOCOUPLE_FAILURE
emergency_stop();
#endif
}
else {
temp = value >> 18;
if (temp & (1 << 13))
temp = 0;
}
#else
temp = 0;
#endif
break;
#endif
#ifdef TEMP_MAX6675
case TT_MAX6675:
#ifdef PRR
PRR &= ~MASK(PRSPI);
#elif defined PRR0
PRR0 &= ~MASK(PRSPI);
#endif
SPCR = MASK(MSTR) | MASK(SPE) | MASK(SPR0);
// enable TT_MAX6675
WRITE(SS, 0);
// No delay required, see
// https://github.com/triffid/Teacup_Firmware/issues/22
// read MSB
SPDR = 0;
for (;(SPSR & MASK(SPIF)) == 0;);
temp = SPDR;
temp <<= 8;
// read LSB
SPDR = 0;
for (;(SPSR & MASK(SPIF)) == 0;);
temp |= SPDR;
// disable TT_MAX6675
WRITE(SS, 1);
temp_sensors_runtime[i].temp_flags = 0;
if ((temp & 0x8002) == 0) {
// got "device id"
temp_sensors_runtime[i].temp_flags |= PRESENT;
if (temp & 4) {
// thermocouple open
temp_sensors_runtime[i].temp_flags |= TCOPEN;
}
else {
temp = temp >> 3;
}
}
// this number depends on how frequently temp_sensor_tick is called. the MAX6675 can give a reading every 0.22s, so set this to about 250ms
temp_sensors_runtime[i].next_read_time = 25;
break;
#endif /* TEMP_MAX6675 */
#ifdef TEMP_THERMISTOR
case TT_THERMISTOR:
do {
uint8_t j, table_num;
//Read current temperature
temp = analog_read(i);
// for thermistors the thermistor table number is in the additional field
table_num = temp_sensors[i].additional;
//Calculate real temperature based on lookup table
for (j = 1; j < NUMTEMPS; j++) {
if (pgm_read_word(&(temptable[table_num][j][0])) > temp) {
// Thermistor table is already in 14.2 fixed point
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR("pin:%d Raw ADC:%d table entry: %d"),temp_sensors[i].temp_pin,temp,j);
#endif
// Linear interpolating temperature value
// y = ((x - x₀)y₁ + (x₁-x)y₀ ) / (x₁ - x₀)
// y = temp
// x = ADC reading
// x₀= temptable[j-1][0]
// x₁= temptable[j][0]
// y₀= temptable[j-1][1]
// y₁= temptable[j][1]
// y =
// Wikipedia's example linear interpolation formula.
temp = (
// ((x - x₀)y₁
((uint32_t)temp - pgm_read_word(&(temptable[table_num][j-1][0]))) * pgm_read_word(&(temptable[table_num][j][1]))
// +
+
// (x₁-x)
(pgm_read_word(&(temptable[table_num][j][0])) - (uint32_t)temp)
// y₀ )
* pgm_read_word(&(temptable[table_num][j-1][1])))
// /
/
// (x₁ - x₀)
(pgm_read_word(&(temptable[table_num][j][0])) - pgm_read_word(&(temptable[table_num][j-1][0])));
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR(" temp:%d.%d"),temp/4,(temp%4)*25);
#endif
break;
}
}
#ifndef EXTRUDER
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR(" Sensor:%d\n"),i);
#endif
//Clamp for overflows
if (j == NUMTEMPS)
temp = temptable[table_num][NUMTEMPS-1][1];
temp_sensors_runtime[i].next_read_time = 0;
} while (0);
break;
#endif /* TEMP_THERMISTOR */
#ifdef TEMP_AD595
case TT_AD595:
temp = analog_read(i);
// convert
// >>8 instead of >>10 because internal temp is stored as 14.2 fixed point
temp = (temp * 500L) >> 8;
temp_sensors_runtime[i].next_read_time = 0;
break;
#endif /* TEMP_AD595 */
#ifdef TEMP_PT100
case TT_PT100:
#warning TODO: PT100 code
break
#endif /* TEMP_PT100 */
#ifdef TEMP_INTERCOM
case TT_INTERCOM:
temp = read_temperature(temp_sensors[i].temp_pin);
temp_sensors_runtime[i].next_read_time = 25;
break;
#endif /* TEMP_INTERCOM */
#ifdef TEMP_DUMMY
case TT_DUMMY:
temp = temp_sensors_runtime[i].last_read_temp;
if (temp_sensors_runtime[i].target_temp > temp)
temp++;
else if (temp_sensors_runtime[i].target_temp < temp)
temp--;
temp_sensors_runtime[i].next_read_time = 0;
break;
#endif /* TEMP_DUMMY */
default: /* prevent compiler warning */
break;
}
/* Exponentially Weighted Moving Average alpha constant for smoothing
noisy sensors. Instrument Engineer's Handbook, 4th ed, Vol 2 p126
says values of 0.05 to 0.1 for TEMP_EWMA are typical. */
#ifndef TEMP_EWMA
#define TEMP_EWMA 1.0
#endif
#define EWMA_SCALE 1024L
#define EWMA_ALPHA ((long) (TEMP_EWMA * EWMA_SCALE))
temp_sensors_runtime[i].last_read_temp = (uint16_t) ((EWMA_ALPHA * temp +
(EWMA_SCALE-EWMA_ALPHA) * temp_sensors_runtime[i].last_read_temp
) / EWMA_SCALE);
}
if (labs((int16_t)(temp_sensors_runtime[i].last_read_temp - temp_sensors_runtime[i].target_temp)) < (TEMP_HYSTERESIS*4)) {
if (temp_sensors_runtime[i].temp_residency < (TEMP_RESIDENCY_TIME*120))
temp_sensors_runtime[i].temp_residency++;
}
else {
// Deal with flakey sensors which occasionally report a wrong value
// by setting residency back, but not entirely to zero.
if (temp_sensors_runtime[i].temp_residency > 10)
temp_sensors_runtime[i].temp_residency -= 10;
else
temp_sensors_runtime[i].temp_residency = 0;
}
if (temp_sensors[i].heater < NUM_HEATERS) {
heater_tick(temp_sensors[i].heater, temp_sensors[i].temp_type, temp_sensors_runtime[i].last_read_temp, temp_sensors_runtime[i].target_temp);
}
if (DEBUG_PID && (debug_flags & DEBUG_PID))
sersendf_P(PSTR("DU temp: {%d %d %d.%d}"), i,
temp_sensors_runtime[i].last_read_temp,
temp_sensors_runtime[i].last_read_temp / 4,
(temp_sensors_runtime[i].last_read_temp & 0x03) * 25);
}
/**
* @brief Configures and activates L3GD20 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p L3GD20Driver object
* @param[in] config pointer to the @p L3GD20Config object
*
* @api
*/
void l3gd20Start(L3GD20Driver *devp, const L3GD20Config *config) {
uint32_t i;
uint8_t cr[5] = {0, 0, 0, 0, 0};
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == L3GD20_STOP) || (devp->state == L3GD20_READY),
"l3gd20Start(), invalid state");
devp->config = config;
/* Control register 1 configuration block.*/
{
cr[0] = L3GD20_CTRL_REG1_XEN | L3GD20_CTRL_REG1_YEN |
L3GD20_CTRL_REG1_ZEN | L3GD20_CTRL_REG1_PD |
devp->config->gyrooutputdatarate;
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
cr[0] |= devp->config->gyrobandwidth;
#endif
}
/* Control register 2 configuration block.*/
{
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
if(devp->config->gyrohpmode != L3GD20_HPM_BYPASSED)
cr[1] = devp->config->gyrohpmode | devp->config->gyrohpconfiguration;
#endif
}
/* Control register 4 configuration block.*/
{
cr[3] = devp->config->gyrofullscale;
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
cr[3] |= devp->config->gyroblockdataupdate |
devp->config->gyroendianness;
#endif
}
/* Control register 5 configuration block.*/
{
#if L3GD20_USE_ADVANCED || defined(__DOXYGEN__)
if((devp->config->gyrohpmode != L3GD20_HPM_BYPASSED)) {
cr[4] = L3GD20_CTRL_REG5_HPEN;
if(devp->config->gyrolp2mode != L3GD20_LP2M_BYPASSED) {
cr[4] |= L3GD20_CTRL_REG5_INT1_SEL1 |
L3GD20_CTRL_REG5_OUT_SEL1;
}
else {
cr[4] |= L3GD20_CTRL_REG5_INT1_SEL0 |
L3GD20_CTRL_REG5_OUT_SEL0;
}
}
#endif
}
#if L3GD20_USE_SPI
#if L3GD20_SHARED_SPI
spiAcquireBus(devp->config->spip);
#endif /* L3GD20_SHARED_SPI */
spiStart(devp->config->spip,
devp->config->spicfg);
l3gd20SPIWriteRegister(devp->config->spip, L3GD20_AD_CTRL_REG1,
5, cr);
#if L3GD20_SHARED_SPI
spiReleaseBus(devp->config->spip);
#endif /* L3GD20_SHARED_SPI */
#endif /* L3GD20_USE_SPI */
/* Storing sensitivity information according to full scale.*/
if(devp->config->gyrofullscale == L3GD20_FS_250DPS) {
devp->gyrofullscale = L3GD20_250DPS;
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
if (devp->config->gyrosensitivity == NULL)
devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_250DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
}
else if(devp->config->gyrofullscale == L3GD20_FS_500DPS) {
devp->gyrofullscale = L3GD20_500DPS;
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
if (devp->config->gyrosensitivity == NULL)
devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_500DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
}
else if(devp->config->gyrofullscale == L3GD20_FS_2000DPS) {
devp->gyrofullscale = L3GD20_2000DPS;
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
if (devp->config->gyrosensitivity == NULL)
devp->gyrosensitivity[i] = L3GD20_GYRO_SENS_2000DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
}
else
osalDbgAssert(FALSE, "l3gd20Start(), full scale issue");
/* Storing bias information.*/
if(devp->config->gyrobias != NULL) {
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
devp->gyrobias[i] = devp->config->gyrobias[i];
}
}
else {
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
devp->gyrobias[i] = L3GD20_GYRO_BIAS;
}
/* This is the Gyroscope transient recovery time.*/
osalThreadSleepMilliseconds(10);
devp->state = L3GD20_READY;
}
static void flash_bus_acquire(M25QDriver *devp) {
spiAcquireBus(devp->config->spip);
spiStart(devp->config->spip, devp->config->spicfg);
}
/**
* @brief Start a SPI transaction.
*/
static int start_spi(void)
{
spiAcquireBus(SPI_SX);
spiSelect(SPI_SX);
return 0;
}
void Rf24ChibiosIo::beginTransaction() {
#ifdef SPI_USE_MUTUAL_EXCLUSION
spiAcquireBus(driver);
#endif
// spiStart(driver, config);
}
