void HW_NVIC_init(void)
{
// There are 3 bits of priority implemented in MDR32F9Qx device
// Setting all bits to pre-emption, see link below
// http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.dui0552a/BABHGEAJ.html
NVIC_SetPriorityGrouping( 3 );
pr[0] = NVIC_GetPriority(SVCall_IRQn);
pr[1] = NVIC_GetPriority(PendSV_IRQn);
pr[2] = NVIC_GetPriority(SysTick_IRQn);
pr[3] = NVIC_GetPriority(DMA_IRQn);
pr[4] = NVIC_GetPriority(Timer2_IRQn);
NVIC_SetPriority(DMA_IRQn,6);
NVIC_SetPriority(Timer2_IRQn,6);
pr[0] = NVIC_GetPriority(SVCall_IRQn);
pr[1] = NVIC_GetPriority(PendSV_IRQn);
pr[2] = NVIC_GetPriority(SysTick_IRQn);
pr[3] = NVIC_GetPriority(DMA_IRQn);
pr[4] = NVIC_GetPriority(Timer2_IRQn);
}
void HW_NVIC_check(void)
{
pr[0] = NVIC_GetPriority(SVCall_IRQn);
pr[1] = NVIC_GetPriority(PendSV_IRQn);
pr[2] = NVIC_GetPriority(SysTick_IRQn);
pr[3] = NVIC_GetPriority(DMA_IRQn);
pr[4] = NVIC_GetPriority(Timer2_IRQn);
}
void HW_X_Config(void) {
uint32_t TickPrio;
//
// Assuming PLL and core clock were already set by SystemInit() (called
// from the startup code), initializing any hardware is not necessary.
// Just ensure that the system clock variable is updated and then
// set the periodic system timer tick for embOS.
//
SystemCoreClockUpdate(); // Ensure, the SystemCoreClock is set
if (SysTick_Config(SystemCoreClock / 1000)) { // Setup SysTick Timer for 1 msec interrupts
while (1); // Handle Error
}
//
// Initialize NVIC vector base address. Might be necessary for RAM targets
// or application not running from 0.
//
// NVIC_VTOR = (U32)&__Vectors;
//
// Set the interrupt priority for the system timer to 2nd lowest level to
// ensure the timer can preempt PendSV handler
//
NVIC_SetPriority(SysTick_IRQn, (uint32_t) -1);
TickPrio = NVIC_GetPriority(SysTick_IRQn);
TickPrio -=1;
NVIC_SetPriority(SysTick_IRQn, (uint32_t)TickPrio);
}
int vIRQ_GetLevel(void)
{
/* if uVisor is disabled we use the standard priority levels */
if (__uvisor_mode == 0) {
/* check if an IRQn is active (an ISR is being served) */
uint32_t ipsr = __get_IPSR();
int irqn = (int) (ipsr & 0x1FF) - NVIC_USER_IRQ_OFFSET;
if (irqn >= NVIC_NUM_VECTORS || !ipsr || !NVIC_GetActive((IRQn_Type) irqn)) {
return -1;
}
/* return the priority level of the active IRQ */
/* if we are in a system interrupt we do not provide the actual
* priority level, which is usually negative, since we already use -1
* for stating "no IRQn is active". This is consistent with the
* behavior of the actual uVisor-managed API, as this call will never
* come from a system interrupt. The caller can still use this
* information to assess that an IRQn is actually active (0 = lowest
* priority) */
if (irqn < 0) {
return 0;
}
else {
return NVIC_GetPriority((IRQn_Type) irqn);
}
}
else {
return UVISOR_SVC(UVISOR_SVC_ID_IRQ_LEVEL_GET, "");
}
}
size_t Uart::write(const uint8_t data)
{
if (sercom->isDataRegisterEmptyUART() && txBuffer.available() == 0) {
sercom->writeDataUART(data);
} else {
// spin lock until a spot opens up in the buffer
while(txBuffer.isFull()) {
uint8_t interruptsEnabled = ((__get_PRIMASK() & 0x1) == 0);
if (interruptsEnabled) {
uint32_t exceptionNumber = (SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk);
if (exceptionNumber == 0 ||
NVIC_GetPriority((IRQn_Type)(exceptionNumber - 16)) > SERCOM_NVIC_PRIORITY) {
// no exception or called from an ISR with lower priority,
// wait for free buffer spot via IRQ
continue;
}
}
// interrupts are disabled or called from ISR with higher or equal priority than the SERCOM IRQ
// manually call the UART IRQ handler when the data register is empty
if (sercom->isDataRegisterEmptyUART()) {
IrqHandler();
}
}
txBuffer.store_char(data);
sercom->enableDataRegisterEmptyInterruptUART();
}
return 1;
}
/**
* @brief Gets the priority of an interrupt.
* @param IRQn: External interrupt number.
* This parameter can be an enumerator of IRQn_Type enumeration
* (For the complete STM32 Devices IRQ Channels list, please refer to the appropriate CMSIS device file (stm32f4xxxx.h))
* @param PriorityGroup: the priority grouping bits length.
* This parameter can be one of the following values:
* @arg NVIC_PRIORITYGROUP_0: 0 bits for preemption priority
* 4 bits for subpriority
* @arg NVIC_PRIORITYGROUP_1: 1 bits for preemption priority
* 3 bits for subpriority
* @arg NVIC_PRIORITYGROUP_2: 2 bits for preemption priority
* 2 bits for subpriority
* @arg NVIC_PRIORITYGROUP_3: 3 bits for preemption priority
* 1 bits for subpriority
* @arg NVIC_PRIORITYGROUP_4: 4 bits for preemption priority
* 0 bits for subpriority
* @param pPreemptPriority: Pointer on the Preemptive priority value (starting from 0).
* @param pSubPriority: Pointer on the Subpriority value (starting from 0).
* @retval None
*/
void HAL_NVIC_GetPriority(IRQn_Type IRQn, uint32_t PriorityGroup, uint32_t *pPreemptPriority, uint32_t *pSubPriority)
{
/* Check the parameters */
assert_param(IS_NVIC_PRIORITY_GROUP(PriorityGroup));
/* Get priority for Cortex-M system or device specific interrupts */
NVIC_DecodePriority(NVIC_GetPriority(IRQn), PriorityGroup, pPreemptPriority, pSubPriority);
}
uint32_t vIRQ_GetPriority(uint32_t irqn)
{
if(__uvisor_mode == 0) {
return NVIC_GetPriority((IRQn_Type) irqn);
}
else {
return UVISOR_SVC(UVISOR_SVC_ID_IRQ_PRIO_GET, "", irqn);
}
}
// This is used to configure UARTs depending on the MRI configuration, see Kernel::Kernel()
static int isDebugMonitorUsingUart0(){
return NVIC_GetPriority(UART0_IRQn) == 0;
}
uint32_t UARTClass::getInterruptPriority()
{
return NVIC_GetPriority(_dwIrq);
}
// The kernel is the central point in Smoothie : it stores modules, and handles event calls
Kernel::Kernel(){
instance= this; // setup the Singleton instance of the kernel
// serial first at fixed baud rate (DEFAULT_SERIAL_BAUD_RATE) so config can report errors to serial
// Set to UART0, this will be changed to use the same UART as MRI if it's enabled
this->serial = new SerialConsole(USBTX, USBRX, DEFAULT_SERIAL_BAUD_RATE);
// Config next, but does not load cache yet
this->config = new Config();
// Pre-load the config cache, do after setting up serial so we can report errors to serial
this->config->config_cache_load();
// now config is loaded we can do normal setup for serial based on config
delete this->serial;
this->serial= NULL;
this->streams = new StreamOutputPool();
this->current_path = "/";
// Configure UART depending on MRI config
// Match up the SerialConsole to MRI UART. This makes it easy to use only one UART for both debug and actual commands.
NVIC_SetPriorityGrouping(0);
#if MRI_ENABLE != 0
switch( __mriPlatform_CommUartIndex() ) {
case 0:
this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number());
break;
case 1:
this->serial = new SerialConsole( p13, p14, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number());
break;
case 2:
this->serial = new SerialConsole( p28, p27, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number());
break;
case 3:
this->serial = new SerialConsole( p9, p10, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number());
break;
}
#endif
// default
if(this->serial == NULL) {
this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(DEFAULT_SERIAL_BAUD_RATE)->as_number());
}
this->add_module( this->config );
this->add_module( this->serial );
// HAL stuff
add_module( this->slow_ticker = new SlowTicker());
this->step_ticker = new StepTicker();
this->adc = new Adc();
// TODO : These should go into platform-specific files
// LPC17xx-specific
NVIC_SetPriorityGrouping(0);
NVIC_SetPriority(TIMER0_IRQn, 2);
NVIC_SetPriority(TIMER1_IRQn, 1);
NVIC_SetPriority(TIMER2_IRQn, 3);
// Set other priorities lower than the timers
NVIC_SetPriority(ADC_IRQn, 4);
NVIC_SetPriority(USB_IRQn, 4);
// If MRI is enabled
if( MRI_ENABLE ){
if( NVIC_GetPriority(UART0_IRQn) > 0 ){ NVIC_SetPriority(UART0_IRQn, 4); }
if( NVIC_GetPriority(UART1_IRQn) > 0 ){ NVIC_SetPriority(UART1_IRQn, 4); }
if( NVIC_GetPriority(UART2_IRQn) > 0 ){ NVIC_SetPriority(UART2_IRQn, 4); }
if( NVIC_GetPriority(UART3_IRQn) > 0 ){ NVIC_SetPriority(UART3_IRQn, 4); }
}else{
NVIC_SetPriority(UART0_IRQn, 4);
NVIC_SetPriority(UART1_IRQn, 4);
NVIC_SetPriority(UART2_IRQn, 4);
NVIC_SetPriority(UART3_IRQn, 4);
}
// Configure the step ticker
this->base_stepping_frequency = this->config->value(base_stepping_frequency_checksum )->by_default(100000)->as_number();
float microseconds_per_step_pulse = this->config->value(microseconds_per_step_pulse_checksum )->by_default(5 )->as_number();
// Configure the step ticker ( TODO : shouldnt this go into stepticker's code ? )
this->step_ticker->set_reset_delay( microseconds_per_step_pulse / 1000000L );
this->step_ticker->set_frequency( this->base_stepping_frequency );
// Core modules
this->add_module( this->gcode_dispatch = new GcodeDispatch() );
this->add_module( this->robot = new Robot() );
this->add_module( this->stepper = new Stepper() );
this->add_module( this->planner = new Planner() );
this->add_module( this->conveyor = new Conveyor() );
this->add_module( this->pauser = new Pauser() );
}
uint32_t np_isr_get_prio(const struct npdev_isr * isr)
{
return (NVIC_GetPriority(isr->irqn));
}
void main_task(void *pvParameters)
{
(void) pvParameters;
vTaskDelay(500 / portTICK_RATE_MS);
SPIMaster spi5(SPI5, SPI_BAUDRATEPRESCALER_32, 0x2000, {
{MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
{MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
{MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI5},
}, {
{GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{ACCEL_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
});
SPIMaster spi2(SPI2, SPI_BAUDRATEPRESCALER_32, 0x2000, {
{EXT_MEMS_SPI_SCK_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
{EXT_MEMS_SPI_MISO_PIN, GPIO_MODE_AF_PP, GPIO_PULLDOWN, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
{EXT_MEMS_SPI_MOSI_PIN, GPIO_MODE_AF_PP, GPIO_NOPULL, GPIO_SPEED_MEDIUM, GPIO_AF5_SPI2},
}, {
{EXT_GYRO_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{LPS25HB_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
{BMP280_PRESSURE_CS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_MEDIUM, 0},
});
L3GD20 gyro(spi5, 0);
LPS25HB lps25hb(spi2, 1);
BMP280 bmp2(spi2, 2);
LSM303D accel(spi5, 1);
uint8_t gyro_wtm = 5;
uint8_t acc_wtm = 8;
TimeStamp console_update_time;
TimeStamp sample_dt;
TimeStamp led_toggle_ts;
FlightControl flight_ctl;
static bool print_to_console = false;
LowPassFilter<Vector3f, float> gyro_lpf({0.5});
LowPassFilter<Vector3f, float> acc_lpf_alt({0.9});
LowPassFilter<Vector3f, float> acc_lpf_att({0.990});
LowPassFilter<float, float> pressure_lpf({0.6});
attitudetracker att;
/*
* Apply the boot configuration from flash memory.
*/
dronestate_boot_config(*drone_state);
L3GD20Reader gyro_reader(gyro, GYRO_INT2_PIN, gyro_align);
LSM303Reader acc_reader(accel, ACC_INT2_PIN, acc_align);
UartRpcServer rpcserver(*drone_state, configdata, acc_reader.mag_calibrator_);
bmp2.set_oversamp_pressure(BMP280_OVERSAMP_16X);
bmp2.set_work_mode(BMP280_ULTRA_HIGH_RESOLUTION_MODE);
bmp2.set_filter(BMP280_FILTER_COEFF_OFF);
Bmp280Reader bmp_reader(bmp2);
HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 1, 1);
HAL_NVIC_EnableIRQ (DMA1_Stream6_IRQn);
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 1, 0);
HAL_NVIC_EnableIRQ (DMA1_Stream5_IRQn);
#ifndef ENABLE_UART_TASK
uart2.uart_dmarx_start();
#endif
printf("Priority Group: %lu\n", NVIC_GetPriorityGrouping());
printf("SysTick_IRQn priority: %lu\n", NVIC_GetPriority(SysTick_IRQn) << __NVIC_PRIO_BITS);
printf("configKERNEL_INTERRUPT_PRIORITY: %d\n", configKERNEL_INTERRUPT_PRIORITY);
printf("configMAX_SYSCALL_INTERRUPT_PRIORITY: %d\n", configMAX_SYSCALL_INTERRUPT_PRIORITY);
printf("LPS25HB Device id: %d\n", lps25hb.Get_DeviceID());
vTaskDelay(500 / portTICK_RATE_MS);
gyro_reader.init(gyro_wtm);
gyro_reader.enable_int2(false);
vTaskDelay(500 / portTICK_RATE_MS);
acc_reader.init(acc_wtm);
acc_reader.enable_int2(false);
acc_reader.mag_calibrator_.set_bias(drone_state->mag_bias_);
acc_reader.mag_calibrator_.set_scale_factor(drone_state->mag_scale_factor_);
vTaskDelay(500 / portTICK_RATE_MS);
printf("Calibrating...");
gyro_reader.enable_int2(true);
gyro_reader.calculate_static_bias_filtered(2400);
printf(" Done!\n");
flight_ctl.start_receiver();
printf("Entering main loop...\n");
gyro_reader.enable_int2(true);
sample_dt.time_stamp();
lps25hb.Set_FifoMode(LPS25HB_FIFO_STREAM_MODE);
lps25hb.Set_FifoModeUse(LPS25HB_ENABLE);
lps25hb.Set_Odr(LPS25HB_ODR_25HZ);
lps25hb.Set_Bdu(LPS25HB_BDU_NO_UPDATE);
LPS25HB_FIFOTypeDef_st fifo_config;
memset(&fifo_config, 0, sizeof(fifo_config));
lps25hb.Get_FifoConfig(&fifo_config);
#ifdef USE_LPS25HB
float base_pressure = lps25hb.Get_PressureHpa();
for (int i = 0; i < 100; i++) {
while (lps25hb.Get_FifoStatus().FIFO_EMPTY)
vTaskDelay(50 / portTICK_RATE_MS);
base_pressure = pressure_lpf.do_filter(lps25hb.Get_PressureHpa());
}
#endif
bmp_reader.calibrate();
// Infinite loop
PerfCounter idle_time;
while (1) {
drone_state->iteration_++;
if (drone_state->iteration_ % 120 == 0) {
led1.toggle();
}
if (drone_state->iteration_ % 4 == 0) {
#ifdef USE_LPS25HB
drone_state->temperature_ = lps25hb.Get_TemperatureCelsius();
while (!lps25hb.Get_FifoStatus().FIFO_EMPTY) {
drone_state->pressure_hpa_ = pressure_lpf.do_filter(lps25hb.Get_PressureHpa());
float alt = (powf(base_pressure/drone_state->pressure_hpa_, 0.1902f) - 1.0f) * ((lps25hb.Get_TemperatureCelsius()) + 273.15f)/0.0065;
drone_state->altitude_ = Distance::from_meters(alt);
}
#else
bmp_reader.pressure_filter_.set_alpha(drone_state->altitude_lpf_);
drone_state->altitude_ = bmp_reader.get_altitude(true);
drone_state->pressure_hpa_ = bmp_reader.get_pressure().hpa();
drone_state->temperature_ = bmp_reader.get_temperature(false).celsius();
#endif
}
idle_time.begin_measure();
gyro_reader.wait_for_data();
idle_time.end_measure();
drone_state->dt_ = sample_dt.elapsed();
sample_dt.time_stamp();
if (drone_state->base_throttle_ > 0.1)
att.accelerometer_correction_speed(drone_state->accelerometer_correction_speed_);
else
att.accelerometer_correction_speed(3.0f);
att.gyro_drift_pid(drone_state->gyro_drift_kp_, drone_state->gyro_drift_ki_, drone_state->gyro_drift_kd_);
att.gyro_drift_leak_rate(drone_state->gyro_drift_leak_rate_);
size_t fifosize = gyro_reader.size();
for (size_t i = 0; i < fifosize; i++)
drone_state->gyro_raw_ = gyro_lpf.do_filter(gyro_reader.read_sample());
if (drone_state->gyro_raw_.length_squared() > 0 && drone_state->dt_.microseconds() > 0) {
drone_state->gyro_ = (drone_state->gyro_raw_ - gyro_reader.bias()) * drone_state->gyro_factor_;
att.track_gyroscope(DEG2RAD(drone_state->gyro_) * 1.0f, drone_state->dt_.seconds_float());
}
fifosize = acc_reader.size();
for (size_t i = 0; i < fifosize; i++) {
Vector3f acc_sample = acc_reader.read_sample_acc();
acc_lpf_att.do_filter(acc_sample);
acc_lpf_alt.do_filter(acc_sample);
}
drone_state->accel_raw_ = acc_lpf_att.output();
drone_state->accel_alt_ = acc_lpf_alt.output();
drone_state->accel_ = (drone_state->accel_raw_ - drone_state->accelerometer_adjustment_).normalize();
#define ALLOW_ACCELEROMETER_OFF
#ifdef ALLOW_ACCELEROMETER_OFF
if (drone_state->track_accelerometer_) {
att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float());
}
#else
att.track_accelerometer(drone_state->accel_, drone_state->dt_.seconds_float());
#endif
#define REALTIME_DATA 0
#if REALTIME_DATA
std::cout << drone_state->gyro_.transpose() << drone_state->accel_.transpose() << drone_state->pid_torque_.transpose();
std::cout << drone_state->dt_.seconds_float() << std::endl;
#endif
drone_state->mag_raw_ = acc_reader.read_sample_mag();
drone_state->mag_ = drone_state->mag_raw_.normalize();
if (drone_state->track_magnetometer_) {
att.track_magnetometer(drone_state->mag_, drone_state->dt_.seconds_float());
}
drone_state->attitude_ = att.get_attitude();
drone_state->gyro_drift_error_ = RAD2DEG(att.get_drift_error());
flight_ctl.update_state(*drone_state);
flight_ctl.send_throttle_to_motors();
if (print_to_console && console_update_time.elapsed() > TimeSpan::from_milliseconds(300)) {
Vector3f drift_err = att.get_drift_error();
console_update_time.time_stamp();
printf("Gyro : %5.3f %5.3f %5.3f\n", drone_state->gyro_.at(0), drone_state->gyro_.at(1), drone_state->gyro_.at(2));
printf("Drift Err : %5.3f %5.3f %5.3f\n", RAD2DEG(drift_err.at(0)), RAD2DEG(drift_err.at(1)), RAD2DEG(drift_err.at(2)));
printf("Gyro Raw : %5.3f %5.3f %5.3f\n", drone_state->gyro_raw_.at(0), drone_state->gyro_raw_.at(1), drone_state->gyro_raw_.at(2));
printf("Accel : %5.3f %5.3f %5.3f\n", drone_state->accel_.at(0), drone_state->accel_.at(1), drone_state->accel_.at(2));
printf("Mag : %5.3f %5.3f %5.3f\n", drone_state->mag_.at(0), drone_state->mag_.at(1), drone_state->mag_.at(2));
printf("dT : %lu uSec\n", (uint32_t)drone_state->dt_.microseconds());
printf("Q : %5.3f %5.3f %5.3f %5.3f\n\n", drone_state->attitude_.w, drone_state->attitude_.x, drone_state->attitude_.y,
drone_state->attitude_.z);
#if 1
printf("Motors : %1.2f %1.2f %1.2f %1.2f\n", drone_state->motors_[0], drone_state->motors_[1],
drone_state->motors_[2], drone_state->motors_[3]);
printf("Throttle : %1.2f\n", drone_state->base_throttle_);
printf("Armed : %d\n", drone_state->motors_armed_);
printf("Altitude : %4.2f m\n", drone_state->altitude_.meters());
printf("GPS : Lon: %3.4f Lat: %3.4f Sat %lu Alt: %4.2f m\n",
drone_state->longitude_.degrees(), drone_state->latitude_.degrees(),
drone_state->satellite_count_, drone_state->gps_altitude_.meters());
printf("Battery : %2.1f V, %2.0f%%\n", drone_state->battery_voltage_.volts(), drone_state->battery_percentage_);
#endif
}
#if 0
if (led_toggle_ts.elapsed() > TimeSpan::from_seconds(1)) {
led_toggle_ts.time_stamp();
led0.toggle();
}
#endif
#ifndef ENABLE_UART_TASK
rpcserver.jsonrpc_request_handler(&uart2);
#endif
}
}
static int TwoWireIrqCtl(IRQn_Type IRQn, void(*ifunc)(void*), int cmd, void *param)
#endif
{
int rc = 0;
unsigned int *ival = (unsigned int *)param;
uint_fast8_t enabled = NVIC_GetEnableIRQ(IRQn);
/* Disable interrupt. */
if (enabled) {
NVIC_DisableIRQ(IRQn);
}
switch(cmd) {
case NUT_IRQCTL_INIT:
/* Set the vector. */
Cortex_RegisterInt(IRQn, ifunc);
/* Initialize Event IRQ with defined priority. */
NVIC_SetPriority(IRQn, NUT_IRQPRI_TWI);
/* Clear interrupt */
NVIC_ClearPendingIRQ(IRQn);
break;
case NUT_IRQCTL_STATUS:
if (enabled) {
*ival |= 1;
}
else {
*ival &= ~1;
}
break;
case NUT_IRQCTL_ENABLE:
enabled = 1;
break;
case NUT_IRQCTL_DISABLE:
enabled = 0;
break;
case NUT_IRQCTL_GETMODE:
*ival = NUT_IRQMODE_LEVEL;
break;
case NUT_IRQCTL_SETMODE:
break;
case NUT_IRQCTL_GETPRIO:
*ival = NVIC_GetPriority(IRQn);
break;
case NUT_IRQCTL_SETPRIO:
NVIC_SetPriority(IRQn, *ival);
break;
#ifdef NUT_PERFMON
case NUT_IRQCTL_GETCOUNT:
*ival = (unsigned int)ih->ir_count;
ih->ir_count = 0;
break;
#endif
default:
rc = -1;
break;
}
/* Enable interrupt. */
if (enabled) {
NVIC_EnableIRQ(IRQn);
}
return rc;
}
// The kernel is the central point in Smoothie : it stores modules, and handles event calls
Kernel::Kernel(){
// Value init for the arrays
for( uint8_t i=0; i<NUMBER_OF_DEFINED_EVENTS; i++ ){
for( uint8_t index=0; index<32; index++ ){
this->hooks[i][index] = NULL;
}
}
// Config first, because we need the baud_rate setting before we start serial
this->config = new Config();
// Serial second, because the other modules might want to say something
this->streams = new StreamOutputPool();
// Configure UART depending on MRI config
NVIC_SetPriorityGrouping(0);
/*
if (strstr(MRI_UART, "MRI_UART_MBED_USB")){
if (strstr(MRI_UART, "MRI_UART_SHARED")){
this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}else{
this->serial = new SerialConsole(p13, p14, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}
}else{
this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}
*/
if( NVIC_GetPriority(UART0_IRQn) > 0 ){
this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}else{
this->serial = new SerialConsole(p13, p14, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}
this->add_module( this->config );
this->add_module( this->serial );
// HAL stuff
this->slow_ticker = new SlowTicker();
this->step_ticker = new StepTicker();
this->adc = new Adc();
this->digipot = new Digipot();
// LPC17xx-specific
NVIC_SetPriorityGrouping(0);
NVIC_SetPriority(TIMER0_IRQn, 2);
NVIC_SetPriority(TIMER1_IRQn, 1);
NVIC_SetPriority(TIMER2_IRQn, 3);
// Set other priorities lower than the timers
NVIC_SetPriority(ADC_IRQn, 4);
NVIC_SetPriority(USB_IRQn, 4);
// If MRI is enabled
if( MRI_ENABLE ){
if( NVIC_GetPriority(UART0_IRQn) > 0 ){ NVIC_SetPriority(UART0_IRQn, 4); }
if( NVIC_GetPriority(UART1_IRQn) > 0 ){ NVIC_SetPriority(UART1_IRQn, 4); }
if( NVIC_GetPriority(UART2_IRQn) > 0 ){ NVIC_SetPriority(UART2_IRQn, 4); }
if( NVIC_GetPriority(UART3_IRQn) > 0 ){ NVIC_SetPriority(UART3_IRQn, 4); }
}else{
NVIC_SetPriority(UART0_IRQn, 4);
NVIC_SetPriority(UART1_IRQn, 4);
NVIC_SetPriority(UART2_IRQn, 4);
NVIC_SetPriority(UART3_IRQn, 4);
}
// Configure the step ticker
int base_stepping_frequency = this->config->value(base_stepping_frequency_checksum )->by_default(100000)->as_number();
double microseconds_per_step_pulse = this->config->value(microseconds_per_step_pulse_checksum )->by_default(5 )->as_number();
this->step_ticker->set_reset_delay( microseconds_per_step_pulse / 1000000L );
this->step_ticker->set_frequency( base_stepping_frequency );
// Core modules
this->add_module( this->gcode_dispatch = new GcodeDispatch() );
this->add_module( this->robot = new Robot() );
this->add_module( this->stepper = new Stepper() );
this->add_module( this->planner = new Planner() );
this->add_module( this->player = new Player() );
this->add_module( this->pauser = new Pauser() );
}
void HAL_Interrupts_Attach(uint16_t pin, HAL_InterruptHandler handler, void* data, InterruptMode mode, HAL_InterruptExtraConfiguration* config)
{
uint8_t GPIO_PortSource = 0; //variable to hold the port number
//EXTI structure to init EXT
EXTI_InitTypeDef EXTI_InitStructure = {0};
//NVIC structure to set up NVIC controller
NVIC_InitTypeDef NVIC_InitStructure = {0};
//Map the Spark pin to the appropriate port and pin on the STM32
STM32_Pin_Info* PIN_MAP = HAL_Pin_Map();
GPIO_TypeDef *gpio_port = PIN_MAP[pin].gpio_peripheral;
uint16_t gpio_pin = PIN_MAP[pin].gpio_pin;
uint8_t GPIO_PinSource = PIN_MAP[pin].gpio_pin_source;
//Clear pending EXTI interrupt flag for the selected pin
EXTI_ClearITPendingBit(gpio_pin);
//Select the port source
if (gpio_port == GPIOA)
{
GPIO_PortSource = 0;
}
else if (gpio_port == GPIOB)
{
GPIO_PortSource = 1;
}
else if (gpio_port == GPIOC)
{
GPIO_PortSource = 2;
}
else if (gpio_port == GPIOD)
{
GPIO_PortSource = 3;
}
// Register the handler for the user function name
if (config && config->version >= HAL_INTERRUPT_EXTRA_CONFIGURATION_VERSION_2 && config->keepHandler) {
// keep the old handler
} else {
exti_channels[GPIO_PinSource].fn = handler;
exti_channels[GPIO_PinSource].data = data;
}
//Connect EXTI Line to appropriate Pin
SYSCFG_EXTILineConfig(GPIO_PortSource, GPIO_PinSource);
//Configure GPIO EXTI line
EXTI_InitStructure.EXTI_Line = gpio_pin;//EXTI_Line;
//select the interrupt mode
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
switch (mode)
{
//case LOW:
//There is no LOW mode in STM32, so using falling edge as default
//EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
//break;
case CHANGE:
//generate interrupt on rising or falling edge
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
break;
case RISING:
//generate interrupt on rising edge
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
break;
case FALLING:
//generate interrupt on falling edge
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling;
break;
}
//enable EXTI line
EXTI_InitStructure.EXTI_LineCmd = ENABLE;
//send values to registers
EXTI_Init(&EXTI_InitStructure);
//configure NVIC
//select NVIC channel to configure
NVIC_InitStructure.NVIC_IRQChannel = GPIO_IRQn[GPIO_PinSource];
if (config == NULL) {
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 14;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
} else {
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = config->IRQChannelPreemptionPriority;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = config->IRQChannelSubPriority;
// Keep the same priority
if (config->version >= HAL_INTERRUPT_EXTRA_CONFIGURATION_VERSION_2) {
if (config->keepPriority) {
uint32_t priorityGroup = NVIC_GetPriorityGrouping();
uint32_t priority = NVIC_GetPriority(NVIC_InitStructure.NVIC_IRQChannel);
uint32_t p, sp;
NVIC_DecodePriority(priority, priorityGroup, &p, &sp);
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = p;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = sp;
}
}
}
//enable IRQ channel
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
//update NVIC registers
NVIC_Init(&NVIC_InitStructure);
}
// The kernel is the central point in Smoothie : it stores modules, and handles event calls
Kernel::Kernel(){
instance= this; // setup the Singleton instance of the kernel
// Config first, because we need the baud_rate setting before we start serial
this->config = new Config();
// Serial second, because the other modules might want to say something
this->streams = new StreamOutputPool();
// Configure UART depending on MRI config
// If MRI is using UART0, we want to use UART1, otherwise, we want to use UART0. This makes it easy to use only one UART for both debug and actual commands.
NVIC_SetPriorityGrouping(0);
if( !isDebugMonitorUsingUart0() ){
this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}else{
this->serial = new SerialConsole(p13, p14, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}
this->add_module( this->config );
this->add_module( this->serial );
// HAL stuff
add_module( this->slow_ticker = new SlowTicker());
this->step_ticker = new StepTicker();
this->adc = new Adc();
// TODO : These should go into platform-specific files
// LPC17xx-specific
NVIC_SetPriorityGrouping(0);
NVIC_SetPriority(TIMER0_IRQn, 2);
NVIC_SetPriority(TIMER1_IRQn, 1);
NVIC_SetPriority(TIMER2_IRQn, 3);
// Set other priorities lower than the timers
NVIC_SetPriority(ADC_IRQn, 4);
NVIC_SetPriority(USB_IRQn, 4);
// If MRI is enabled
if( MRI_ENABLE ){
if( NVIC_GetPriority(UART0_IRQn) > 0 ){ NVIC_SetPriority(UART0_IRQn, 4); }
if( NVIC_GetPriority(UART1_IRQn) > 0 ){ NVIC_SetPriority(UART1_IRQn, 4); }
if( NVIC_GetPriority(UART2_IRQn) > 0 ){ NVIC_SetPriority(UART2_IRQn, 4); }
if( NVIC_GetPriority(UART3_IRQn) > 0 ){ NVIC_SetPriority(UART3_IRQn, 4); }
}else{
NVIC_SetPriority(UART0_IRQn, 4);
NVIC_SetPriority(UART1_IRQn, 4);
NVIC_SetPriority(UART2_IRQn, 4);
NVIC_SetPriority(UART3_IRQn, 4);
}
// Configure the step ticker
int base_stepping_frequency = this->config->value(base_stepping_frequency_checksum )->by_default(100000)->as_number();
double microseconds_per_step_pulse = this->config->value(microseconds_per_step_pulse_checksum )->by_default(5 )->as_number();
// Configure the step ticker ( TODO : shouldnt this go into stepticker's code ? )
this->step_ticker->set_reset_delay( microseconds_per_step_pulse / 1000000L );
this->step_ticker->set_frequency( base_stepping_frequency );
// Core modules
this->add_module( this->gcode_dispatch = new GcodeDispatch() );
this->add_module( this->robot = new Robot() );
this->add_module( this->stepper = new Stepper() );
this->add_module( this->planner = new Planner() );
this->add_module( this->conveyor = new Conveyor() );
this->add_module( this->pauser = new Pauser() );
this->add_module( this->public_data = new PublicData() );
}
/**
* @brief Gets the priority of an interrupt.
* @param IRQn: External interrupt number
* This parameter can be an enumerator of IRQn_Type enumeration
* (For the complete STM32 Devices IRQ Channels list, please refer to the appropriate CMSIS device file (stm32f0xxxx.h))
* @retval None
*/
uint32_t HAL_NVIC_GetPriority(IRQn_Type IRQn)
{
/* Get priority for Cortex-M system or device specific interrupts */
return NVIC_GetPriority(IRQn);
}
/*!
* \brief RTC interrupt control.
*
* \param cmd Control command.
* - NUT_IRQCTL_INIT Initialize and disable interrupt.
* - NUT_IRQCTL_STATUS Query interrupt status.
* - NUT_IRQCTL_ENABLE Enable interrupt.
* - NUT_IRQCTL_DISABLE Disable interrupt.
* - NUT_IRQCTL_GETMODE Query interrupt mode.
* - NUT_IRQCTL_SETMODE Set interrupt mode (NUT_IRQMODE_LEVEL or NUT_IRQMODE_EDGE).
* - NUT_IRQCTL_GETPRIO Query interrupt priority.
* - NUT_IRQCTL_SETPRIO Set interrupt priority.
* - NUT_IRQCTL_GETCOUNT Query and clear interrupt counter.
* \param param Pointer to optional parameter.
*
* \return 0 on success, -1 otherwise.
*/
static int RtcIrqCtl(int cmd, void *param)
{
int rc = 0;
uint32_t *ival = (uint32_t *)param;
int enabled = NVIC_GetEnableIRQ(RTC_IRQn);
/* Disable interrupt. */
if (enabled) {
NVIC_DisableIRQ(RTC_IRQn);
}
switch(cmd) {
case NUT_IRQCTL_INIT:
/* Set the vector. */
Cortex_RegisterInt(RTC_IRQn, RtcIrqEntry);
/* Initialize with defined priority. */
NVIC_SetPriority(RTC_IRQn, NUT_IRQPRI_RTC);
/* Clear interrupt */
NVIC_ClearPendingIRQ(RTC_IRQn);
break;
case NUT_IRQCTL_STATUS:
if (enabled) {
*ival |= 1;
}
else {
*ival &= ~1;
}
break;
case NUT_IRQCTL_ENABLE:
enabled = 1;
break;
case NUT_IRQCTL_DISABLE:
enabled = 0;
break;
case NUT_IRQCTL_GETMODE:
*ival = NUT_IRQMODE_EDGE;
break;
case NUT_IRQCTL_SETMODE:
rc = -1;
break;
case NUT_IRQCTL_GETPRIO:
*ival = NVIC_GetPriority(RTC_IRQn);
break;
case NUT_IRQCTL_SETPRIO:
NVIC_SetPriority(RTC_IRQn, *ival);
break;
#ifdef NUT_PERFMON
case NUT_IRQCTL_GETCOUNT:
*ival = (uint32_t)sig_RTC.ir_count;
sig_RTC.ir_count = 0;
break;
#endif
default:
rc = -1;
break;
}
/* Enable interrupt. */
if (enabled) {
NVIC_EnableIRQ(RTC_IRQn);
}
return rc;
}
/*__STATIC_INLINE*/ uint32_t CMSIS_STUB_NVIC_GetPriority(IRQn_Type IRQn)
{
return NVIC_GetPriority(IRQn);
}
// The kernel is the central point in Smoothie : it stores modules, and handles event calls
Kernel::Kernel() {
// Config first, because we need the baud_rate setting before we start serial
this->config = new Config();
// Serial second, because the other modules might want to say something
this->streams = new StreamOutputPool();
// Configure UART depending on MRI config
NVIC_SetPriorityGrouping(0);
if( !isDebugMonitorUsingUart0() ) {
this->serial = new SerialConsole(USBTX, USBRX, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
} else {
this->serial = new SerialConsole(p13, p14, this->config->value(uart0_checksum,baud_rate_setting_checksum)->by_default(9600)->as_number());
}
this->add_module( this->config );
this->add_module( this->serial );
// HAL stuff
add_module( this->slow_ticker = new SlowTicker());
this->step_ticker = new StepTicker();
this->adc = new Adc();
// LPC17xx-specific
NVIC_SetPriorityGrouping(0);
NVIC_SetPriority(TIMER0_IRQn, 2);
NVIC_SetPriority(TIMER1_IRQn, 1);
NVIC_SetPriority(TIMER2_IRQn, 3);
// Set other priorities lower than the timers
NVIC_SetPriority(ADC_IRQn, 4);
NVIC_SetPriority(USB_IRQn, 4);
// If MRI is enabled
if( MRI_ENABLE ) {
if( NVIC_GetPriority(UART0_IRQn) > 0 ) {
NVIC_SetPriority(UART0_IRQn, 4);
}
if( NVIC_GetPriority(UART1_IRQn) > 0 ) {
NVIC_SetPriority(UART1_IRQn, 4);
}
if( NVIC_GetPriority(UART2_IRQn) > 0 ) {
NVIC_SetPriority(UART2_IRQn, 4);
}
if( NVIC_GetPriority(UART3_IRQn) > 0 ) {
NVIC_SetPriority(UART3_IRQn, 4);
}
} else {
NVIC_SetPriority(UART0_IRQn, 4);
NVIC_SetPriority(UART1_IRQn, 4);
NVIC_SetPriority(UART2_IRQn, 4);
NVIC_SetPriority(UART3_IRQn, 4);
}
// Configure the step ticker
int base_stepping_frequency = this->config->value(base_stepping_frequency_checksum )->by_default(100000)->as_number();
double microseconds_per_step_pulse = this->config->value(microseconds_per_step_pulse_checksum )->by_default(5 )->as_number();
this->step_ticker->set_reset_delay( microseconds_per_step_pulse / 1000000L );
this->step_ticker->set_frequency( base_stepping_frequency );
// Core modules
this->add_module( this->gcode_dispatch = new GcodeDispatch() );
this->add_module( this->robot = new Robot() );
this->add_module( this->stepper = new Stepper() );
this->add_module( this->planner = new Planner() );
this->add_module( this->conveyor = new Conveyor() );
this->add_module( this->pauser = new Pauser() );
}