int
adc_init(void)
{
adc_perf = perf_alloc(PC_ELAPSED, "adc");
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL;
up_udelay(1);
if (rCR2 & ADC_CR2_RSTCAL)
return -1;
rCR2 |= ADC_CR2_CAL;
up_udelay(100);
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/* arbitrarily configure all channels for 55 cycle sample time */
rSMPR1 = 0b00000011011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
/* XXX for F2/4, might want to select 12-bit mode? */
rCR1 = 0;
/* enable the temperature sensor / Vrefint channel if supported*/
rCR2 =
#ifdef ADC_CR2_TSVREFE
/* enable the temperature sensor in CR2 */
ADC_CR2_TSVREFE |
#endif
0;
#ifdef ADC_CCR_TSVREFE
/* enable temperature sensor in CCR */
rCCR = ADC_CCR_TSVREFE;
#endif
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel each tick */
/* power-cycle the ADC and turn it on */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
return 0;
}
int BMI160::reset()
{
write_reg(BMIREG_CONF, (1 << 1)); //Enable NVM programming
write_checked_reg(BMIREG_ACC_CONF, BMI_ACCEL_US | BMI_ACCEL_BWP_NORMAL); //Normal operation, no decimation
write_checked_reg(BMIREG_ACC_RANGE, 0);
write_checked_reg(BMIREG_GYR_CONF, BMI_GYRO_BWP_NORMAL); //Normal operation, no decimation
write_checked_reg(BMIREG_GYR_RANGE, 0);
write_checked_reg(BMIREG_INT_EN_1, BMI_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMIREG_INT_OUT_CTRL, BMI_INT1_EN); //Enable interrupts on pin INT1
write_checked_reg(BMIREG_INT_MAP_1, BMI_DRDY_INT1); //DRDY interrupt on pin INT1
write_checked_reg(BMIREG_IF_CONF, BMI_SPI_4_WIRE |
BMI_AUTO_DIS_SEC); //Disable secondary interface; Work in SPI 4-wire mode
write_checked_reg(BMIREG_NV_CONF, BMI_SPI); //Disable I2C interface
set_accel_range(BMI160_ACCEL_DEFAULT_RANGE_G);
accel_set_sample_rate(BMI160_ACCEL_DEFAULT_RATE);
set_gyro_range(BMI160_GYRO_DEFAULT_RANGE_DPS);
gyro_set_sample_rate(BMI160_GYRO_DEFAULT_RATE);
//_set_dlpf_filter(BMI160_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ); //NOT CONSIDERING FILTERING YET
//Enable Accelerometer in normal mode
write_reg(BMIREG_CMD, BMI_ACCEL_NORMAL_MODE);
up_udelay(4100);
//usleep(4100);
//Enable Gyroscope in normal mode
write_reg(BMIREG_CMD, BMI_GYRO_NORMAL_MODE);
up_udelay(80300);
//usleep(80300);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
_accel_reads = 0;
_gyro_reads = 0;
return OK;
}
/**
* @brief Exchange one bit in mode 0
*
* The function simulates SPI one bit transaction function in SPI mode 0.
*
* @param outdata data for SPI output
* @param holdtime SPI hold time timing
* @param loopback lookback mode supported or not
* @return input data that read from SPI input pin
*/
static uint8_t tsb_spi_bitexchange0(uint32_t outdata, uint32_t holdtime, bool loopback)
{
uint8_t indata = 0;
gpio_set_value(SPI_SDO, (outdata)? 1 : 0);
up_udelay(holdtime);
gpio_set_value(SPI_SCK, 1);
indata = gpio_get_value((loopback)? SPI_SDO : SPI_SDI);
up_udelay(holdtime);
gpio_set_value(SPI_SCK, 0);
return indata;
}
int
adc_init(void)
{
adc_perf = perf_alloc(PC_ELAPSED, "adc");
/* put the ADC into power-down mode */
rCR2 &= ~ADC_CR2_ADON;
up_udelay(10);
/* bring the ADC out of power-down mode */
rCR2 |= ADC_CR2_ADON;
up_udelay(10);
/* do calibration if supported */
#ifdef ADC_CR2_CAL
rCR2 |= ADC_CR2_RSTCAL;
up_udelay(1);
if (rCR2 & ADC_CR2_RSTCAL)
return -1;
rCR2 |= ADC_CR2_CAL;
up_udelay(100);
if (rCR2 & ADC_CR2_CAL)
return -1;
#endif
/*
* Configure sampling time.
*
* For electrical protection reasons, we want to be able to have
* 10K in series with ADC inputs that leave the board. At 12MHz this
* means we need 28.5 cycles of sampling time (per table 43 in the
* datasheet).
*/
rSMPR1 = 0b00000000011011011011011011011011;
rSMPR2 = 0b00011011011011011011011011011011;
rCR2 |= ADC_CR2_TSVREFE; /* enable the temperature sensor / Vrefint channel */
/* configure for a single-channel sequence */
rSQR1 = 0;
rSQR2 = 0;
rSQR3 = 0; /* will be updated with the channel at conversion time */
return 0;
}
/**
* @brief Manage the regulator state; Turn it on when needed
* @returns: 0 on success, <0 on error
*/
int vreg_get(struct vreg *vreg) {
unsigned int i, rc = 0;
if (!vreg) {
return -ENODEV;
}
dbg_verbose("%s %s\n", __func__, vreg->name ? vreg->name : "unknown");
/* Enable the regulator on the first use; Update use count */
if (atomic_inc(&vreg->use_count) == 1) {
for (i = 0; i < vreg->nr_vregs; i++) {
if (!&vreg->vregs[i]) {
rc = -EINVAL;
break;
}
dbg_insane("%s: %s vreg, gpio %d to %d, hold %dus\n", __func__,
vreg->name ? vreg->name : "unknown",
vreg->vregs[i].gpio, !!vreg->vregs[i].active_high,
vreg->vregs[i].hold_time);
gpio_set_value(vreg->vregs[i].gpio, vreg->vregs[i].active_high);
up_udelay(vreg->vregs[i].hold_time);
}
}
/* Update state */
vreg->power_state = true;
return rc;
}
__EXPORT int nsh_archinitialize(void)
{
/* the interruption subsystem is not initialized when stm32_boardinitialize() is called */
stm32_gpiosetevent(GPIO_FORCE_BOOTLOADER, true, false, false, _bootloader_force_pin_callback);
/* configure power supply control/sense pins */
stm32_configgpio(GPIO_VDD_5V_SENSORS_EN);
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure the DMA allocator */
dma_alloc_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
led_off(LED_BLUE);
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
return OK;
}
/**
* @brief Given a table of interfaces, power off all associated
* power supplies
* @param interfaces table of interfaces to initialize
* @param nr_ints number of interfaces to initialize
* @param nr_spring_ints number of spring interfaces
* @returns: 0 on success, <0 on error
*/
int interface_early_init(struct interface **ints,
size_t nr_ints, size_t nr_spring_ints) {
unsigned int i;
int rc;
int fail = 0;
dbg_info("Power off all interfaces\n");
if (!ints) {
return -ENODEV;
}
interfaces = ints;
nr_interfaces = nr_ints;
nr_spring_interfaces = nr_spring_ints;
for (i = 0; i < nr_interfaces; i++) {
rc = interface_config(interfaces[i]);
if (rc < 0) {
dbg_error("Failed to power interface %s\n", interfaces[i]->name);
fail = 1;
/* Continue configuring remaining interfaces */
continue;
}
}
if (fail) {
return -1;
}
/* Let everything settle for a good long while.*/
up_udelay(POWER_OFF_TIME_IN_US);
return 0;
}
static uint16_t ads7843e_sendcmd(FAR struct ads7843e_dev_s *priv, uint8_t cmd)
{
uint8_t buffer[2];
uint16_t result;
/* Select the ADS7843E */
SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, true);
/* Send the command */
(void)SPI_SEND(priv->spi, cmd);
/* Wait a tiny amount to make sure that the aquisition time is complete */
up_udelay(3); /* 3 microseconds */
/* Read the 12-bit data (LS 4 bits will be padded with zero) */
SPI_RECVBLOCK(priv->spi, buffer, 2);
SPI_SELECT(priv->spi, SPIDEV_TOUCHSCREEN, false);
result = ((uint16_t)buffer[0] << 8) | (uint16_t)buffer[1];
result = result >> 4;
ivdbg("cmd:%02x response:%04x\n", cmd, result);
return result;
}
void sam_sckc_enable(bool enable)
{
uint32_t regval;
#ifdef ATSAMA5D3
/* REVISIT: Missing the logic that disables the external OSC32 */
/* Enable external OSC 32 kHz */
regval = getreg32(SAM_SCKC_CR);
regval |= SCKC_CR_OSC32EN;
putreg32(regval, SAM_SCKC_CR);
/* Wait for 32,768 XTAL start-up time */
up_udelay(5 * USEC_PER_SEC / BOARD_SLOWCLK_FREQUENCY);
/* Disable OSC 32 kHz bypass */
regval &= ~SCKC_CR_OSC32BYP;
putreg32(regval, SAM_SCKC_CR);
/* Switch slow clock source to external OSC 32 kHz (*/
regval |= SCKC_CR_OSCSEL;
putreg32(regval, SAM_SCKC_CR);
/* Wait 5 slow clock cycles for internal resynchronization */
up_udelay(5 * USEC_PER_SEC / BOARD_SLOWCLK_FREQUENCY);
/* Disable internal RC 32 kHz */
regval &= ~SCKC_CR_RCEN;
putreg32(regval, SAM_SCKC_CR);
#else
/* Switch slow clock source to external OSC 32 kHz */
regval = enable ? SCKC_CR_OSCSEL : 0;
putreg32(regval, SAM_SCKC_CR);
/* Wait 5 slow clock cycles for internal resynchronization */
up_udelay(5 * USEC_PER_SEC / BOARD_SLOWCLK_FREQUENCY);
#endif
}
int
bma180_attach(struct spi_dev_s *spi, int spi_id)
{
int result = ERROR;
bma180_dev.spi = spi;
bma180_dev.spi_id = spi_id;
SPI_LOCK(bma180_dev.spi, true);
/* verify that the device is attached and functioning */
if (bma180_read_reg(ADDR_CHIP_ID) == CHIP_ID) {
bma180_write_reg(ADDR_RESET, SOFT_RESET); // page 48
up_udelay(13000); // wait 12 ms, see page 49
/* Configuring the BMA180 */
/* enable writing to chip config */
uint8_t ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 |= REG0_WRITE_ENABLE;
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
/* disable I2C interface, datasheet page 31 */
uint8_t disi2c = bma180_read_reg(ADDR_DIS_I2C);
disi2c |= 0x01;
bma180_write_reg(ADDR_DIS_I2C, disi2c);
/* block writing to chip config */
ctrl0 = bma180_read_reg(ADDR_CTRL_REG0);
ctrl0 &= (~REG0_WRITE_ENABLE);
bma180_write_reg(ADDR_CTRL_REG0, ctrl0);
// up_udelay(500);
/* set rate */
result = bma180_set_rate(BMA180_RATE_LP_600HZ);
// up_udelay(500);
/* set range */
result += bma180_set_range(BMA180_RANGE_4G);
// up_udelay(500);
if (result == 0) {
/* make ourselves available */
register_driver("/dev/bma180", &bma180_fops, 0666, NULL);
}
} else {
errno = EIO;
}
SPI_LOCK(bma180_dev.spi, false);
return result;
}
static int determin_hw_version(int *version, int *revision)
{
*revision = 0; /* default revision */
int rv = 0;
int pos = 0;
stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB4 & ~GPIO_PUPD_MASK));
up_udelay(10);
rv |= stm32_gpioread(HW_VER_PB4) << pos++;
stm32_configgpio(HW_VER_PB4);
up_udelay(10);
rv |= stm32_gpioread(HW_VER_PB4) << pos++;
int votes = 16;
int ones[2] = {0, 0};
int zeros[2] = {0, 0};
while (votes--) {
stm32_configgpio(GPIO_PULLDOWN | (HW_VER_PB12 & ~GPIO_PUPD_MASK));
up_udelay(10);
stm32_gpioread(HW_VER_PB12) ? ones[0]++ : zeros[0]++;
stm32_configgpio(HW_VER_PB12);
up_udelay(10);
stm32_gpioread(HW_VER_PB12) ? ones[1]++ : zeros[1]++;
}
if (ones[0] > zeros[0]) {
rv |= 1 << pos;
}
pos++;
if (ones[1] > zeros[1]) {
rv |= 1 << pos;
}
stm32_configgpio(HW_VER_PB4_INIT);
stm32_configgpio(HW_VER_PB12_INIT);
*version = rv;
return OK;
}
/* (init as in... make it accessible for the app - the driver
* itself is initialized in nsh_archinitialize() in up_nsh.c)
*/
void helper_init_adc(void)
{
/* open the ADC file */
adc_fd = open( ORCACTRL_ADC_FILE, O_RDONLY | O_NONBLOCK );
if( adc_fd < 0 ) /* fd < 0 means error opening the file */
{ /* no other error handling here at the moment */
printf( "failed opening " ORCACTRL_ADC_FILE "\n" );
}
/* wait a while to make sure it is started properly */
up_udelay(10000); /* numerical value taken from ADC test app,
* most propably empirical */
}
static void spi_sndblock(FAR struct spi_dev_s *dev, FAR const void *buffer, size_t nwords)
{
FAR const uint8_t *ptr = (FAR const uint8_t *)buffer;
uint8_t sr;
/* Loop while thre are bytes remaining to be sent */
spidbg("nwords: %d\n", nwords);
while (nwords > 0)
{
/* While the TX FIFO is not full and there are bytes left to send */
while ((getreg8(LPC214X_SPI1_SR) & LPC214X_SPI1SR_TNF) && nwords)
{
/* Send the data */
putreg16((uint16_t)*ptr, LPC214X_SPI1_DR);
ptr++;
nwords--;
}
}
/* Then discard all card responses until the RX & TX FIFOs are emptied. */
spidbg("discarding\n");
do
{
/* Is there anything in the RX fifo? */
sr = getreg8(LPC214X_SPI1_SR);
if ((sr & LPC214X_SPI1SR_RNE) != 0)
{
/* Yes.. Read and discard */
(void)getreg16(LPC214X_SPI1_DR);
}
/* There is a race condition where TFE may go true just before
* RNE goes true and this loop terminates prematurely. The nasty little
* delay in the following solves that (it could probably be tuned
* to improve performance).
*/
else if ((sr & LPC214X_SPI1SR_TFE) != 0)
{
up_udelay(100);
sr = getreg8(LPC214X_SPI1_SR);
}
}
while ((sr & LPC214X_SPI1SR_RNE) != 0 || (sr & LPC214X_SPI1SR_TFE) == 0);
}
FAR struct lcd_dev_s *up_nxdrvinit(unsigned int devno)
{
FAR struct spi_dev_s *spi;
FAR struct lcd_dev_s *dev;
/* Configure the LCD GPIOs */
lcd_dumpgpio("up_nxdrvinit: On entry");
lpc17_configgpio(LPC1766STK_LCD_RST);
lpc17_configgpio(LPC1766STK_LCD_BL);
lcd_dumpgpio("up_nxdrvinit: After GPIO setup");
/* Reset the LCD */
lpc17_gpiowrite(LPC1766STK_LCD_RST, false);
up_udelay(10);
lpc17_gpiowrite(LPC1766STK_LCD_RST, true);
up_mdelay(5);
/* Configure PWM1 to support the backlight */
nokia_blinitialize();
/* Get the SSP port (configure as a Freescale SPI port) */
spi = up_spiinitialize(0);
if (!spi)
{
glldbg("Failed to initialize SSP port 0\n");
}
else
{
/* Bind the SSP port to the LCD */
dev = nokia_lcdinitialize(spi, devno);
if (!dev)
{
glldbg("Failed to bind SSP port 0 to LCD %d: %d\n", devno);
}
else
{
gllvdbg("Bound SSP port 0 to LCD %d\n", devno);
/* And turn the LCD on (CONFIG_LCD_MAXPOWER should be 1) */
(void)dev->setpower(dev, CONFIG_LCD_MAXPOWER);
return dev;
}
}
return NULL;
}
int BMI055_accel::reset()
{
write_reg(BMI055_ACC_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset
up_udelay(5000);
write_checked_reg(BMI055_ACC_BW, BMI055_ACCEL_BW_1000); //Write accel bandwidth
write_checked_reg(BMI055_ACC_RANGE, BMI055_ACCEL_RANGE_2_G);//Write range
write_checked_reg(BMI055_ACC_INT_EN_1, BMI055_ACC_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMI055_ACC_INT_MAP_1, BMI055_ACC_DRDY_INT1); //Map DRDY interrupt on pin INT1
set_accel_range(BMI055_ACCEL_DEFAULT_RANGE_G);//set accel range
accel_set_sample_rate(BMI055_ACCEL_DEFAULT_RATE);//set accel ODR
//Enable Accelerometer in normal mode
write_reg(BMI055_ACC_PMU_LPW, BMI055_ACCEL_NORMAL);
up_udelay(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
_accel_reads = 0;
return OK;
}
/* EVT1 */
static int evt1_board_init(struct ara_board_info *board_info) {
int rc;
/* For now, just always enable REFCLK_MAIN and the buffers. */
rc = vreg_config(&refclk_main_vreg) || vreg_get(&refclk_main_vreg);
if (rc) {
dbg_error("%s: can't start REFCLK_MAIN: %d\n", __func__, rc);
return ERROR;
}
/* Configure the switch power supply lines. */
rc = vreg_config(&sw_vreg);
if (rc) {
dbg_error("%s: can't configure switch regulators: %d\n", __func__, rc);
return ERROR;
}
stm32_configgpio(evt1_board_info.sw_data.gpio_reset);
up_udelay(POWER_SWITCH_OFF_STAB_TIME_US);
/* Configure the wake/detect lines. */
stm32_configgpio(WD_1_DET_IN_GPIO);
stm32_configgpio(WD_2_DET_IN_GPIO);
stm32_configgpio(WD_3A_DET_IN_GPIO);
stm32_configgpio(WD_3B_DET_IN_GPIO);
stm32_configgpio(WD_4A_DET_IN_GPIO);
stm32_configgpio(WD_4B_DET_IN_GPIO);
stm32_configgpio(WD_5_DET_IN_GPIO);
stm32_configgpio(WD_8A_DET_IN_GPIO);
stm32_configgpio(WD_8B_DET_IN_GPIO);
/* Configure the module release pins */
stm32_configgpio(MOD_RELEASE_1_CONFIG);
stm32_configgpio(MOD_RELEASE_2_CONFIG);
stm32_configgpio(MOD_RELEASE_3A_CONFIG);
stm32_configgpio(MOD_RELEASE_3B_CONFIG);
stm32_configgpio(MOD_RELEASE_4A_CONFIG);
stm32_configgpio(MOD_RELEASE_4B_CONFIG);
stm32_configgpio(MOD_RELEASE_5_CONFIG);
/* Configure ARA key input pin */
stm32_configgpio(ARA_KEY_CONFIG);
/*
* (Module hotplug pins unconfigured. TODO, part of SW-1942.)
*/
return 0;
}
int
SPI::_transferword(uint16_t *send, uint16_t *recv, unsigned len)
{
SPI_SETFREQUENCY(_dev, _frequency);
SPI_SETMODE(_dev, _mode);
SPI_SETBITS(_dev, 16); /* 16 bit transfer */
SPI_SELECT(_dev, _device, true);
/* do the transfer */
//SPI_EXCHANGE(_dev, send, recv, len); /* Try to be compatible with the t_stall of the ADIS16448 which is 9usec (by applying 5usec delay ) */
SPI_EXCHANGE(_dev, send, nullptr, 1);
up_udelay(5); /* Reduced to 5 usec (from 10 use) */
SPI_EXCHANGE(_dev, nullptr, recv+1, len-1);
////
/* and clean up */
SPI_SELECT(_dev, _device, false);
return OK;
}
int BMI055_gyro::reset()
{
write_reg(BMI055_GYR_SOFTRESET, BMI055_SOFT_RESET);//Soft-reset
usleep(5000);
write_checked_reg(BMI055_GYR_BW, 0); // Write Gyro Bandwidth
write_checked_reg(BMI055_GYR_RANGE, 0);// Write Gyro range
write_checked_reg(BMI055_GYR_INT_EN_0, BMI055_GYR_DRDY_INT_EN); //Enable DRDY interrupt
write_checked_reg(BMI055_GYR_INT_MAP_1, BMI055_GYR_DRDY_INT1); //Map DRDY interrupt on pin INT1
set_gyro_range(BMI055_GYRO_DEFAULT_RANGE_DPS);// set Gyro range
gyro_set_sample_rate(BMI055_GYRO_DEFAULT_RATE);// set Gyro ODR
//Enable Gyroscope in normal mode
write_reg(BMI055_GYR_LPM1, BMI055_GYRO_NORMAL);
up_udelay(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
_gyro_reads = 0;
return OK;
}
void stm32_pwr_enablebkp(bool writable)
{
uint16_t regval;
irqstate_t flags;
flags = irqsave();
/* Enable or disable the ability to write*/
regval = stm32_pwr_getreg(STM32_PWR_CR_OFFSET);
regval &= ~PWR_CR_DBP;
regval |= writable ? PWR_CR_DBP : 0;
stm32_pwr_putreg(STM32_PWR_CR_OFFSET, regval);
irqrestore(flags);
if (writable)
{
/* Enable does not happen right away */
up_udelay(4);
}
}
__EXPORT int board_app_initialize(uintptr_t arg)
{
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
/* run C++ ctors before we go any further */
up_cxxinitialize();
# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)
# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
# endif
#else
# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
#endif
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure the DMA allocator */
if (board_dma_alloc_init() < 0) {
message("DMA alloc FAILED");
}
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
#if defined(CONFIG_STM32_BBSRAM)
/* NB. the use of the console requires the hrt running
* to poll the DMA
*/
/* Using Battery Backed Up SRAM */
int filesizes[CONFIG_STM32_BBSRAM_FILES + 1] = BSRAM_FILE_SIZES;
stm32_bbsraminitialize(BBSRAM_PATH, filesizes);
#if defined(CONFIG_STM32_SAVE_CRASHDUMP)
/* Panic Logging in Battery Backed Up Files */
/*
* In an ideal world, if a fault happens in flight the
* system save it to BBSRAM will then reboot. Upon
* rebooting, the system will log the fault to disk, recover
* the flight state and continue to fly. But if there is
* a fault on the bench or in the air that prohibit the recovery
* or committing the log to disk, the things are too broken to
* fly. So the question is:
*
* Did we have a hard fault and not make it far enough
* through the boot sequence to commit the fault data to
* the SD card?
*/
/* Do we have an uncommitted hard fault in BBSRAM?
* - this will be reset after a successful commit to SD
*/
int hadCrash = hardfault_check_status("boot");
if (hadCrash == OK) {
message("[boot] There is a hard fault logged. Hold down the SPACE BAR," \
" while booting to halt the system!\n");
/* Yes. So add one to the boot count - this will be reset after a successful
* commit to SD
*/
int reboots = hardfault_increment_reboot("boot", false);
/* Also end the misery for a user that holds for a key down on the console */
int bytesWaiting;
ioctl(fileno(stdin), FIONREAD, (unsigned long)((uintptr_t) &bytesWaiting));
if (reboots > 2 || bytesWaiting != 0) {
/* Since we can not commit the fault dump to disk. Display it
* to the console.
*/
hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false);
message("[boot] There were %d reboots with Hard fault that were not committed to disk - System halted %s\n",
reboots,
(bytesWaiting == 0 ? "" : " Due to Key Press\n"));
/* For those of you with a debugger set a break point on up_assert and
* then set dbgContinue = 1 and go.
*/
/* Clear any key press that got us here */
static volatile bool dbgContinue = false;
int c = '>';
while (!dbgContinue) {
switch (c) {
case EOF:
case '\n':
case '\r':
case ' ':
continue;
default:
putchar(c);
putchar('\n');
switch (c) {
case 'D':
case 'd':
hardfault_write("boot", fileno(stdout), HARDFAULT_DISPLAY_FORMAT, false);
break;
case 'C':
case 'c':
hardfault_rearm("boot");
hardfault_increment_reboot("boot", true);
break;
case 'B':
case 'b':
dbgContinue = true;
break;
default:
break;
} // Inner Switch
message("\nEnter B - Continue booting\n" \
"Enter C - Clear the fault log\n" \
"Enter D - Dump fault log\n\n?>");
fflush(stdout);
if (!dbgContinue) {
c = getchar();
}
break;
} // outer switch
} // for
} // inner if
} // outer if
#endif // CONFIG_STM32_SAVE_CRASHDUMP
#endif // CONFIG_STM32_BBSRAM
/* initial LED state */
drv_led_start();
led_off(LED_RED);
led_off(LED_GREEN);
led_off(LED_BLUE);
/* Configure SPI-based devices */
spi1 = stm32_spibus_initialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\n");
board_autoled_on(LED_RED);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_HMC, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
/* Get the SPI port for the FRAM */
spi2 = stm32_spibus_initialize(2);
if (!spi2) {
message("[boot] FAILED to initialize SPI port 2\n");
board_autoled_on(LED_RED);
return -ENODEV;
}
/* Default SPI2 to 12MHz and de-assert the known chip selects.
* MS5611 has max SPI clock speed of 20MHz
*/
// XXX start with 10.4 MHz and go up to 20 once validated
SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
SPI_SELECT(spi2, PX4_SPIDEV_BARO, false);
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
message("[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
/* Now bind the SDIO interface to the MMC/SD driver */
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
if (ret != OK) {
message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
sdio_mediachange(sdio, true);
#endif
return OK;
}
__EXPORT int nsh_archinitialize(void)
{
int result;
message("\n");
/* configure always-on ADC pins */
stm32_configgpio(GPIO_ADC1_IN0);
stm32_configgpio(GPIO_ADC1_IN10);
stm32_configgpio(GPIO_ADC1_IN11);
stm32_configgpio(GPIO_UART_SBUS_INVERTER);
#ifdef CONFIG_RC_INPUTS_TYPE(RC_INPUT_SBUS)
stm32_gpiowrite(GPIO_UART_SBUS_INVERTER, 1);
#else
stm32_gpiowrite(GPIO_UART_SBUS_INVERTER, 0);
#endif
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* initial BUZZER state */
drv_buzzer_start();
buzzer_off(BUZZER_EXT);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
led_off(LED_BLUE);
led_off(LED_GREEN);
led_off(LED_EXT1);
led_off(LED_EXT2);
/* Configure SPI-based devices */
message("[boot] Initializing SPI port 1\n");
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\r\n");
led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, GPIO_SPI_CS_MS5611, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_MS5611, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_MPU6000, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_HMC5983, false);
SPI_SELECT(spi1, GPIO_SPI_CS_EXP_WIFI_EEPROM, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
// message("[boot] Initializing Wireless Module\n");
// wireless_archinitialize();
message("[boot] Initializing SPI port 2\n");
spi2 = up_spiinitialize(2);
if (!spi2) {
message("[boot] FAILED to initialize SPI port 2\r\n");
led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI2 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi2, 10000000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, GPIO_SPI_CS_MPU6000, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_MS5611, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_MPU6000, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_HMC5983, false);
SPI_SELECT(spi2, GPIO_SPI_CS_IMU_EEPROM, false);
message("[boot] Successfully initialized SPI port 2\n");
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI3 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi3, 10000000);
SPI_SETBITS(spi3, 8);
SPI_SETMODE(spi3, SPIDEV_MODE3);
SPI_SELECT(spi3, GPIO_SPI_CS_DATAFLASH, false);
SPI_SELECT(spi3, GPIO_SPI_CS_EEPROM, false);
SPI_SELECT(spi3, GPIO_SPI_CS_SDCARD, false);
message("[boot] Successfully initialized SPI port 3\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK) {
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
led_on(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
return OK;
}
int
user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* configure the high-resolution time/callout interface */
hrt_init();
/* calculate our fw CRC so FMU can decide if we need to update */
calculate_fw_crc();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
#ifdef CONFIG_ARCH_DMA
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
/* default all the LEDs to off while we start */
LED_AMBER(false);
LED_BLUE(false);
LED_SAFETY(false);
#ifdef GPIO_LED4
LED_RING(false);
#endif
/* turn on servo power (if supported) */
#ifdef POWER_SERVO
POWER_SERVO(true);
#endif
/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
ENABLE_SBUS_OUT(false);
#endif
/* start the safety switch handler */
safety_init();
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* initialise the control inputs */
controls_init();
/* set up the ADC */
adc_init();
/* start the FMU interface */
interface_init();
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
/* initialize PWM limit lib */
pwm_limit_init(&pwm_limit);
/*
* P O L I C E L I G H T S
*
* Not enough memory, lock down.
*
* We might need to allocate mixers later, and this will
* ensure that a developer doing a change will notice
* that he just burned the remaining RAM with static
* allocations. We don't want him to be able to
* get past that point. This needs to be clearly
* documented in the dev guide.
*
*/
if (minfo.mxordblk < 600) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
while (true) {
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
up_udelay(250000);
phase = !phase;
}
}
/* Start the failsafe led init */
failsafe_led_init();
/*
* Run everything in a tight loop.
*/
uint64_t last_debug_time = 0;
uint64_t last_heartbeat_time = 0;
for (;;) {
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) {
last_heartbeat_time = hrt_absolute_time();
heartbeat_blink();
}
ring_blink();
check_reboot();
/* check for debug activity (default: none) */
show_debug_messages();
/* post debug state at ~1Hz - this is via an auxiliary serial port
* DEFAULTS TO OFF!
*/
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(unsigned)r_status_flags,
(unsigned)r_setup_arming,
(unsigned)r_setup_features,
(unsigned)mallinfo().mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}
int MPU9250::reset()
{
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
up_udelay(10000);
write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
up_udelay(1000);
write_checked_reg(MPUREG_PWR_MGMT_2, 0);
up_udelay(1000);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
usleep(1000);
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
set_accel_range(16);
usleep(1000);
// INT CFG => Interrupt on Data Ready
write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready
usleep(1000);
#ifdef USE_I2C
bool bypass = !_mag->is_passthrough();
#else
bool bypass = false;
#endif
write_checked_reg(MPUREG_INT_PIN_CFG,
BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN :
0)); // INT: Clear on any read, also use i2c bypass is master mode isn't needed
usleep(1000);
write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ);
usleep(1000);
uint8_t retries = 10;
while (retries--) {
bool all_ok = true;
for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) {
if (read_reg(_checked_registers[i]) != _checked_values[i]) {
write_reg(_checked_registers[i], _checked_values[i]);
all_ok = false;
}
}
if (all_ok) {
break;
}
}
return OK;
}
int up_lcd1602_initialize(void)
{
uint32_t regval;
int ret = OK;
/* Only initialize the driver once. */
if (!g_lcd1602.initialized)
{
lcdvdbg("Initializing\n");
/* PMP Master mode configuration */
/* Make sure that interrupts are disabled */
putreg32(INT_PMP, PIC32MX_INT_IEC1CLR);
/* Stop and reset the PMP module and clear the mode and control registers. */
putreg32(0, PIC32MX_PMP_MODE);
putreg32(0, PIC32MX_PMP_AEN);
putreg32(0, PIC32MX_PMP_CON);
putreg32(0, PIC32MX_PMP_ADDR);
/* Set LCD timing values, PMP master mode 3, 8-bit mode, no address
* increment, and no interrupts.
*/
regval = (PMP_MODE_WAITE_RD(0) | PMP_MODE_WAITM(3) | PMP_MODE_WAITB_1TPB |
PMP_MODE_MODE_MODE1 | PMP_MODE_MODE8 | PMP_MODE_INCM_NONE |
PMP_MODE_IRQM_NONE);
putreg32(regval, PIC32MX_PMP_MODE);
/* Enable the PMP for reading and writing
* PMRD/PMWR is active high (1=RD; 0=WR)
* PMENB is active high.
* No chip selects
* Address latch is active high
* Enable PMRD/PMWR, PMENB, and the PMP.
*/
regval = (PMP_CON_RDSP | PMP_CON_WRSP | PMP_CON_ALP |
PMP_CON_CSF_ADDR1415 | PMP_CON_PTRDEN | PMP_CON_PTWREN |
PMP_CON_ADRMUX_NONE | PMP_CON_ON);
putreg32(regval, PIC32MX_PMP_CON);
/* Configure and enable the LCD */
/* Wait > 15 milliseconds afer Vdd > 4.5V */
up_mdelay(100);
/* Select the 8-bit interface. BF cannot be checked before this command.
* This needs to be done a few times with some magic delays.
*/
lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);
up_mdelay(50);
lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);
up_udelay(50);
lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);
lcd_wrcommand(HD4478OU_FUNC | HD4478OU_FUNC_DL8D | HD4478OU_FUNC_N1);
/* Configure the display */
lcd_wrcommand(HD4478OU_DISPLAY); /* Display, cursor, and blink off */
lcd_wrcommand(HD4478OU_CLEAR); /* Clear the display */
lcd_wrcommand(HD4478OU_INPUT | HD4478OU_INPUT_INCR); /* Increment mode */
lcd_wrcommand(HD4478OU_DISPLAY | HD4478OU_DISPLAY_ON); /* Display on, cursor and blink off */
lcd_wrcommand(HD4478OU_DDRAM_AD(0)); /* Select DDRAM RAM AD=0 */
/* Register the LCD device driver */
ret = register_driver("/dev/lcd1602", &g_lcdops, 0644, &g_lcd1602);
g_lcd1602.initialized = true;
}
return ret;
}
__EXPORT int nsh_archinitialize(void)
{
int result;
message("\n");
/* configure always-on ADC pins */
stm32_configgpio(GPIO_ADC1_IN1);
stm32_configgpio(GPIO_ADC1_IN2);
stm32_configgpio(GPIO_ADC1_IN3);
stm32_configgpio(GPIO_ADC1_IN10);
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure the DMA allocator */
dma_alloc_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
/* initial BUZZER state */
drv_buzzer_start();
buzzer_off(BUZZER_EXT);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
led_off(LED_BLUE);
led_off(LED_GREEN);
led_off(LED_EXT1);
led_off(LED_EXT2);
/* Configure SPI-based devices */
message("[boot] Initializing SPI port 1\n");
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\r\n");
led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, SPIDEV_WIRELESS, false);
SPI_SELECT(spi1, SPIDEV_MS5611, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
message("[boot] Initializing SPI port 2\n");
spi2 = up_spiinitialize(2);
if (!spi2) {
message("[boot] FAILED to initialize SPI port 2\r\n");
led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI2 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi2, 10000000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_MPU6000, false);
message("[boot] Successfully initialized SPI port 2\n");
/* Get the SPI port for the microSD slot */
message("[boot] Initializing SPI port 3\n");
spi3 = up_spiinitialize(3);
if (!spi3) {
message("[boot] FAILED to initialize SPI port 3\n");
led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI3 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi3, 10000000);
SPI_SETBITS(spi3, 8);
SPI_SETMODE(spi3, SPIDEV_MODE3);
SPI_SELECT(spi3, SPIDEV_MMCSD, false);
SPI_SELECT(spi3, SPIDEV_FLASH, false);
message("[boot] Successfully initialized SPI port 3\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK) {
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\n");
led_on(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
return OK;
}
int nsh_archinitialize(void)
{
int result;
/* INIT 1 Lowest level NuttX initialization has been done at this point, LEDs and UARTs are configured */
/* INIT 2 Configuring PX4 low-level peripherals, these will be always needed */
/* configure the high-resolution time/callout interface */
#ifdef CONFIG_HRT_TIMER
hrt_init();
#endif
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
#ifdef SERIAL_HAVE_DMA
{
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
}
#endif
message("\r\n");
up_ledoff(LED_BLUE);
up_ledoff(LED_AMBER);
up_ledon(LED_BLUE);
/* Configure user-space led driver */
px4fmu_led_init();
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1)
{
message("[boot] FAILED to initialize SPI port 1\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
// Setup 10 MHz clock (maximum rate the BMA180 can sustain)
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
message("[boot] Successfully initialized SPI port 1\r\n");
/* initialize SPI peripherals redundantly */
int gyro_attempts = 0;
int gyro_fail = 0;
while (gyro_attempts < 5)
{
gyro_fail = l3gd20_attach(spi1, PX4_SPIDEV_GYRO);
gyro_attempts++;
if (gyro_fail == 0) break;
up_udelay(1000);
}
if (gyro_fail) message("[boot] FAILED to attach L3GD20 gyro\r\n");
int acc_attempts = 0;
int acc_fail = 0;
while (acc_attempts < 5)
{
acc_fail = bma180_attach(spi1, PX4_SPIDEV_ACCEL);
acc_attempts++;
if (acc_fail == 0) break;
up_udelay(1000);
}
if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n");
int mpu_attempts = 0;
int mpu_fail = 0;
while (mpu_attempts < 1)
{
mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU);
mpu_attempts++;
if (mpu_fail == 0) break;
up_udelay(200);
}
if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n");
/* initialize I2C2 bus */
i2c2 = up_i2cinitialize(2);
if (!i2c2) {
message("[boot] FAILED to initialize I2C bus 2\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C2 speed */
I2C_SETFREQUENCY(i2c2, 400000);
i2c3 = up_i2cinitialize(3);
if (!i2c3) {
message("[boot] FAILED to initialize I2C bus 3\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C3 speed */
I2C_SETFREQUENCY(i2c3, 400000);
int mag_attempts = 0;
int mag_fail = 0;
while (mag_attempts < 5)
{
mag_fail = hmc5883l_attach(i2c2);
mag_attempts++;
if (mag_fail == 0) break;
up_udelay(1000);
}
if (mag_fail) message("[boot] FAILED to attach HMC5883L magnetometer\r\n");
int baro_attempts = 0;
int baro_fail = 0;
while (baro_attempts < 5)
{
baro_fail = ms5611_attach(i2c2);
baro_attempts++;
if (baro_fail == 0) break;
up_udelay(1000);
}
if (baro_fail) message("[boot] FAILED to attach MS5611 baro at addr #1 or #2 (0x76 or 0x77)\r\n");
/* try to attach, don't fail if device is not responding */
(void)eeprom_attach(i2c3, FMU_BASEBOARD_EEPROM_ADDRESS,
FMU_BASEBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_BASEBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/baseboard_eeprom", 1);
int eeprom_attempts = 0;
int eeprom_fail;
while (eeprom_attempts < 5)
{
/* try to attach, fail if device does not respond */
eeprom_fail = eeprom_attach(i2c2, FMU_ONBOARD_EEPROM_ADDRESS,
FMU_ONBOARD_EEPROM_TOTAL_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_SIZE_BYTES,
FMU_ONBOARD_EEPROM_PAGE_WRITE_TIME_US, "/dev/eeprom", 1);
eeprom_attempts++;
if (eeprom_fail == OK) break;
up_udelay(1000);
}
if (eeprom_fail) message("[boot] FAILED to attach FMU EEPROM\r\n");
/* Report back sensor status */
if (acc_fail || gyro_fail || mag_fail || baro_fail || eeprom_fail)
{
up_ledon(LED_AMBER);
}
#if defined(CONFIG_STM32_SPI3)
/* Get the SPI port */
message("[boot] Initializing SPI port 3\r\n");
spi3 = up_spiinitialize(3);
if (!spi3)
{
message("[boot] FAILED to initialize SPI port 3\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully initialized SPI port 3\r\n");
/* Now bind the SPI interface to the MMCSD driver */
result = mmcsd_spislotinitialize(CONFIG_NSH_MMCSDMINOR, CONFIG_NSH_MMCSDSLOTNO, spi3);
if (result != OK)
{
message("[boot] FAILED to bind SPI port 3 to the MMCSD driver\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
message("[boot] Successfully bound SPI port 3 to the MMCSD driver\r\n");
#endif /* SPI3 */
/* initialize I2C1 bus */
i2c1 = up_i2cinitialize(1);
if (!i2c1) {
message("[boot] FAILED to initialize I2C bus 1\r\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* set I2C1 speed */
I2C_SETFREQUENCY(i2c1, 400000);
/* INIT 3: MULTIPORT-DEPENDENT INITIALIZATION */
/* Get board information if available */
/* Initialize the user GPIOs */
px4fmu_gpio_init();
#ifdef CONFIG_ADC
int adc_state = adc_devinit();
if (adc_state != OK)
{
/* Try again */
adc_state = adc_devinit();
if (adc_state != OK)
{
/* Give up */
message("[boot] FAILED adc_devinit: %d\r\n", adc_state);
return -ENODEV;
}
}
#endif
/* configure the tone generator */
#ifdef CONFIG_TONE_ALARM
tone_alarm_init();
#endif
return OK;
}
/**
* Hold the usb hub under reset
*
* @param dev Device
* @return 0 if successful
*/
static int usb3813_hold_reset(struct device *dev)
{
gpio_direction_out(HUB_LINE_N_RESET, 0);
up_udelay(HUB_RESET_ASSERTION_TIME_IN_USEC);
return 0;
}
__EXPORT int nsh_archinitialize(void)
{
/* configure ADC pins */
stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
stm32_configgpio(GPIO_ADC1_IN11); /* BATT2_VOLTAGE_SENS */
stm32_configgpio(GPIO_ADC1_IN13); /* BATT2_CURRENT_SENS */
/* configure power supply control/sense pins */
stm32_configgpio(GPIO_VDD_3V3_PERIPH_EN);
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
stm32_configgpio(GPIO_VDD_5V_HIPOWER_EN);
stm32_configgpio(GPIO_VDD_BRICK_VALID);
stm32_configgpio(GPIO_VDD_BRICK2_VALID);
stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
stm32_configgpio(GPIO_VBUS_VALID);
// stm32_configgpio(GPIO_SBUS_INV);
// stm32_configgpio(GPIO_8266_GPIO0);
// stm32_configgpio(GPIO_SPEKTRUM_PWR_EN);
// stm32_configgpio(GPIO_8266_PD);
// stm32_configgpio(GPIO_8266_RST);
// stm32_configgpio(GPIO_BTN_SAFETY_FMU);
/* configure the GPIO pins to outputs and keep them low */
stm32_configgpio(GPIO_GPIO0_OUTPUT);
stm32_configgpio(GPIO_GPIO1_OUTPUT);
stm32_configgpio(GPIO_GPIO2_OUTPUT);
stm32_configgpio(GPIO_GPIO3_OUTPUT);
stm32_configgpio(GPIO_GPIO4_OUTPUT);
stm32_configgpio(GPIO_GPIO5_OUTPUT);
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure the DMA allocator */
dma_alloc_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1) {
message("[boot] FAILED to initialize SPI port 1\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_ICM, false);
SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
SPI_SELECT(spi1, PX4_SPIDEV_LIS, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
SPI_SELECT(spi1, PX4_SPIDEV_EEPROM, false);
up_udelay(20);
/* Get the SPI port for the FRAM */
spi2 = up_spiinitialize(2);
if (!spi2) {
message("[boot] FAILED to initialize SPI port 2\n");
up_ledon(LED_AMBER);
return -ENODEV;
}
/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
* and de-assert the known chip selects. */
// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
/* Configure SPI 5-based devices */
spi5 = up_spiinitialize(PX4_SPI_EXT0);
if (!spi5) {
message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_EXT0);
up_ledon(LED_RED);
return -ENODEV;
}
/* Default SPI5 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi5, 10000000);
SPI_SETBITS(spi5, 8);
SPI_SETMODE(spi5, SPIDEV_MODE3);
SPI_SELECT(spi5, PX4_SPIDEV_EXT0, false);
/* Configure SPI 6-based devices */
spi6 = up_spiinitialize(PX4_SPI_EXT1);
if (!spi6) {
message("[boot] FAILED to initialize SPI port %d\n", PX4_SPI_EXT1);
up_ledon(LED_RED);
return -ENODEV;
}
/* Default SPI6 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi6, 10000000);
SPI_SETBITS(spi6, 8);
SPI_SETMODE(spi6, SPIDEV_MODE3);
SPI_SELECT(spi6, PX4_SPIDEV_EXT1, false);
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
message("[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
/* Now bind the SDIO interface to the MMC/SD driver */
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
if (ret != OK) {
message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
sdio_mediachange(sdio, true);
#endif
return OK;
}
__EXPORT int nsh_archinitialize(void)
{
/* configure ADC pins */
stm32_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
/* configure power supply control/sense pins */
stm32_configgpio(GPIO_VDD_5V_PERIPH_EN);
stm32_configgpio(GPIO_VDD_3V3_SENSORS_EN);
stm32_configgpio(GPIO_VDD_BRICK_VALID);
stm32_configgpio(GPIO_VDD_SERVO_VALID);
stm32_configgpio(GPIO_VDD_5V_HIPOWER_OC);
stm32_configgpio(GPIO_VDD_5V_PERIPH_OC);
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
/* run C++ ctors before we go any further */
up_cxxinitialize();
# if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)
# error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
# endif
#else
# error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
#endif
/* configure the high-resolution time/callout interface */
hrt_init();
/* configure the DMA allocator */
dma_alloc_init();
/* configure CPU load estimation */
#ifdef CONFIG_SCHED_INSTRUMENTATION
cpuload_initialize_once();
#endif
/* set up the serial DMA polling */
static struct hrt_call serial_dma_call;
struct timespec ts;
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
ts.tv_sec = 0;
ts.tv_nsec = 1000000;
hrt_call_every(&serial_dma_call,
ts_to_abstime(&ts),
ts_to_abstime(&ts),
(hrt_callout)stm32_serial_dma_poll,
NULL);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
/* Configure SPI-based devices */
spi1 = up_spiinitialize(1);
if (!spi1) {
syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 1\n");
board_led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI1 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi1, 10000000);
SPI_SETBITS(spi1, 8);
SPI_SETMODE(spi1, SPIDEV_MODE3);
SPI_SELECT(spi1, PX4_SPIDEV_GYRO, false);
SPI_SELECT(spi1, PX4_SPIDEV_ACCEL_MAG, false);
SPI_SELECT(spi1, PX4_SPIDEV_BARO, false);
SPI_SELECT(spi1, PX4_SPIDEV_MPU, false);
up_udelay(20);
syslog(LOG_INFO, "[boot] Initialized SPI port 1 (SENSORS)\n");
/* Get the SPI port for the FRAM */
spi2 = up_spiinitialize(2);
if (!spi2) {
syslog(LOG_ERR, "[boot] FAILED to initialize SPI port 2\n");
board_led_on(LED_AMBER);
return -ENODEV;
}
/* Default SPI2 to 37.5 MHz (40 MHz rounded to nearest valid divider, F4 max)
* and de-assert the known chip selects. */
// XXX start with 10.4 MHz in FRAM usage and go up to 37.5 once validated
SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000);
SPI_SETBITS(spi2, 8);
SPI_SETMODE(spi2, SPIDEV_MODE3);
SPI_SELECT(spi2, SPIDEV_FLASH, false);
syslog(LOG_INFO, "[boot] Initialized SPI port 2 (RAMTRON FRAM)\n");
spi4 = up_spiinitialize(4);
/* Default SPI4 to 1MHz and de-assert the known chip selects. */
SPI_SETFREQUENCY(spi4, 10000000);
SPI_SETBITS(spi4, 8);
SPI_SETMODE(spi4, SPIDEV_MODE3);
SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false);
SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false);
syslog(LOG_INFO, "[boot] Initialized SPI port 4\n");
#ifdef CONFIG_MMCSD
/* First, get an instance of the SDIO interface */
sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO);
if (!sdio) {
syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n",
CONFIG_NSH_MMCSDSLOTNO);
return -ENODEV;
}
/* Now bind the SDIO interface to the MMC/SD driver */
int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio);
if (ret != OK) {
syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret);
return ret;
}
/* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */
sdio_mediachange(sdio, true);
syslog(LOG_INFO, "[boot] Initialized SDIO\n");
#endif
return OK;
}
int
PX4IO_Uploader::upload(const char *filenames[])
{
int ret;
const char *filename = NULL;
size_t fw_size;
#ifndef PX4IO_SERIAL_DEVICE
#error Must define PX4IO_SERIAL_DEVICE in board configuration to support firmware upload
#endif
/* allow an early abort and look for file first */
for (unsigned i = 0; filenames[i] != nullptr; i++) {
_fw_fd = open(filenames[i], O_RDONLY);
if (_fw_fd < 0) {
log("failed to open %s", filenames[i]);
continue;
}
log("using firmware from %s", filenames[i]);
filename = filenames[i];
break;
}
if (filename == NULL) {
log("no firmware found");
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
_io_fd = open(PX4IO_SERIAL_DEVICE, O_RDWR);
if (_io_fd < 0) {
log("could not open interface");
return -errno;
}
/* save initial uart configuration to reset after the update */
struct termios t_original;
tcgetattr(_io_fd, &t_original);
/* adjust line speed to match bootloader */
struct termios t;
tcgetattr(_io_fd, &t);
cfsetspeed(&t, 115200);
tcsetattr(_io_fd, TCSANOW, &t);
/* look for the bootloader for 150 ms */
for (int i = 0; i < 15; i++) {
ret = sync();
if (ret == OK) {
break;
} else {
usleep(10000);
}
}
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -EIO;
}
struct stat st;
if (stat(filename, &st) != 0) {
log("Failed to stat %s - %d\n", filename, (int)errno);
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -errno;
}
fw_size = st.st_size;
if (_fw_fd == -1) {
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -ENOENT;
}
/* do the usual program thing - allow for failure */
for (unsigned retries = 0; retries < 1; retries++) {
if (retries > 0) {
log("retrying update...");
ret = sync();
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return -EIO;
}
}
ret = get_info(INFO_BL_REV, bl_rev);
if (ret == OK) {
if (bl_rev <= BL_REV) {
log("found bootloader revision: %d", bl_rev);
} else {
log("found unsupported bootloader revision %d, exiting", bl_rev);
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return OK;
}
}
ret = erase();
if (ret != OK) {
log("erase failed");
continue;
}
ret = program(fw_size);
if (ret != OK) {
log("program failed");
continue;
}
if (bl_rev <= 2) {
ret = verify_rev2(fw_size);
} else {
/* verify rev 3 and higher. Every version *needs* to be verified. */
ret = verify_rev3(fw_size);
}
if (ret != OK) {
log("verify failed");
continue;
}
ret = reboot();
if (ret != OK) {
log("reboot failed");
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_io_fd);
_io_fd = -1;
return ret;
}
log("update complete");
ret = OK;
break;
}
/* reset uart to previous/default baudrate */
tcsetattr(_io_fd, TCSANOW, &t_original);
close(_fw_fd);
close(_io_fd);
_io_fd = -1;
// sleep for enough time for the IO chip to boot. This makes
// forceupdate more reliably startup IO again after update
up_udelay(100*1000);
return ret;
}
