/*
* Initialise the timer we are going to use.
*/
void PWMIN::_timer_init(void)
{
/* run with interrupts disabled in case the timer is already
* setup. We don't want it firing while we are doing the setup */
irqstate_t flags = px4_enter_critical_section();
/* configure input pin */
px4_arch_configgpio(GPIO_PWM_IN);
// XXX refactor this out of this driver
/* configure reset pin */
px4_arch_configgpio(GPIO_VDD_RANGEFINDER_EN);
/* claim our interrupt vector */
irq_attach(PWMIN_TIMER_VECTOR, pwmin_tim_isr, NULL);
/* Clear no bits, set timer enable bit.*/
modifyreg32(PWMIN_TIMER_POWER_REG, 0, PWMIN_TIMER_POWER_BIT);
/* disable and configure the timer */
rCR1 = 0;
rCR2 = 0;
rSMCR = 0;
rDIER = DIER_PWMIN_A;
rCCER = 0; /* unlock CCMR* registers */
rCCMR1 = CCMR1_PWMIN;
rCCMR2 = CCMR2_PWMIN;
rSMCR = SMCR_PWMIN_1; /* Set up mode */
rSMCR = SMCR_PWMIN_2; /* Enable slave mode controller */
rCCER = CCER_PWMIN;
rDCR = 0;
/* for simplicity scale by the clock in MHz. This gives us
* readings in microseconds which is typically what is needed
* for a PWM input driver */
uint32_t prescaler = PWMIN_TIMER_CLOCK / 1000000UL;
/*
* define the clock speed. We want the highest possible clock
* speed that avoids overflows.
*/
rPSC = prescaler - 1;
/* run the full span of the counter. All timers can handle
* uint16 */
rARR = UINT16_MAX;
/* generate an update event; reloads the counter, all registers */
rEGR = GTIM_EGR_UG;
/* enable the timer */
rCR1 = GTIM_CR1_CEN;
/* enable interrupts */
up_enable_irq(PWMIN_TIMER_VECTOR);
px4_leave_critical_section(flags);
_timer_started = true;
}
__EXPORT void stm32_spiinitialize(void)
{
#ifdef CONFIG_STM32_SPI1
px4_arch_configgpio(GPIO_SPI_CS_MPU9250);
px4_arch_configgpio(GPIO_SPI_CS_HMC5983);
px4_arch_configgpio(GPIO_SPI_CS_MS5611);
px4_arch_configgpio(GPIO_SPI_CS_ICM_20608_G);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
px4_arch_gpiowrite(GPIO_SPI_CS_MPU9250, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_HMC5983, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MS5611, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
px4_arch_configgpio(GPIO_DRDY_MPU9250);
px4_arch_configgpio(GPIO_DRDY_HMC5983);
px4_arch_configgpio(GPIO_DRDY_ICM_20608_G);
#endif
#ifdef CONFIG_STM32_SPI2
px4_arch_configgpio(GPIO_SPI_CS_FRAM);
px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
#endif
}
int
ToneAlarm::init()
{
int ret;
ret = CDev::init();
if (ret != OK) {
return ret;
}
/* configure the GPIO to the idle state */
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
#ifdef GPIO_TONE_ALARM_NEG
px4_arch_configgpio(GPIO_TONE_ALARM_NEG);
#endif
/* clock/power on our timer */
modifyreg32(TONE_ALARM_CLOCK_POWER_REG, 0, TONE_ALARM_CLOCK_ENABLE);
/* initialise the timer */
rCR1 = 0;
rCR2 = 0;
rSMCR = 0;
rDIER = 0;
rCCER &= TONE_CCER; /* unlock CCMR* registers */
rCCMR1 = TONE_CCMR1;
rCCMR2 = TONE_CCMR2;
rCCER = TONE_CCER;
rDCR = 0;
#ifdef rBDTR // If using an advanced timer, you need to activate the output
rBDTR = ATIM_BDTR_MOE; // enable the main output of the advanced timer
#endif
/* toggle the CC output each time the count passes 1 */
TONE_rCCR = 1;
/* default the timer to a prescale value of 1; playing notes will change this */
rPSC = 0;
/* make sure the timer is running */
rCR1 = GTIM_CR1_CEN;
DEVICE_DEBUG("ready");
return OK;
}
static void
led_pwm_channel_init(unsigned channel)
{
unsigned timer = led_pwm_channels[channel].timer_index;
/* configure the GPIO first */
px4_arch_configgpio(led_pwm_channels[channel].gpio_out);
/* configure the channel */
switch (led_pwm_channels[channel].timer_channel) {
case 1:
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
rCCER(timer) |= GTIM_CCER_CC1E;
break;
case 2:
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
rCCER(timer) |= GTIM_CCER_CC2E;
break;
case 3:
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
rCCER(timer) |= GTIM_CCER_CC3E;
break;
case 4:
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
rCCER(timer) |= GTIM_CCER_CC4E;
break;
}
}
void
ToneAlarm::start_note(unsigned note)
{
// check if circuit breaker is enabled
if (_cbrk == CBRK_UNINIT) {
_cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY);
}
if (_cbrk != CBRK_OFF) { return; }
// compute the divisor
unsigned divisor = note_to_divisor(note);
// pick the lowest prescaler value that we can use
// (note that the effective prescale value is 1 greater)
unsigned prescale = divisor / 65536;
// calculate the timer period for the selected prescaler value
unsigned period = (divisor / (prescale + 1)) - 1;
rPSC = prescale; // load new prescaler
rARR = period; // load new toggle period
rEGR = GTIM_EGR_UG; // force a reload of the period
rCCER |= TONE_CCER; // enable the output
// configure the GPIO to enable timer output
px4_arch_configgpio(GPIO_TONE_ALARM);
}
int
TEST_PPM::init()
{
px4_arch_configgpio(TEST_PPM_PIN);
start();
return OK;
}
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/************************************************************************************
* Name: board_peripheral_reset
*
* Description:
*
************************************************************************************/
__EXPORT void board_peripheral_reset(int ms)
{
/* set the peripheral rails off */
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 1);
/* wait for the peripheral rail to reach GND */
usleep(ms * 1000);
warnx("reset done, %d ms", ms);
/* re-enable power */
/* switch the peripheral rail back on */
px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_EN, 0);
}
void CameraInterfaceGPIO::setup()
{
for (unsigned i = 0; i < arraySize(_pins); i++) {
px4_arch_configgpio(_gpios[_pins[i]]);
px4_arch_gpiowrite(_gpios[_pins[i]], !_polarity);
}
}
void ToneAlarmInterface::stop_note()
{
// Stop the current note.
rSC &= ~TPM_SC_CMOD_MASK;
// Ensure the GPIO is not driving the speaker.
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
}
__END_DECLS
__EXPORT void led_init()
{
/* Configure LED1 GPIO for output */
px4_arch_configgpio(GPIO_LED1);
}
/**
* Initialise the high-resolution timing module.
*/
void
hrt_init(void)
{
sq_init(&callout_queue);
hrt_tim_init();
#ifdef HRT_PPM_CHANNEL
/* configure the PPM input pin */
px4_arch_configgpio(GPIO_PPM_IN);
#endif
}
void
ToneAlarm::stop_note()
{
/* stop the current note */
rCCER &= ~TONE_CCER;
/*
* Make sure the GPIO is not driving the speaker.
*/
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
}
void ToneAlarmInterface::init()
{
#ifdef GPIO_TONE_ALARM_NEG
px4_arch_configgpio(GPIO_TONE_ALARM_NEG);
#else
// Configure the GPIO to the idle state.
px4_arch_configgpio(GPIO_TONE_ALARM_IDLE);
#endif
// Select a the clock source to the TPM.
uint32_t regval = _REG(KINETIS_SIM_SOPT2);
regval &= ~(SIM_SOPT2_TPMSRC_MASK);
regval |= BOARD_TPM_CLKSRC;
_REG(KINETIS_SIM_SOPT2) = regval;
// Enabled System Clock Gating Control for TPM.
regval = _REG(KINETIS_SIM_SCGC2);
regval |= TONE_ALARM_SIM_SCGC2_TPM;
_REG(KINETIS_SIM_SCGC2) = regval;
// Disable and configure the timer.
rSC = TPM_SC_TOF;
rCNT = 0;
rMOD = TONE_ALARM_COUNTER_PERIOD - 1;
rSTATUS = (TPM_STATUS_TOF | STATUS);
// Configure for output compare to toggle on over flow.
rCNSC = (TPM_CnSC_CHF | TPM_CnSC_MSA | TPM_CnSC_ELSA);
rCOMBINE = 0;
rPOL = 0;
rFILTER = 0;
rQDCTRL = 0;
rCONF = TPM_CONF_DBGMODE_CONT;
rCNV = 0; // Toggle the CC output each time the count passes 0.
rSC |= (TPM_SC_CMOD_LPTPM_CLK | TONE_ALARM_TIMER_PRESCALE); // Enable the timer.
rMOD = 0; // Default the timer to a modulo value of 1; playing notes will change this.
}
void
GPS::cmd_reset()
{
#ifdef GPIO_GPS_NRESET
PX4_WARN("Toggling GPS reset pin");
px4_arch_configgpio(GPIO_GPS_NRESET);
px4_arch_gpiowrite(GPIO_GPS_NRESET, 0);
usleep(100);
px4_arch_gpiowrite(GPIO_GPS_NRESET, 1);
PX4_WARN("Toggled GPS reset pin");
#endif
}
__EXPORT void stm32_spiinitialize(void)
{
#ifdef CONFIG_STM32_SPI1
px4_arch_configgpio(GPIO_SPI_CS_ICM_20608_G);
px4_arch_configgpio(GPIO_SPI_CS_BARO);
px4_arch_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
px4_arch_gpiowrite(GPIO_SPI_CS_ICM_20608_G, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
px4_arch_configgpio(GPIO_EXTI_ICM_20608_G_DRDY);
#endif
#ifdef CONFIG_STM32_SPI2
px4_arch_configgpio(GPIO_SPI_CS_FRAM);
px4_arch_gpiowrite(GPIO_SPI_CS_FRAM, 1);
#endif
}
__EXPORT void stm32_usbinitialize(void)
{
/* The OTG FS has an internal soft pull-up */
/* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */
#ifdef CONFIG_STM32_OTGFS
px4_arch_configgpio(GPIO_OTGFS_VBUS);
/* XXX We only support device mode
px4_arch_configgpio(GPIO_OTGFS_PWRON);
px4_arch_configgpio(GPIO_OTGFS_OVER);
*/
#endif
}
void ToneAlarmInterface::start_note(unsigned frequency)
{
// Calculate the signal switching period.
// (Signal switching period is one half of the frequency period).
float signal_period = (1.0f / frequency) * 0.5f;
// Calculate the hardware clock divisor rounded to the nearest integer.
unsigned int divisor = std::round(signal_period * TONE_ALARM_CLOCK);
rCNT = 0;
rMOD = divisor; // Load new signal switching period.
rSC |= (TPM_SC_CMOD_LPTPM_CLK);
// Configure the GPIO to enable timer output.
px4_arch_configgpio(GPIO_TONE_ALARM);
}
static void
led_pwm_channel_init(unsigned channel)
{
/* Only initialize used channels */
if (led_pwm_channels[channel].timer_channel) {
unsigned timer = led_pwm_channels[channel].timer_index;
printf("init led pwm timer #%d, channel #%d\n", timer, channel);
/* configure the GPIO first */
px4_arch_configgpio(led_pwm_channels[channel].gpio_out);
uint16_t polarity = 0; //led_pwm_channels[channel].masks;
printf("polarity for channel #%d: %d\n", channel, polarity);
/* configure the channel */
switch (led_pwm_channels[channel].timer_channel) {
case 1:
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE;
rCCER(timer) |= polarity | GTIM_CCER_CC1E;
break;
case 2:
rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE;
rCCER(timer) |= polarity | GTIM_CCER_CC2E;
break;
case 3:
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE;
rCCER(timer) |= polarity | GTIM_CCER_CC3E;
break;
case 4:
rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE;
rCCER(timer) |= polarity | GTIM_CCER_CC4E;
break;
}
}
}
__EXPORT void stm32_spiinitialize(void)
{
#ifdef CONFIG_STM32_SPI1
px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X);
px4_arch_configgpio(GPIO_SPI_CS_BARO);
px4_arch_configgpio(GPIO_SPI_CS_MPU);
px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY);
#endif
#ifdef CONFIG_STM32_SPI2
px4_arch_configgpio(GPIO_SPI_CS_FRAM);
#endif
}
void
interface_init(void)
{
pc_txns = perf_alloc(PC_ELAPSED, "txns");
pc_errors = perf_alloc(PC_COUNT, "errors");
pc_ore = perf_alloc(PC_COUNT, "overrun");
pc_fe = perf_alloc(PC_COUNT, "framing");
pc_ne = perf_alloc(PC_COUNT, "noise");
pc_idle = perf_alloc(PC_COUNT, "idle");
pc_badidle = perf_alloc(PC_COUNT, "badidle");
pc_regerr = perf_alloc(PC_COUNT, "regerr");
pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
/* allocate DMA */
tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
/* configure pins for serial use */
px4_arch_configgpio(PX4FMU_SERIAL_TX_GPIO);
px4_arch_configgpio(PX4FMU_SERIAL_RX_GPIO);
/* reset and configure the UART */
rCR1 = 0;
rCR2 = 0;
rCR3 = 0;
/* clear status/errors */
(void)rSR;
(void)rDR;
/* configure line speed */
uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2);
uint32_t mantissa = usartdiv32 >> 5;
uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
/* connect our interrupt */
irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt, NULL);
up_enable_irq(PX4FMU_SERIAL_VECTOR);
/* enable UART and error/idle interrupts */
rCR3 = USART_CR3_EIE;
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
#if 0 /* keep this for signal integrity testing */
for (;;) {
while (!(rSR & USART_SR_TXE))
;
rDR = 0xfa;
while (!(rSR & USART_SR_TXE))
;
rDR = 0xa0;
}
#endif
/* configure RX DMA and return to listening state */
dma_reset();
debug("serial init");
}
__END_DECLS
/****************************************************************************
* Protected Functions
****************************************************************************/
/****************************************************************************
* Public Functions
****************************************************************************/
/************************************************************************************
* Name: stm32_boardinitialize
*
* Description:
* All STM32 architectures must provide the following entry point. This entry point
* is called early in the intitialization -- after all memory has been configured
* and mapped but before any devices have been initialized.
*
************************************************************************************/
__EXPORT void
stm32_boardinitialize(void)
{
/* configure LEDs */
board_autoled_initialize();
/* configure ADC pins */
px4_arch_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
px4_arch_configgpio(GPIO_ADC1_IN10); /* BATT_CURRENT_SENS */
px4_arch_configgpio(GPIO_ADC1_IN12); /* BATT_VOLTAGE_SENS */
px4_arch_configgpio(GPIO_ADC1_IN11); /* RSSI analog in */
px4_arch_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
px4_arch_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
px4_arch_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
/* configure power supply control/sense pins */
px4_arch_configgpio(GPIO_SBUS_INV);
px4_arch_configgpio(GPIO_FRSKY_INV);
/* configure the GPIO pins to outputs and keep them low */
px4_arch_configgpio(GPIO_GPIO0_OUTPUT);
px4_arch_configgpio(GPIO_GPIO1_OUTPUT);
px4_arch_configgpio(GPIO_GPIO2_OUTPUT);
px4_arch_configgpio(GPIO_GPIO3_OUTPUT);
px4_arch_configgpio(GPIO_GPIO4_OUTPUT);
px4_arch_configgpio(GPIO_GPIO5_OUTPUT);
px4_arch_configgpio(GPIO_GPIO6_OUTPUT);
px4_arch_configgpio(GPIO_GPIO7_OUTPUT);
/* configure SPI interfaces */
stm32_spiinitialize();
stm32_configgpio(GPIO_I2C2_SCL);
stm32_configgpio(GPIO_I2C2_SDA);
stm32_configgpio(GPIO_I2C1_SCL);
stm32_configgpio(GPIO_I2C1_SDA);
}
__EXPORT void board_spi_reset(int ms)
{
/* disable SPI bus */
px4_arch_configgpio(GPIO_SPI_CS_GYRO_OFF);
px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG_OFF);
px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF);
px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF);
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
px4_arch_configgpio(GPIO_SPI1_SCK_OFF);
px4_arch_configgpio(GPIO_SPI1_MISO_OFF);
px4_arch_configgpio(GPIO_SPI1_MOSI_OFF);
px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
px4_arch_configgpio(GPIO_GYRO_DRDY_OFF);
px4_arch_configgpio(GPIO_ACCEL_DRDY_OFF);
px4_arch_configgpio(GPIO_MAG_DRDY_OFF);
px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
px4_arch_gpiowrite(GPIO_GYRO_DRDY_OFF, 0);
px4_arch_gpiowrite(GPIO_ACCEL_DRDY_OFF, 0);
px4_arch_gpiowrite(GPIO_MAG_DRDY_OFF, 0);
px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
/* set the sensor rail off */
px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
/* wait for the sensor rail to reach GND */
usleep(ms * 1000);
warnx("reset done, %d ms", ms);
/* re-enable power */
/* switch the sensor rail back on */
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
/* wait a bit before starting SPI, different times didn't influence results */
usleep(100);
/* reconfigure the SPI pins */
#ifdef CONFIG_STM32_SPI4
px4_arch_configgpio(GPIO_SPI_CS_GYRO);
px4_arch_configgpio(GPIO_SPI_CS_ACCEL_MAG);
px4_arch_configgpio(GPIO_SPI_CS_BARO);
px4_arch_configgpio(GPIO_SPI_CS_MPU);
/* De-activate all peripherals,
* required for some peripheral
* state machines
*/
px4_arch_gpiowrite(GPIO_SPI_CS_GYRO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO, 1);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU, 1);
px4_arch_configgpio(GPIO_SPI4_SCK);
px4_arch_configgpio(GPIO_SPI4_MISO);
px4_arch_configgpio(GPIO_SPI4_MOSI);
#endif
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* GPIO config.
* Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
* If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
* fail during initialization.
*/
#if defined(GPIO_CAN1_RX)
px4_arch_configgpio(GPIO_CAN1_RX);
px4_arch_configgpio(GPIO_CAN1_TX);
#endif
#if defined(GPIO_CAN2_RX)
px4_arch_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
px4_arch_configgpio(GPIO_CAN2_TX);
#endif
#if !defined(GPIO_CAN1_RX) && !defined(GPIO_CAN2_RX)
# error "Need to define GPIO_CAN1_RX and/or GPIO_CAN2_RX"
#endif
/*
* CAN driver init
* Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
* shipped with libuavcan does not support deinitialization.
*/
static CanInitHelper* can = nullptr;
if (can == nullptr) {
can = new CanInitHelper();
if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown
warnx("Out of memory");
return -1;
}
const int can_init_res = can->init(bitrate);
if (can_init_res < 0) {
warnx("CAN driver init failed %i", can_init_res);
return can_init_res;
}
}
/*
* Node init
*/
_instance = new UavcanNode(can->driver, uavcan_stm32::SystemClock::instance());
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
const int node_init_res = _instance->init(node_id);
if (node_init_res < 0) {
delete _instance;
_instance = nullptr;
warnx("Node init failed %i", node_init_res);
return node_init_res;
}
/*
* Start the task. Normally it should never exit.
*/
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
_instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
warnx("start failed: %d", errno);
return -errno;
}
return OK;
}
PX4IO_serial_f7::PX4IO_serial_f7() :
_tx_dma(nullptr),
_rx_dma(nullptr),
_current_packet(nullptr),
_rx_dma_status(_dma_status_inactive),
_completion_semaphore(SEM_INITIALIZER(0)),
#if 0
_pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
_pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs "))
#else
_pc_dmasetup(nullptr),
_pc_dmaerrs(nullptr)
#endif
{
}
PX4IO_serial_f7::~PX4IO_serial_f7()
{
if (_tx_dma != nullptr) {
stm32_dmastop(_tx_dma);
stm32_dmafree(_tx_dma);
}
if (_rx_dma != nullptr) {
stm32_dmastop(_rx_dma);
stm32_dmafree(_rx_dma);
}
/* reset the UART */
rCR1 = 0;
rCR2 = 0;
rCR3 = 0;
/* detach our interrupt handler */
up_disable_irq(PX4IO_SERIAL_VECTOR);
irq_detach(PX4IO_SERIAL_VECTOR);
/* restore the GPIOs */
px4_arch_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
px4_arch_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
/* Disable APB clock for the USART peripheral */
modifyreg32(PX4IO_SERIAL_RCC_REG, PX4IO_SERIAL_RCC_EN, 0);
/* and kill our semaphores */
px4_sem_destroy(&_completion_semaphore);
perf_free(_pc_dmasetup);
perf_free(_pc_dmaerrs);
}
int
PX4IO_serial_f7::init()
{
/* initialize base implementation */
int r;
if ((r = PX4IO_serial::init((IOPacket *)ROUND_UP_TO_POW2_CT((uintptr_t)_io_buffer_storage, CACHE_LINE_SIZE))) != 0) {
return r;
}
/* allocate DMA */
_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) {
return -1;
}
/* Enable the APB clock for the USART peripheral */
modifyreg32(PX4IO_SERIAL_RCC_REG, 0, PX4IO_SERIAL_RCC_EN);
/* configure pins for serial use */
px4_arch_configgpio(PX4IO_SERIAL_TX_GPIO);
px4_arch_configgpio(PX4IO_SERIAL_RX_GPIO);
/* reset & configure the UART */
rCR1 = 0;
rCR2 = 0;
rCR3 = 0;
/* clear data that may be in the RDR and clear overrun error: */
if (rISR & USART_ISR_RXNE) {
(void)rRDR;
}
rICR = rISR & rISR_ERR_FLAGS_MASK; /* clear the flags */
/* configure line speed */
uint32_t usartdiv32 = (PX4IO_SERIAL_CLOCK + (PX4IO_SERIAL_BITRATE) / 2) / (PX4IO_SERIAL_BITRATE);
rBRR = usartdiv32;
/* attach serial interrupt handler */
irq_attach(PX4IO_SERIAL_VECTOR, _interrupt, this);
up_enable_irq(PX4IO_SERIAL_VECTOR);
/* enable UART in DMA mode, enable error and line idle interrupts */
rCR3 = USART_CR3_EIE;
/* TODO: maybe use DDRE */
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
/* TODO: maybe we need to adhere to the procedure as described in the reference manual page 1251 (34.5.2) */
/* create semaphores */
px4_sem_init(&_completion_semaphore, 0, 0);
/* _completion_semaphore use case is a signal */
px4_sem_setprotocol(&_completion_semaphore, SEM_PRIO_NONE);
/* XXX this could try talking to IO */
return 0;
}
__EXPORT int nsh_archinitialize(void)
{
/* configure ADC pins */
px4_arch_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
px4_arch_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
px4_arch_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
px4_arch_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
px4_arch_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
px4_arch_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
/* configure power supply control/sense pins */
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC);
/* configure the GPIO pins to outputs and keep them low */
px4_arch_configgpio(GPIO_GPIO0_OUTPUT);
px4_arch_configgpio(GPIO_GPIO1_OUTPUT);
px4_arch_configgpio(GPIO_GPIO2_OUTPUT);
px4_arch_configgpio(GPIO_GPIO3_OUTPUT);
px4_arch_configgpio(GPIO_GPIO4_OUTPUT);
px4_arch_configgpio(GPIO_GPIO5_OUTPUT);
/* 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);
/* initial LED state */
drv_led_start();
led_off(LED_AMBER);
/* Configure SPI-based devices */
spi1 = px4_spibus_initialize(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_MPU, false);
up_udelay(20);
/* Get the SPI port for the FRAM */
spi2 = px4_spibus_initialize(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);
#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 void board_spi_reset(int ms)
{
/* disable SPI bus */
px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X_OFF);
px4_arch_configgpio(GPIO_SPI_CS_BARO_OFF);
px4_arch_configgpio(GPIO_SPI_CS_MPU_OFF);
px4_arch_gpiowrite(GPIO_SPI_CS_ICM_2060X_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_BARO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI_CS_MPU_OFF, 0);
px4_arch_configgpio(GPIO_SPI1_SCK_OFF);
px4_arch_configgpio(GPIO_SPI1_MISO_OFF);
px4_arch_configgpio(GPIO_SPI1_MOSI_OFF);
px4_arch_gpiowrite(GPIO_SPI1_SCK_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI1_MISO_OFF, 0);
px4_arch_gpiowrite(GPIO_SPI1_MOSI_OFF, 0);
px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY_OFF);
px4_arch_configgpio(GPIO_EXTI_MPU_DRDY_OFF);
px4_arch_gpiowrite(GPIO_EXTI_ICM_2060X_DRDY_OFF, 0);
px4_arch_gpiowrite(GPIO_EXTI_MPU_DRDY_OFF, 0);
/* set the sensor rail off */
px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0);
/* wait for the sensor rail to reach GND */
usleep(ms * 1000);
syslog(LOG_DEBUG, "reset done, %d ms\n", ms);
/* re-enable power */
/* switch the sensor rail back on */
px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1);
/* wait a bit before starting SPI, different times didn't influence results */
usleep(100);
/* reconfigure the SPI pins */
#ifdef CONFIG_STM32_SPI1
px4_arch_configgpio(GPIO_SPI_CS_ICM_2060X);
px4_arch_configgpio(GPIO_SPI_CS_BARO);
px4_arch_configgpio(GPIO_SPI_CS_MPU);
px4_arch_configgpio(GPIO_SPI1_SCK);
px4_arch_configgpio(GPIO_SPI1_MISO);
px4_arch_configgpio(GPIO_SPI1_MOSI);
// // XXX bring up the EXTI pins again
// px4_arch_configgpio(GPIO_EXTI_MPU_DRDY);
// px4_arch_configgpio(GPIO_EXTI_ICM_2060X_DRDY);
#endif
}
void
Syslink::task_main()
{
_bridge = new SyslinkBridge(this);
_bridge->init();
_memory = new SyslinkMemory(this);
_memory->init();
_battery.reset(&_battery_status);
// int ret;
/* Open serial port */
const char *dev = "/dev/ttyS2";
_fd = open_serial(dev);
if (_fd < 0) {
err(1, "can't open %s", dev);
return;
}
/* Set non-blocking */
/*
int flags = fcntl(_fd, F_GETFL, 0);
fcntl(_fd, F_SETFL, flags | O_NONBLOCK);
*/
px4_arch_configgpio(GPIO_NRF_TXEN);
px4_pollfd_struct_t fds[2];
fds[0].fd = _fd;
fds[0].events = POLLIN;
_params_sub = orb_subscribe(ORB_ID(parameter_update));
fds[1].fd = _params_sub;
fds[1].events = POLLIN;
int error_counter = 0;
char buf[64];
int nread;
syslink_parse_state state;
syslink_message_t msg;
syslink_parse_init(&state);
//setup initial parameters
update_params(true);
while (_task_running) {
int poll_ret = px4_poll(fds, 2, 500);
/* handle the poll result */
if (poll_ret == 0) {
/* timeout: this means none of our providers is giving us data */
} else if (poll_ret < 0) {
/* this is seriously bad - should be an emergency */
if (error_counter < 10 || error_counter % 50 == 0) {
/* use a counter to prevent flooding (and slowing us down) */
PX4_ERR("[syslink] ERROR return value from poll(): %d"
, poll_ret);
}
error_counter++;
} else {
if (fds[0].revents & POLLIN) {
if ((nread = read(_fd, buf, sizeof(buf))) < 0) {
continue;
}
for (int i = 0; i < nread; i++) {
if (syslink_parse_char(&state, buf[i], &msg)) {
handle_message(&msg);
}
}
}
if (fds[1].revents & POLLIN) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), _params_sub, &update);
update_params(false);
}
}
}
close(_fd);
}
__EXPORT void
stm32_boardinitialize(void)
{
/* configure LEDs */
board_autoled_initialize();
/* configure ADC pins */
px4_arch_configgpio(GPIO_ADC1_IN2); /* BATT_VOLTAGE_SENS */
px4_arch_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */
px4_arch_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */
px4_arch_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */
px4_arch_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */
px4_arch_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */
/* configure power supply control/sense pins */
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_EN);
px4_arch_configgpio(GPIO_VDD_3V3_SENSORS_EN);
px4_arch_configgpio(GPIO_VDD_BRICK_VALID);
px4_arch_configgpio(GPIO_VDD_5V_PERIPH_OC);
/* configure the GPIO pins to outputs and keep them low */
px4_arch_configgpio(GPIO_GPIO0_OUTPUT);
px4_arch_configgpio(GPIO_GPIO1_OUTPUT);
px4_arch_configgpio(GPIO_GPIO2_OUTPUT);
px4_arch_configgpio(GPIO_GPIO3_OUTPUT);
px4_arch_configgpio(GPIO_GPIO4_OUTPUT);
px4_arch_configgpio(GPIO_GPIO5_OUTPUT);
/* configure SPI interfaces */
stm32_spiinitialize();
/* configure USB interface */
stm32_usbinitialize();
}
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
{
if (_instance != nullptr) {
warnx("Already started");
return -1;
}
/*
* GPIO config.
* Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
* If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
* fail during initialization.
*/
px4_arch_configgpio(GPIO_CAN1_RX);
px4_arch_configgpio(GPIO_CAN1_TX);
#if defined(GPIO_CAN2_RX)
px4_arch_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
px4_arch_configgpio(GPIO_CAN2_TX);
#endif
/*
* CAN driver init
*/
static CanInitHelper can;
static bool can_initialized = false;
if (!can_initialized) {
const int can_init_res = can.init(bitrate);
if (can_init_res < 0) {
warnx("CAN driver init failed %i", can_init_res);
return can_init_res;
}
can_initialized = true;
}
/*
* Node init
*/
_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
if (_instance == nullptr) {
warnx("Out of memory");
return -1;
}
resources("Before _instance->init:");
const int node_init_res = _instance->init(node_id);
resources("After _instance->init:");
if (node_init_res < 0) {
delete _instance;
_instance = nullptr;
warnx("Node init failed %i", node_init_res);
return node_init_res;
}
/* Keep the bit rate for reboots on BenginFirmware updates */
_instance->active_bitrate = bitrate;
/*
* Start the task. Normally it should never exit.
*/
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
_instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
static_cast<main_t>(run_trampoline), nullptr);
if (_instance->_task < 0) {
warnx("start failed: %d", errno);
return -errno;
}
return OK;
}
PX4IO_serial_f4::PX4IO_serial_f4() :
_tx_dma(nullptr),
_rx_dma(nullptr),
_current_packet(nullptr),
_rx_dma_status(_dma_status_inactive),
_completion_semaphore(SEM_INITIALIZER(0)),
#if 0
_pc_dmasetup(perf_alloc(PC_ELAPSED, "io_dmasetup ")),
_pc_dmaerrs(perf_alloc(PC_COUNT, "io_dmaerrs "))
#else
_pc_dmasetup(nullptr),
_pc_dmaerrs(nullptr)
#endif
{
}
PX4IO_serial_f4::~PX4IO_serial_f4()
{
if (_tx_dma != nullptr) {
stm32_dmastop(_tx_dma);
stm32_dmafree(_tx_dma);
}
if (_rx_dma != nullptr) {
stm32_dmastop(_rx_dma);
stm32_dmafree(_rx_dma);
}
/* reset the UART */
rCR1 = 0;
rCR2 = 0;
rCR3 = 0;
/* detach our interrupt handler */
up_disable_irq(PX4IO_SERIAL_VECTOR);
irq_detach(PX4IO_SERIAL_VECTOR);
/* restore the GPIOs */
px4_arch_unconfiggpio(PX4IO_SERIAL_TX_GPIO);
px4_arch_unconfiggpio(PX4IO_SERIAL_RX_GPIO);
/* Disable APB clock for the USART peripheral */
modifyreg32(PX4IO_SERIAL_RCC_REG, PX4IO_SERIAL_RCC_EN, 0);
/* and kill our semaphores */
px4_sem_destroy(&_completion_semaphore);
perf_free(_pc_dmasetup);
perf_free(_pc_dmaerrs);
}
int
PX4IO_serial_f4::init()
{
/* initialize base implementation */
int r;
if ((r = PX4IO_serial::init(&_io_buffer_storage)) != 0) {
return r;
}
/* allocate DMA */
_tx_dma = stm32_dmachannel(PX4IO_SERIAL_TX_DMAMAP);
_rx_dma = stm32_dmachannel(PX4IO_SERIAL_RX_DMAMAP);
if ((_tx_dma == nullptr) || (_rx_dma == nullptr)) {
return -1;
}
/* Enable the APB clock for the USART peripheral */
modifyreg32(PX4IO_SERIAL_RCC_REG, 0, PX4IO_SERIAL_RCC_EN);
/* configure pins for serial use */
px4_arch_configgpio(PX4IO_SERIAL_TX_GPIO);
px4_arch_configgpio(PX4IO_SERIAL_RX_GPIO);
/* reset & configure the UART */
rCR1 = 0;
rCR2 = 0;
rCR3 = 0;
/* eat any existing interrupt status */
(void)rSR;
(void)rDR;
/* configure line speed */
uint32_t usartdiv32 = PX4IO_SERIAL_CLOCK / (PX4IO_SERIAL_BITRATE / 2);
uint32_t mantissa = usartdiv32 >> 5;
uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
/* attach serial interrupt handler */
irq_attach(PX4IO_SERIAL_VECTOR, _interrupt, this);
up_enable_irq(PX4IO_SERIAL_VECTOR);
/* enable UART in DMA mode, enable error and line idle interrupts */
rCR3 = USART_CR3_EIE;
rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
/* create semaphores */
px4_sem_init(&_completion_semaphore, 0, 0);
/* _completion_semaphore use case is a signal */
px4_sem_setprotocol(&_completion_semaphore, SEM_PRIO_NONE);
/* XXX this could try talking to IO */
return 0;
}
