/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
chEvtInit(&eventImuIrq);
chEvtInit(&eventMagnIrq);
chEvtInit(&eventImuRead);
chEvtInit(&eventMagnRead);
chEvtInit(&eventEKFDone);
palSetPadMode(GPIOB, 3, PAL_MODE_OUTPUT_PUSHPULL); // BLUE
palSetPadMode(GPIOB, 4, PAL_MODE_OUTPUT_PUSHPULL); // GREEN
palSetPadMode(GPIOB, 5, PAL_MODE_OUTPUT_PUSHPULL); // RED
chThdCreateStatic(waThreadLed, sizeof(waThreadLed), NORMALPRIO, ThreadLed, NULL );
I2CInitLocal();
configInit();
mavlinkInit();
initSensors();
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE);
TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = 84 - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure);
TIM_Cmd(TIM5, ENABLE);
startEstimation();
startSensors();
radioInit();
motorsInit();
extStart(&EXTD1, &extcfg);
extChannelEnable(&EXTD1, 0);
extChannelEnable(&EXTD1, 1);
chEvtBroadcastFlags(&eventEKFDone, EVT_EKF_DONE);
while (TRUE) {
chThdSleepMilliseconds(1000);
}
}
void transmitter_receiver_init(void) {
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
RCC_ClocksTypeDef RCC_Clocks;
uint16_t prescaler = 0;
last_update_time = 0;
for (int i = 0; i < RECEIVER_CHANNELS; i++) {
palSetPadMode(transmitter_receiver_channel[i].port,
transmitter_receiver_channel[i].pin, PAL_STM32_MODE_INPUT);
extChannelEnable(&EXTD1, transmitter_receiver_channel[i].pin);
}
// Timer
RCC_GetClocksFreq(&RCC_Clocks);
prescaler = (uint16_t) ((RCC_Clocks.SYSCLK_Frequency / 2) / RECEIVER_TIMER_FREQUENCY) - 1;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
// Time base configuration
TIM_TimeBaseStructure.TIM_Period = 65535;
TIM_TimeBaseStructure.TIM_Prescaler = prescaler;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
TIM_Cmd(TIM3, ENABLE);
}
/**
* @brief Starts the interrupts and timers so sensors will be
* serviced.
*
* @param[in] srdp Pointer to the SensorReadDriver object.
*
* @return The operation status.
* @retval MSG_OK The enabling of interrupts and starting of the
* virtual timers was successful.
* @retval MSG_RESET The driver was not in the correct state.
*
* @api
*/
msg_t SensorReadStart(SensorReadDriver *srdp)
{
size_t i;
chDbgCheck(srdp != NULL);
if (srdp->state == SRD_STOPPED)
{
/* Enable interrupts for interrupt driven sensors */
for (i = 0; i < srdp->interrupt_sensor_cnt; i++)
extChannelEnable(&SRD_EXT_DRIVER,
srdp->interrupt_sensor_ptr[i].interrupt_channel);
/* Enable timers for polled driven sensors */
for (i = 0; i < srdp->polled_sensor_cnt; i++)
/* The parameter sent to the VT is the pointer to the corresponding
polled_sensor_t structure to be able to access it from the
callback function */
chVTSet(srdp->polled_sensor_ptr[i].polling_vt,
CH_CFG_ST_FREQUENCY / srdp->polled_sensor_ptr[i].frequency_hz,
sensors_polled_callback,
(void *)&srdp->polled_sensor_ptr[i]);
/* Everything OK, transverse the state */
srdp->state = SRD_STARTED;
return MSG_OK;
}
else
return MSG_RESET;
}
void ExtiPnc::pps(bool flag){
osalDbgCheck(ready);
#if defined(BOARD_BEZVODIATEL)
if (flag)
extChannelEnable(&EXTD1, GPIOA_GPS_PPS);
else
extChannelDisable(&EXTD1, GPIOA_GPS_PPS);
#elif defined(BOARD_MNU)
if (flag)
extChannelEnable(&EXTD1, GPIOB_PPS4);
else
extChannelDisable(&EXTD1, GPIOB_PPS4);
#else
#error "Unknown board"
#endif
}
/**
* Enables interrupts from MPU
*/
void ExtiPnc::mpu6050(bool flag){
osalDbgCheck(ready);
#if defined(BOARD_BEZVODIATEL)
if (flag)
extChannelEnable(&EXTD1, GPIOE_MPU9150_INT);
else
extChannelDisable(&EXTD1, GPIOE_MPU9150_INT);
#elif defined(BOARD_MNU)
if (flag)
extChannelEnable(&EXTD1, GPIOG_MPU9150_INT);
else
extChannelDisable(&EXTD1, GPIOG_MPU9150_INT);
#else
#error "Unknown board"
#endif
}
/*
* Application entry point.
*/
int main(void) {
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activates the EXT driver 1.
*/
extStart(&EXTD1, &extcfg);
/*
* Normal main() thread activity, in this demo it enables and disables the
* button EXT channel using 5 seconds intervals.
*/
while (TRUE) {
chThdSleepMilliseconds(5000);
extChannelDisable(&EXTD1, 0);
chThdSleepMilliseconds(5000);
extChannelEnable(&EXTD1, 0);
}
}
int main(void) {
halInit();
chSysInit();
uint8_t i;
event_listener_t tel;
// Serial Port Setup
sdStart(&SD1, NULL);
palSetPadMode(GPIOC, 4, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPIOC, 5, PAL_MODE_ALTERNATE(7));
chprintf((BaseSequentialStream*)&SD1, "Up and Running\n\r");
palSetPadMode(GPIOB, 3, PAL_MODE_ALTERNATE(6)); /* SCK. */
palSetPadMode(GPIOB, 4, PAL_MODE_ALTERNATE(6)); /* MISO.*/
palSetPadMode(GPIOB, 5, PAL_MODE_ALTERNATE(6)); /* MOSI.*/
palSetPadMode(GPIOC, GPIOC_PIN1, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad(GPIOC, GPIOC_PIN1);
palSetPadMode(GPIOC, GPIOC_PIN2, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(GPIOC, GPIOC_PIN2);
palSetPadMode(GPIOC, GPIOC_PIN3, PAL_MODE_INPUT_PULLUP);
spiStart(&SPID3, &nrf24l01SPI);
chMtxObjectInit(&nrfMutex);
//FROM RX---
extStart(&EXTD1, &extcfg);
//---
nrf24l01ObjectInit(&nrf24l01);
nrf24l01Start(&nrf24l01, &nrf24l01Config);
//FROM RX ---
extChannelEnable(&EXTD1, 3);
//-----
initNRF24L01(&nrf24l01);
chprintf((BaseSequentialStream*)&SD1, "\n\rUp and Running\n\r");
shellInit();
chEvtRegister(&shell_terminated, &tel, 0);
shelltp1 = shellCreate(&shell_cfg1, sizeof(waShell), NORMALPRIO);
//FROM RX---
chThdCreateStatic(recieverWorkingArea, sizeof(recieverWorkingArea), NORMALPRIO, receiverThread, NULL);
//FROM RX^^^^
/*
for (i=0;i<32;i++) {
serialOutBuf[i] = 3;
}
*/
for (;;) {
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
}
}
void
vexSonarStart( tVexSonarChannel channel )
{
if( vexSonars[channel].flags & SONAR_ENABLED ) {
// start interrupts
extChannelEnable( &EXTD1, vexSonars[channel].pb_pad );
}
}
/** Set up NAP GPIO interrupt.
* Interrupt alerts STM that a channel in NAP needs to be serviced.
*/
void nap_exti_setup(void)
{
/* Signal from the FPGA is on PA1. */
extStart(&EXTD1, &extconfig);
extChannelEnable(&EXTD1, 1);
/* Enable EXTI1 interrupt */
chThdCreateStatic(wa_nap_exti, sizeof(wa_nap_exti), HIGHPRIO-1, nap_exti_thread, NULL);
}
void
vexDigitalIntrRun()
{
int16_t pin;
for(pin=0;pin<kVexDigital_Num;pin++)
{
if(vexioDefinition[pin].intrCount == 0)
extChannelEnable( &EXTD1, vexioDefinition[pin].pad );
}
}
Thread *gyroRun(SPIDriver *spip, tprio_t prio) {
Thread *tp;
init_l3g4200d(spip);
tp = chThdCreateFromHeap(NULL, GYRO_WA_SIZE, prio, l3g4200d_update_thread,
(void*)spip);
extChannelEnable(&EXTD1, GYRO_INT2);
l3g4200d_update(spip);
return tp;
}
Thread *magRun(SPIDriver *spip, tprio_t prio) {
Thread *tp;
// lsm303_mag_init(spip);
chThdSleepMilliseconds(200);
lsm303_mag_update(spip);
tp = chThdCreateFromHeap(NULL, MAG_WA_SIZE, prio, lsm303_mag_update_thread, (void*) spip);
extChannelEnable(&EXTD1, GPIOA_AM_INT2);
return tp;
}
Thread *magRun(I2CDriver *i2cp, tprio_t prio) {
Thread *tp;
lsm303_mag_init(i2cp);
chThdSleepMilliseconds(200);
lsm303_mag_update(i2cp);
tp = chThdCreateFromHeap(NULL, MAG_WA_SIZE, prio, lsm303_mag_update_thread,
(void*)i2cp);
extChannelEnable(&EXTD1, AM_DRDY);
return tp;
}
Thread *accRun(SPIDriver *spip, tprio_t prio) {
Thread *tp;
if (lsm303_init(spip) != 0) chSysHalt();
chThdSleepMilliseconds(200);
lsm303_acc_update(spip);
tp = chThdCreateFromHeap(NULL, ACC_WA_SIZE, prio, lsm303_acc_update_thread, (void*) spip);
extChannelEnable(&EXTD1, GPIOA_AM_INT1);
return tp;
}
void ExtiPnc::bmp085(bool flag) {
osalDbgCheck(ready);
#if defined(BOARD_BEZVODIATEL)
if (flag)
extChannelEnable(&EXTD1, GPIOE_BMP085_EOC);
else
extChannelDisable(&EXTD1, GPIOE_BMP085_EOC);
#elif defined(BOARD_MNU)
(void)flag;
#else
#error "Unknown board"
#endif
}
//TODO: Ideally should have an EXTDriver as an argument, but there's no good
// way to dynamically have an EXTConfig for each one.
void extAddCallback(const struct pin * pin, uint32_t mode, extcallback_t cb){
EXTDriver *EXTD = &EXTD1;
//If an extconfig already exists, copy it over to our struct
if(EXTD->config != NULL && EXTD->config != &extconfig){
int i;
for(i = 0; i < EXT_MAX_CHANNELS; ++i){
extconfig.channels[i].cb = EXTD->config->channels[i].cb;
extconfig.channels[i].mode = EXTD->config->channels[i].mode;
}
}
uint32_t CHANNEL_MODE = 0;
if(pin->port == GPIOA){
CHANNEL_MODE = EXT_MODE_GPIOA;
} else if(pin->port == GPIOB){
CHANNEL_MODE = EXT_MODE_GPIOB;
} else if(pin->port == GPIOC){
CHANNEL_MODE = EXT_MODE_GPIOC;
} else if(pin->port == GPIOD){
CHANNEL_MODE = EXT_MODE_GPIOD;
} else if(pin->port == GPIOE){
CHANNEL_MODE = EXT_MODE_GPIOE;
} else if(pin->port == GPIOF){
CHANNEL_MODE = EXT_MODE_GPIOF;
} else if(pin->port == GPIOG){
CHANNEL_MODE = EXT_MODE_GPIOG;
} else if(pin->port == GPIOH){
CHANNEL_MODE = EXT_MODE_GPIOH;
} else if(pin->port == GPIOI){
CHANNEL_MODE = EXT_MODE_GPIOI;
}
//TODO: warn if overwriting?
extconfig.channels[pin->pad].cb = cb;
extconfig.channels[pin->pad].mode = (mode & ~EXT_MODE_GPIO_MASK) | CHANNEL_MODE;
if(EXTD->state == EXT_ACTIVE && (mode & EXT_CH_MODE_AUTOSTART)){
extChannelEnable(EXTD, pin->pad);
}
}
/**
* Enables interrupts from PPS from GPS receiver
*/
void ExtiEnablePps(void){
extChannelEnable(&EXTD1, 0);
}
/***********************************************************************************
* @fn halRfEnableRxInterrupt
*
* @brief Enable RX interrupt.
*
* @param none
*
* @return none
*/
void halRfEnableRxInterrupt(void) {
extChannelEnable(&EXTD1, 7);
}
static void ready_isr_enable(void) {
extChannelEnable(&EXTD1, GPIOD_NAND_RB_NWAIT);
}
void ExtiPnc::start(void){
extStart(&EXTD1, &extcfg);
extChannelEnable(&EXTD1, GPIOH_FPGA_IO11);
ready = true;
}
