/*
* 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.
*/
halInit();
chSysInit();
// Enable special "compensation cell" for IO working on 100MHz.
// Looks like FSMC works slower when it enabled
// rccEnableAPB2(RCC_APB2ENR_SYSCFGEN, false);
// SYSCFG->CMPCR |= SYSCFG_CMPCR_CMP_PD;
// while (! SYSCFG->CMPCR & SYSCFG_CMPCR_READY)
// ;
osalThreadSleepMilliseconds(100);
fpgaObjectInit(&FPGAD1);
fpgaStart(&FPGAD1);
fpga_memtest(&FPGAD1, true, -1, FPGA_WB_SLICE_MEMTEST, 32768);
while (true) {
osalThreadSleepMilliseconds(100);
orange_led_toggle();
}
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
*/
halInit();
/*
* Enabling interrupts, initialization done.
*/
osalSysEnable();
/*
* Activates the serial driver 2 using the driver default configuration.
* PA2(TX) and PA3(RX) are routed to USART2.
*/
sdStart(&SD2, NULL);
palSetPadMode(GPIOA, 2, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOA, 3, PAL_MODE_ALTERNATE(7));
/*
* Normal main() thread activity, in this demo it just performs
* a shell respawn upon its termination.
*/
while (true) {
chnWriteTimeout(&SD2, (uint8_t *)"Hello World!\r\n", 14, TIME_INFINITE);
palSetPad(GPIOD, GPIOD_LED3); /* Orange. */
osalThreadSleepMilliseconds(500);
palClearPad(GPIOD, GPIOD_LED3); /* Orange. */
osalThreadSleepMilliseconds(500);
}
}
static THD_FUNCTION(PollEepromThread, arg) {
unsigned i;
uint8_t tx_data[5];
uint8_t rx_data[4];
msg_t status;
(void)arg;
chRegSetThreadName("PollEeprom");
/* set initial data to write */
tx_data[0] = EEPROM_START_ADDR;
tx_data[1] = 0xA0;
tx_data[2] = 0xA1;
tx_data[3] = 0xA2;
tx_data[4] = 0xA3;
while (true) {
/* write out initial data */
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, sizeof(tx_data), NULL, 0, TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
/* read back inital data */
osalThreadSleepMilliseconds(2);
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, 1, rx_data, sizeof(rx_data), TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
/* invert the data */
for (i = 1; i < sizeof(tx_data); i++)
tx_data[i] ^= 0xff;
/* write out inverted data */
osalThreadSleepMilliseconds(2);
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, sizeof(tx_data), NULL, 0, TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
/* read back inverted data */
osalThreadSleepMilliseconds(2);
i2cAcquireBus(&I2CD1);
status = i2cMasterTransmitTimeout(&I2CD1, I2C_ADDR, tx_data, 1, rx_data, sizeof(rx_data), TIME_INFINITE);
i2cReleaseBus(&I2CD1);
osalDbgCheck(MSG_OK == status);
osalThreadSleepMilliseconds(TIME_INFINITE);
}
}
void fpgaStart(FPGADriver *fpgap) {
fsmcSramInit();
fsmcSramStart(&SRAMD1, &sram_cfg);
while ( ! FPGAReady()) {
orange_led_on();
osalThreadSleepMilliseconds(30);
orange_led_off();
osalThreadSleepMilliseconds(70);
}
fpgap->memspace = (fpgaword_t *)FSMC_Bank1_1_MAP;
fpgap->state = FPGA_READY;
}
/*
* Entry point, note, the main() function is already a thread in the system
* on entry.
*/
int main(void) {
halInit();
chSysInit();
i2cStart(&I2CD1, &i2cfg);
/* Create EEPROM thread. */
chThdCreateStatic(PollEepromThreadWA,
sizeof(PollEepromThreadWA),
NORMALPRIO,
PollEepromThread,
NULL);
/* Create not responding thread. */
chThdCreateStatic(PollFakeThreadWA,
sizeof(PollFakeThreadWA),
NORMALPRIO,
PollFakeThread,
NULL);
/* main loop handles LED */
while (true) {
palTogglePad(IOPORT1, LED0); /* on */
osalThreadSleepMilliseconds(500);
}
return 0;
}
/*
* 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.
* - lwIP subsystem initialization using the default configuration.
*/
halInit();
chSysInit();
lwipInit(NULL);
/*
* Start the serial driver with the default configuration.
* Used for debug output of LwIP.
*/
sdStart(&SD1, NULL);
/*
* Creates the HTTP thread (it changes priority internally).
*/
chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1,
http_server, NULL);
while (1) {
osalThreadSleepMilliseconds(500);
}
return 0;
}
/**
* @brief Shell thread function.
*
* @param[in] p pointer to a @p BaseSequentialStream object
*/
THD_FUNCTION(shellThread, p) {
int n;
BaseSequentialStream *chp = ((ShellConfig *)p)->sc_channel;
const ShellCommand *scp = ((ShellConfig *)p)->sc_commands;
char *lp, *cmd, *tokp, line[SHELL_MAX_LINE_LENGTH];
char *args[SHELL_MAX_ARGUMENTS + 1];
chprintf(chp, "\r\nChibiOS/RT Shell\r\n");
while (true) {
chprintf(chp, "ch> ");
if (shellGetLine(chp, line, sizeof(line))) {
#if (SHELL_CMD_EXIT_ENABLED == TRUE) && !defined(_CHIBIOS_NIL_)
chprintf(chp, "\r\nlogout");
break;
#else
/* Putting a delay in order to avoid an endless loop trying to read
an unavailable stream.*/
osalThreadSleepMilliseconds(100);
#endif
}
lp = parse_arguments(line, &tokp);
cmd = lp;
n = 0;
while ((lp = parse_arguments(NULL, &tokp)) != NULL) {
if (n >= SHELL_MAX_ARGUMENTS) {
chprintf(chp, "too many arguments\r\n");
cmd = NULL;
break;
}
args[n++] = lp;
}
args[n] = NULL;
if (cmd != NULL) {
if (strcmp(cmd, "exit") == 0) {
if (n > 0) {
usage(chp, "exit");
continue;
}
break;
}
else if (strcmp(cmd, "help") == 0) {
if (n > 0) {
usage(chp, "help");
continue;
}
chprintf(chp, "Commands: help exit ");
list_commands(chp, shell_local_commands);
if (scp != NULL)
list_commands(chp, scp);
chprintf(chp, "\r\n");
}
else if (cmdexec(shell_local_commands, chp, cmd, n, args) &&
((scp == NULL) || cmdexec(scp, chp, cmd, n, args))) {
chprintf(chp, "%s", cmd);
chprintf(chp, " ?\r\n");
}
}
}
shellExit(MSG_OK);
}
void mem_error_cb(memtest_t *memp, testtype e, size_t address) {
(void)memp;
(void)e;
(void)address;
green_led_off();
red_led_on();
osalThreadSleepMilliseconds(10);
errors++;
osalSysHalt("Memory broken");
}
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;
}
static bool _msd_bot_reset(USBHMassStorageDriver *msdp) {
usbh_urbstatus_t res;
res = usbhControlRequest(msdp->dev,
USBH_REQTYPE_CLASSOUT(USBH_REQTYPE_RECIP_INTERFACE),
0xFF, 0, msdp->ifnum, 0, NULL);
if (res != USBH_URBSTATUS_OK) {
return FALSE;
}
osalThreadSleepMilliseconds(100);
return usbhEPReset(&msdp->epin) && usbhEPReset(&msdp->epout);
}
/**
* @brief Waits that the card reaches an idle state.
*
* @param[in] mmcp pointer to the @p MMCDriver object
*
* @notapi
*/
static void sync(MMCDriver *mmcp) {
uint8_t buf[1];
spiSelect(mmcp->config->spip);
while (true) {
spiReceive(mmcp->config->spip, 1, buf);
if (buf[0] == 0xFFU) {
break;
}
#if MMC_NICE_WAITING == TRUE
/* Trying to be nice with the other threads.*/
osalThreadSleepMilliseconds(1);
#endif
}
spiUnselect(mmcp->config->spip);
}
/**
* @brief Executes the SDRAM memory initialization sequence.
*
* @param[in] cfgp pointer to the @p SDRAMConfig object
*
* @notapi
*/
static void _sdram_init_sequence(const SDRAMConfig *cfgp) {
uint32_t command_target = 0;
#if STM32_SDRAM_USE_FSMC_SDRAM1
command_target |= FMC_SDCMR_CTB1;
#endif
#if STM32_SDRAM_USE_FSMC_SDRAM2
command_target |= FMC_SDCMR_CTB2;
#endif
/* Step 3: Configure a clock configuration enable command.*/
_sdram_wait_ready();
SDRAMD.sdram->SDCMR = FMCCM_CLK_ENABLED | command_target;
/* Step 4: Insert delay (tipically 100uS).*/
osalThreadSleepMilliseconds(1);
/* Step 5: Configure a PALL (precharge all) command.*/
_sdram_wait_ready();
SDRAMD.sdram->SDCMR = FMCCM_PALL | command_target;
/* Step 6.1: Configure a Auto-Refresh command: send the first command.*/
_sdram_wait_ready();
SDRAMD.sdram->SDCMR = FMCCM_AUTO_REFRESH | command_target |
(cfgp->sdcmr & FMC_SDCMR_NRFS);
/* Step 6.2: Send the second command.*/
_sdram_wait_ready();
SDRAMD.sdram->SDCMR = FMCCM_AUTO_REFRESH | command_target |
(cfgp->sdcmr & FMC_SDCMR_NRFS);
/* Step 7: Program the external memory mode register.*/
_sdram_wait_ready();
SDRAMD.sdram->SDCMR = FMCCM_LOAD_MODE | command_target |
(cfgp->sdcmr & FMC_SDCMR_MRD);
/* Step 8: Set clock.*/
_sdram_wait_ready();
SDRAMD.sdram->SDRTR = cfgp->sdrtr & FMC_SDRTR_COUNT;
_sdram_wait_ready();
}
/**
* @brief Waits an idle condition.
*
* @param[in] mmcp pointer to the @p MMCDriver object
*
* @notapi
*/
static void wait(MMCDriver *mmcp) {
int i;
uint8_t buf[4];
for (i = 0; i < 16; i++) {
spiReceive(mmcp->config->spip, 1, buf);
if (buf[0] == 0xFFU) {
return;
}
}
/* Looks like it is a long wait.*/
while (true) {
spiReceive(mmcp->config->spip, 1, buf);
if (buf[0] == 0xFFU) {
break;
}
#if MMC_NICE_WAITING == TRUE
/* Trying to be nice with the other threads.*/
osalThreadSleepMilliseconds(1);
#endif
}
}
THD_FUNCTION(TimeKeeper::TimekeeperThread, arg) {
chRegSetThreadName("Timekeeper");
TimeKeeper *self = static_cast<TimeKeeper *>(arg);
self->GNSS.subscribe(&gps);
while (!chThdShouldTerminateX()) {
if ((gps.fresh) && (gps.fix > 0) && (0 == gps.msec)) {
int64_t tmp = 1000000;
tmp *= mktime(&gps.time);
osalSysLock();
time_gps_us = tmp;
if (! time_verified) {
time_verified = true;
unix_usec = time_gps_us;
}
osalSysUnlock();
/* now correct time in internal RTC (if needed) */
int32_t t1 = time_gps_us / 1000000;
int32_t t2 = rtc_get_time_unix(nullptr);
int32_t dt = t1 - t2;
if (abs(dt) > TIME_CORRECTION_THRESHOLD)
rtc_set_time(&gps.time);
}
if (gps.fresh) {
gps.fresh = false;
}
osalThreadSleepMilliseconds(20);
}
self->GNSS.unsubscribe(&gps);
chThdExit(MSG_OK);
}
/*
* 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.
*/
halInit();
chSysInit();
palSetPadMode(GPIOB, GPIOB_PIN0, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, GPIOB_PIN1, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, GPIOB_PIN4, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
palSetPadMode(GPIOB, GPIOB_PIN5, PAL_MODE_ALTERNATE(2) | PAL_STM32_PUDR_FLOATING);
eicuStart(&EICUD3, &eicucfg);
eicuEnable(&EICUD3);
osalThreadSleepMicroseconds(10); // need to stabilize input puns
chThdCreateStatic(PulseThreadWA_LED3, sizeof(PulseThreadWA_LED3),
NORMALPRIO+1, PulseThread, &pulse_led3);
chThdCreateStatic(PulseThreadWA_LED4, sizeof(PulseThreadWA_LED4),
NORMALPRIO+1, PulseThread, &pulse_led4);
chThdCreateStatic(PulseThreadWA_LED5, sizeof(PulseThreadWA_LED5),
NORMALPRIO+1, PulseThread, &pulse_led5);
chThdCreateStatic(PulseThreadWA_LED6, sizeof(PulseThreadWA_LED6),
NORMALPRIO+1, PulseThread, &pulse_led6);
while (true) {
osalThreadSleepMilliseconds(500);
}
return 0;
}
sensor_state_t fram::get(void){
msg_t status = MSG_RESET;
int cmp_status = -1;
osalDbgCheck(state == SENSOR_STATE_READY);
/* first write pattern */
memset(txbuf, pattern, sizeof(txbuf));
memcpy(txbuf, &address, sizeof(address));
status = transmit(txbuf, sizeof(txbuf), nullptr, 0);
osalDbgCheck(MSG_OK == status);
osalThreadSleepMilliseconds(10);
/* read back and compare */
status = transmit(txbuf, 2, rxbuf, sizeof(rxbuf));
osalDbgCheck(MSG_OK == status);
cmp_status = memcmp(rxbuf, &txbuf[2], sizeof(rxbuf));
osalDbgCheck(0 == cmp_status);
/* change pattern */
pattern++;
return state;
}
static THD_FUNCTION(PollFakeThread, arg) {
(void)arg;
chRegSetThreadName("PollFake");
while (true) {
msg_t status;
uint8_t rx_data[2];
i2cflags_t errors;
i2cAcquireBus(&I2CD1);
status = i2cMasterReceiveTimeout(&I2CD1, I2C_FAKE_ADDR, rx_data, 2, MS2ST(4));
i2cReleaseBus(&I2CD1);
if (status == MSG_RESET){
errors = i2cGetErrors(&I2CD1);
osalDbgCheck(I2C_ACK_FAILURE == errors);
}
palTogglePad(IOPORT1, LED1); /* on */
osalThreadSleepMilliseconds(1000);
}
}
/**
* @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 Test execution thread function.
*
* @param[in] stream pointer to a @p BaseSequentialStream object for test
* output
* @return A failure boolean value casted to @p msg_t.
* @retval FALSE if no errors occurred.
* @retval TRUE if one or more tests failed.
*
* @api
*/
msg_t test_execute(BaseSequentialStream *stream) {
int i, j;
test_chp = stream;
test_println("");
#if defined(TEST_SUITE_NAME)
test_println("*** " TEST_SUITE_NAME);
#else
test_println("*** ChibiOS test suite");
#endif
test_println("***");
test_print("*** Compiled: ");
test_println(__DATE__ " - " __TIME__);
#ifdef PLATFORM_NAME
test_print("*** Platform: ");
test_println(PLATFORM_NAME);
#endif
#ifdef BOARD_NAME
test_print("*** Test Board: ");
test_println(BOARD_NAME);
#endif
test_println("");
test_global_fail = FALSE;
i = 0;
while (test_suite[i]) {
j = 0;
while (test_suite[i][j]) {
print_line();
test_print("--- Test Case ");
test_printn(i + 1);
test_print(".");
test_printn(j + 1);
test_print(" (");
test_print(test_suite[i][j]->name);
test_println(")");
#if TEST_DELAY_BETWEEN_TESTS > 0
osalThreadSleepMilliseconds(TEST_DELAY_BETWEEN_TESTS);
#endif
execute_test(test_suite[i][j]);
if (test_local_fail) {
test_print("--- Result: FAILURE (#");
test_printn(test_step);
test_print(" [");
print_tokens();
test_print("] \"");
test_print(test_failure_message);
test_println("\")");
}
else
test_println("--- Result: SUCCESS");
j++;
}
i++;
}
print_line();
test_println("");
test_print("Final result: ");
if (test_global_fail)
test_println("FAILURE");
else
test_println("SUCCESS");
return (msg_t)test_global_fail;
}
void onewireTest(void) {
int16_t tmp;
uint8_t rombuf[24];
size_t devices_on_bus = 0;
size_t i = 0;
bool presence;
onewireObjectInit(&OWD1);
onewireStart(&OWD1, &ow_cfg);
#if ONEWIRE_SYNTH_SEARCH_TEST
synthSearchRomTest(&OWD1);
#endif
for (i=0; i<3; i++)
temperature[i] = -666;
while (true) {
if (true == onewireReset(&OWD1)){
memset(rombuf, 0x55, sizeof(rombuf));
search_led_on();
devices_on_bus = onewireSearchRom(&OWD1, rombuf, 3);
search_led_off();
osalDbgCheck(devices_on_bus <= 3);
osalDbgCheck(devices_on_bus > 0);
if (1 == devices_on_bus){
/* test read rom command */
presence = onewireReset(&OWD1);
osalDbgCheck(true == presence);
testbuf[0] = ONEWIRE_CMD_READ_ROM;
onewireWrite(&OWD1, testbuf, 1, 0);
onewireRead(&OWD1, testbuf, 8);
osalDbgCheck(testbuf[7] == onewireCRC(testbuf, 7));
osalDbgCheck(0 == memcmp(rombuf, testbuf, 8));
}
/* start temperature measurement on all connected devices at once */
presence = onewireReset(&OWD1);
osalDbgCheck(true == presence);
testbuf[0] = ONEWIRE_CMD_SKIP_ROM;
testbuf[1] = ONEWIRE_CMD_CONVERT_TEMP;
#if ONEWIRE_USE_STRONG_PULLUP
onewireWrite(&OWD1, testbuf, 2, MS2ST(750));
#else
onewireWrite(&OWD1, testbuf, 2, 0);
/* poll bus waiting ready signal from all connected devices */
testbuf[0] = 0;
while (testbuf[0] == 0){
osalThreadSleepMilliseconds(50);
onewireRead(&OWD1, testbuf, 1);
}
#endif
for (i=0; i<devices_on_bus; i++) {
/* read temperature device by device from their scratchpads */
presence = onewireReset(&OWD1);
osalDbgCheck(true == presence);
testbuf[0] = ONEWIRE_CMD_MATCH_ROM;
memcpy(&testbuf[1], &rombuf[i*8], 8);
testbuf[9] = ONEWIRE_CMD_READ_SCRATCHPAD;
onewireWrite(&OWD1, testbuf, 10, 0);
onewireRead(&OWD1, testbuf, 9);
osalDbgCheck(testbuf[8] == onewireCRC(testbuf, 8));
memcpy(&tmp, &testbuf, 2);
temperature[i] = ((int32_t)tmp * 625) / 10;
}
}
else {
osalSysHalt("No devices found");
}
osalThreadSleep(1); /* enforce ChibiOS's stack overflow check */
}
onewireStop(&OWD1);
}
/**
* @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;
}
void time_keeper_delay_ms(uint64_t milliseconds)
{
osalThreadSleepMilliseconds(milliseconds);
}
/**
* @brief Shell thread function.
*
* @param[in] p pointer to a @p BaseSequentialStream object
*/
THD_FUNCTION(shellThread, p) {
int n;
ShellConfig *scfg = p;
BaseSequentialStream *chp = scfg->sc_channel;
const ShellCommand *scp = scfg->sc_commands;
char *lp, *cmd, *tokp, line[SHELL_MAX_LINE_LENGTH];
char *args[SHELL_MAX_ARGUMENTS + 1];
#if SHELL_USE_HISTORY == TRUE
*(scfg->sc_histbuf) = 0;
ShellHistory hist = {
scfg->sc_histbuf,
scfg->sc_histsize,
0,
0,
0
};
ShellHistory *shp = &hist;
#else
ShellHistory *shp = NULL;
#endif
chprintf(chp, SHELL_NEWLINE_STR);
chprintf(chp, "ChibiOS/RT Shell"SHELL_NEWLINE_STR);
while (true) {
chprintf(chp, SHELL_PROMPT_STR);
if (shellGetLine(scfg, line, sizeof(line), shp)) {
#if (SHELL_CMD_EXIT_ENABLED == TRUE) && !defined(_CHIBIOS_NIL_)
chprintf(chp, SHELL_NEWLINE_STR);
chprintf(chp, "logout");
break;
#else
/* Putting a delay in order to avoid an endless loop trying to read
an unavailable stream.*/
osalThreadSleepMilliseconds(100);
#endif
}
lp = parse_arguments(line, &tokp);
cmd = lp;
n = 0;
while ((lp = parse_arguments(NULL, &tokp)) != NULL) {
if (n >= SHELL_MAX_ARGUMENTS) {
chprintf(chp, "too many arguments"SHELL_NEWLINE_STR);
cmd = NULL;
break;
}
args[n++] = lp;
}
args[n] = NULL;
if (cmd != NULL) {
if (strcmp(cmd, "help") == 0) {
if (n > 0) {
shellUsage(chp, "help");
continue;
}
chprintf(chp, "Commands: help ");
list_commands(chp, shell_local_commands);
if (scp != NULL)
list_commands(chp, scp);
chprintf(chp, SHELL_NEWLINE_STR);
}
else if (cmdexec(shell_local_commands, chp, cmd, n, args) &&
((scp == NULL) || cmdexec(scp, chp, cmd, n, args))) {
chprintf(chp, "%s", cmd);
chprintf(chp, " ?"SHELL_NEWLINE_STR);
}
}
}
shellExit(MSG_OK);
}
/**
* @brief Configures and activates LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
* @param[in] config pointer to the @p LSM6DS0Config object
*
* @api
*/
void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
uint32_t i;
uint8_t cr[5];
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == LSM6DS0_STOP) ||
(devp->state == LSM6DS0_READY),
"lsm6ds0Start(), invalid state");
devp->config = config;
/* Configuring common registers.*/
/* Control register 8 configuration block.*/
{
cr[0] = LSM6DS0_AD_CTRL_REG8;
cr[1] = LSM6DS0_CTRL_REG8_IF_ADD_INC;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->endianness | devp->config->blockdataupdate;
#endif
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
i2cStart(devp->config->i2cp, devp->config->i2ccfg);
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
/* Configuring Accelerometer subsystem.*/
/* Multiple write starting address.*/
cr[0] = LSM6DS0_AD_CTRL_REG5_XL;
/* Control register 5 configuration block.*/
{
cr[1] = LSM6DS0_CTRL_REG5_XL_XEN_XL | LSM6DS0_CTRL_REG5_XL_YEN_XL |
LSM6DS0_CTRL_REG5_XL_ZEN_XL;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->accdecmode;
#endif
}
/* Control register 6 configuration block.*/
{
cr[2] = devp->config->accoutdatarate |
devp->config->accfullscale;
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(devp->config->i2cp);
i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 2);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
/* Storing sensitivity according to user settings */
if(devp->config->accfullscale == LSM6DS0_ACC_FS_2G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
else
devp->accsensitivity[i] = devp->config->accsensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_2G;
}
else if(devp->config->accfullscale == LSM6DS0_ACC_FS_4G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
else
devp->accsensitivity[i] = devp->config->accsensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_4G;
}
else if(devp->config->accfullscale == LSM6DS0_ACC_FS_8G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
else
devp->accsensitivity[i] = devp->config->accsensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_8G;
}
else if(devp->config->accfullscale == LSM6DS0_ACC_FS_16G) {
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
else
devp->accsensitivity[i] = devp->config->accsensitivity[i];
}
devp->accfullscale = LSM6DS0_ACC_16G;
}
else
osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
/* Storing bias information */
if(devp->config->accbias != NULL)
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
devp->accbias[i] = devp->config->accbias[i];
else
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
devp->accbias[i] = LSM6DS0_ACC_BIAS;
/* Configuring Gyroscope subsystem.*/
/* Multiple write starting address.*/
cr[0] = LSM6DS0_AD_CTRL_REG1_G;
/* Control register 1 configuration block.*/
{
cr[1] = devp->config->gyrofullscale |
devp->config->gyrooutdatarate;
}
/* Control register 2 configuration block.*/
{
cr[2] = 0;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
cr[2] |= devp->config->gyrooutsel;
#endif
}
/* Control register 3 configuration block.*/
{
cr[3] = 0;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
cr[3] |= devp->config->gyrohpfenable |
devp->config->gyrolowmodecfg |
devp->config->gyrohpcfg;
#endif
}
/* Control register 4 configuration block.*/
{
cr[4] = LSM6DS0_CTRL_REG4_XEN_G | LSM6DS0_CTRL_REG4_YEN_G |
LSM6DS0_CTRL_REG4_ZEN_G;
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(devp->config->i2cp);
i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 4);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
cr[0] = LSM6DS0_AD_CTRL_REG9;
/* Control register 9 configuration block.*/
{
cr[1] = 0;
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(devp->config->i2cp);
i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_245DPS) {
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
if(devp->config->gyrosensitivity == NULL)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
devp->gyrofullscale = LSM6DS0_GYRO_245DPS;
}
else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_500DPS) {
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
if(devp->config->gyrosensitivity == NULL)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
devp->gyrofullscale = LSM6DS0_GYRO_500DPS;
}
else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_2000DPS) {
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
if(devp->config->gyrosensitivity == NULL)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
else
devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
devp->gyrofullscale = LSM6DS0_GYRO_2000DPS;
}
else
osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
/* Storing bias information */
if(devp->config->gyrobias != NULL)
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrobias[i] = devp->config->gyrobias[i];
else
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrobias[i] = LSM6DS0_GYRO_BIAS;
/* This is the MEMS transient recovery time */
osalThreadSleepMilliseconds(5);
devp->state = LSM6DS0_READY;
}
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;
}
/**
* @brief Configures and activates LPS25H Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LPS25HDriver object
* @param[in] config pointer to the @p LPS25HConfig object
*
* @api
*/
void lps25hStart(LPS25HDriver *devp, const LPS25HConfig *config) {
uint8_t cr[2];
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == LPS25H_STOP) || (devp->state == LPS25H_READY),
"lps25hStart(), invalid state");
devp->config = config;
#if LPS25H_USE_I2C
/* Control register 1 configuration block.*/
{
cr[0] = LPS25H_AD_CTRL_REG1;
cr[1] = devp->config->outputdatarate | LPS25H_CTRL_REG1_PD;
#if LPS25H_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->blockdataupdate;
#endif
}
#if LPS25H_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
#endif /* LPS25H_SHARED_I2C */
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
lps25hI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress, cr, 1);
#if LPS25H_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LPS25H_SHARED_I2C */
/* Resolution configuration block.*/
{
cr[0] = LPS25H_AD_RES_CONF;
cr[1] = 0x05;
#if LPS25H_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] = devp->config->respressure | devp->config->restemperature;
#endif
}
#if LPS25H_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
#endif /* LPS25H_SHARED_I2C */
lps25hI2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
#if LPS25H_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LPS25H_SHARED_I2C */
#endif /* LPS25H_USE_I2C */
if(devp->config->sensitivity == NULL) {
devp->sensitivity = LPS25H_SENS;
}
else{
/* Taking Sensitivity from user configurations */
devp->sensitivity = *devp->config->sensitivity;
}
if(devp->config->bias == NULL) {
devp->bias = 0.0f;
}
else{
/* Taking Bias from user configurations */
devp->bias = *devp->config->bias;
}
/* This is the Barometer transient recovery time */
osalThreadSleepMilliseconds(5);
devp->state = LPS25H_READY;
}
/**
* @brief Performs the initialization procedure on the inserted card.
* @details This function should be invoked when a card is inserted and
* brings the driver in the @p MMC_READY state where it is possible
* to perform read and write operations.
* @note It is possible to invoke this function from the insertion event
* handler.
*
* @param[in] mmcp pointer to the @p MMCDriver object
*
* @return The operation status.
* @retval HAL_SUCCESS the operation succeeded and the driver is now
* in the @p MMC_READY state.
* @retval HAL_FAILED the operation failed.
*
* @api
*/
bool mmcConnect(MMCDriver *mmcp) {
unsigned i;
uint8_t r3[4];
osalDbgCheck(mmcp != NULL);
osalDbgAssert((mmcp->state == BLK_ACTIVE) || (mmcp->state == BLK_READY),
"invalid state");
/* Connection procedure in progress.*/
mmcp->state = BLK_CONNECTING;
mmcp->block_addresses = false;
/* Slow clock mode and 128 clock pulses.*/
spiStart(mmcp->config->spip, mmcp->config->lscfg);
spiIgnore(mmcp->config->spip, 16);
/* SPI mode selection.*/
i = 0;
while (true) {
if (send_command_R1(mmcp, MMCSD_CMD_GO_IDLE_STATE, 0) == 0x01U) {
break;
}
if (++i >= MMC_CMD0_RETRY) {
goto failed;
}
osalThreadSleepMilliseconds(10);
}
/* Try to detect if this is a high capacity card and switch to block
addresses if possible.
This method is based on "How to support SDC Ver2 and high capacity cards"
by ElmChan.*/
if (send_command_R3(mmcp, MMCSD_CMD_SEND_IF_COND,
MMCSD_CMD8_PATTERN, r3) != 0x05U) {
/* Switch to SDHC mode.*/
i = 0;
while (true) {
/*lint -save -e9007 [13.5] Side effect unimportant.*/
if ((send_command_R1(mmcp, MMCSD_CMD_APP_CMD, 0) == 0x01U) &&
(send_command_R3(mmcp, MMCSD_CMD_APP_OP_COND, 0x400001AAU, r3) == 0x00U)) {
/*lint -restore*/
break;
}
if (++i >= MMC_ACMD41_RETRY) {
goto failed;
}
osalThreadSleepMilliseconds(10);
}
/* Execute dedicated read on OCR register */
(void) send_command_R3(mmcp, MMCSD_CMD_READ_OCR, 0, r3);
/* Check if CCS is set in response. Card operates in block mode if set.*/
if ((r3[0] & 0x40U) != 0U) {
mmcp->block_addresses = true;
}
}
/* Initialization.*/
i = 0;
while (true) {
uint8_t b = send_command_R1(mmcp, MMCSD_CMD_INIT, 0);
if (b == 0x00U) {
break;
}
if (b != 0x01U) {
goto failed;
}
if (++i >= MMC_CMD1_RETRY) {
goto failed;
}
osalThreadSleepMilliseconds(10);
}
/* Initialization complete, full speed.*/
spiStart(mmcp->config->spip, mmcp->config->hscfg);
/* Setting block size.*/
if (send_command_R1(mmcp, MMCSD_CMD_SET_BLOCKLEN,
MMCSD_BLOCK_SIZE) != 0x00U) {
goto failed;
}
/* Determine capacity.*/
if (read_CxD(mmcp, MMCSD_CMD_SEND_CSD, mmcp->csd)) {
goto failed;
}
mmcp->capacity = _mmcsd_get_capacity(mmcp->csd);
if (mmcp->capacity == 0U) {
goto failed;
}
if (read_CxD(mmcp, MMCSD_CMD_SEND_CID, mmcp->cid)) {
goto failed;
}
mmcp->state = BLK_READY;
return HAL_SUCCESS;
/* Connection failed, state reset to BLK_ACTIVE.*/
failed:
spiStop(mmcp->config->spip);
mmcp->state = BLK_ACTIVE;
return HAL_FAILED;
}
bool usbhmsdLUNConnect(USBHMassStorageLUNDriver *lunp) {
osalDbgCheck(lunp != NULL);
osalDbgCheck(lunp->msdp != NULL);
msd_result_t res;
chSemWait(&lunp->sem);
osalDbgAssert((lunp->state == BLK_READY) || (lunp->state == BLK_ACTIVE), "invalid state");
if (lunp->state == BLK_READY) {
chSemSignal(&lunp->sem);
return HAL_SUCCESS;
}
lunp->state = BLK_CONNECTING;
{
USBH_DEFINE_BUFFER(scsi_inquiry_response_t inq);
uinfo("INQUIRY...");
res = scsi_inquiry(lunp, &inq);
if (res == MSD_RESULT_DISCONNECTED) {
goto failed;
} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
//retry?
goto failed;
} else if (res == MSD_RESULT_FAILED) {
//retry?
goto failed;
}
uinfof("\tPDT=%02x", inq.peripheral & 0x1f);
if (inq.peripheral != 0) {
uerr("\tUnsupported PDT");
goto failed;
}
}
// Test if unit ready
uint8_t i;
for (i = 0; i < 10; i++) {
uinfo("TEST UNIT READY...");
res = scsi_testunitready(lunp);
if (res == MSD_RESULT_DISCONNECTED) {
goto failed;
} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
//retry?
goto failed;
} else if (res == MSD_RESULT_FAILED) {
uinfo("\tTEST UNIT READY: Command Failed, retry");
osalThreadSleepMilliseconds(200);
continue;
}
uinfo("\tReady.");
break;
}
if (i == 10) goto failed;
{
USBH_DEFINE_BUFFER(scsi_readcapacity10_response_t cap);
// Read capacity
uinfo("READ CAPACITY(10)...");
res = scsi_readcapacity10(lunp, &cap);
if (res == MSD_RESULT_DISCONNECTED) {
goto failed;
} else if (res == MSD_RESULT_TRANSPORT_ERROR) {
//retry?
goto failed;
} else if (res == MSD_RESULT_FAILED) {
//retry?
goto failed;
}
lunp->info.blk_size = __REV(cap.block_size);
lunp->info.blk_num = __REV(cap.last_block_addr) + 1;
}
uinfof("\tBlock size=%dbytes, blocks=%u (~%u MB)", lunp->info.blk_size, lunp->info.blk_num,
(uint32_t)(((uint64_t)lunp->info.blk_size * lunp->info.blk_num) / (1024UL * 1024UL)));
uinfo("MSD Connected.");
lunp->state = BLK_READY;
chSemSignal(&lunp->sem);
return HAL_SUCCESS;
/* Connection failed, state reset to BLK_ACTIVE.*/
failed:
uinfo("MSD Connect failed.");
lunp->state = BLK_ACTIVE;
chSemSignal(&lunp->sem);
return HAL_FAILED;
}
/**
* @brief Test execution thread function.
*
* @param[in] stream pointer to a @p BaseSequentialStream object for test
* output
* @param[in] tsp test suite to execute
* @return A failure boolean value casted to @p msg_t.
* @retval false if no errors occurred.
* @retval true if one or more tests failed.
*
* @api
*/
msg_t test_execute(BaseSequentialStream *stream, const testsuite_t *tsp) {
int tseq, tcase;
test_chp = stream;
test_println("");
if (tsp->name != NULL) {
test_print("*** ");
test_println(tsp->name);
}
else {
test_println("*** Test Suite");
}
test_println("***");
test_print("*** Compiled: ");
test_println(__DATE__ " - " __TIME__);
#if defined(PLATFORM_NAME)
test_print("*** Platform: ");
test_println(PLATFORM_NAME);
#endif
#if defined(BOARD_NAME)
test_print("*** Test Board: ");
test_println(BOARD_NAME);
#endif
#if defined(TEST_SIZE_REPORT)
{
extern uint8_t __text_base, __text_end,
_data_start, _data_end,
_bss_start, _bss_end;
test_println("***");
test_print("*** Text size: ");
test_printn((uint32_t)(&__text_end - &__text_base));
test_println(" bytes");
test_print("*** Data size: ");
test_printn((uint32_t)(&_data_end - &_data_start));
test_println(" bytes");
test_print("*** BSS size: ");
test_printn((uint32_t)(&_bss_end - &_bss_start));
test_println(" bytes");
}
#endif
#if defined(TEST_REPORT_HOOK_HEADER)
TEST_REPORT_HOOK_HEADER
#endif
test_println("");
test_global_fail = false;
tseq = 0;
while (tsp->sequences[tseq] != NULL) {
#if TEST_SHOW_SEQUENCES == TRUE
print_fat_line();
test_print("=== Test Sequence ");
test_printn(tseq + 1);
test_print(" (");
test_print(tsp->sequences[tseq]->name);
test_println(")");
#endif
tcase = 0;
while (tsp->sequences[tseq]->cases[tcase] != NULL) {
print_line();
test_print("--- Test Case ");
test_printn(tseq + 1);
test_print(".");
test_printn(tcase + 1);
test_print(" (");
test_print(tsp->sequences[tseq]->cases[tcase]->name);
test_println(")");
#if TEST_DELAY_BETWEEN_TESTS > 0
osalThreadSleepMilliseconds(TEST_DELAY_BETWEEN_TESTS);
#endif
execute_test(tsp->sequences[tseq]->cases[tcase]);
if (test_local_fail) {
test_print("--- Result: FAILURE (#");
test_printn(test_step);
test_print(" [");
print_tokens();
test_print("] \"");
test_print(test_failure_message);
test_println("\")");
}
else {
test_println("--- Result: SUCCESS");
}
tcase++;
}
tseq++;
}
print_line();
test_println("");
test_print("Final result: ");
if (test_global_fail)
test_println("FAILURE");
else
test_println("SUCCESS");
#if defined(TEST_REPORT_HOOK_END)
TEST_REPORT_HOOK_END
#endif
return (msg_t)test_global_fail;
}
/**
* @brief Configures and activates LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
* @param[in] config pointer to the @p LSM6DS0Config object
*
* @api
*/
void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
uint32_t i;
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Start(), invalid state");
devp->config = config;
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
if((devp)->config->acccfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG5_XL,
devp->config->acccfg->decmode |
devp->config->acccfg->axesenabling);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG6_XL,
devp->config->acccfg->outdatarate |
devp->config->acccfg->fullscale );
}
if((devp)->config->gyrocfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG1_G,
devp->config->gyrocfg->fullscale |
devp->config->gyrocfg->outdatarate);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG2_G,
devp->config->gyrocfg->outsel);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG3_G,
devp->config->gyrocfg->hpfenable |
devp->config->gyrocfg->lowmodecfg |
devp->config->gyrocfg->hpcfg);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG4,
devp->config->gyrocfg->axesenabling);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG9,
LSM6DS0_GYRO_SLP_DISABLED);
}
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG8,
devp->config->endianness |
devp->config->blockdataupdate);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
/* Storing sensitivity information according to full scale value */
if((devp)->config->acccfg != NULL) {
if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G)
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
else
osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
}
if((devp)->config->gyrocfg != NULL) {
if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
else
osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
}
/* This is the Gyroscope transient recovery time */
osalThreadSleepMilliseconds(5);
devp->state = LSM6DS0_READY;
}
