void BoardInitMcu( void )
{
Gpio_t ioPin;
if( McuInitialized == false )
{
HAL_Init( );
// LEDs
GpioInit( &Led1, LED_1, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 1 );
GpioInit( &Led2, LED_2, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 1 );
GpioInit( &Led3, LED_3, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, Led3Status );
SystemClockConfig( );
GpioInit( &ioPin, UART_RX, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
if( GpioRead( &ioPin ) == 1 ) // Debug Mode
{
UsbIsConnected = true;
FifoInit( &Uart1.FifoTx, UartTxBuffer, UART_FIFO_TX_SIZE );
FifoInit( &Uart1.FifoRx, UartRxBuffer, UART_FIFO_RX_SIZE );
// Configure your terminal for 8 Bits data (7 data bit + 1 parity bit), no parity and no flow ctrl
UartInit( &Uart1, UART_1, UART_TX, UART_RX );
UartConfig( &Uart1, RX_TX, 115200, UART_8_BIT, UART_1_STOP_BIT, NO_PARITY, NO_FLOW_CTRL );
}
else
{
UsbIsConnected = false;
UartDeInit( &Uart1 );
}
RtcInit( );
BoardUnusedIoInit( );
}
else
{
SystemClockReConfig( );
}
I2cInit( &I2c, I2C_SCL, I2C_SDA );
AdcInit( &Adc, BAT_LEVEL_PIN );
SpiInit( &SX1272.Spi, RADIO_MOSI, RADIO_MISO, RADIO_SCLK, NC );
SX1272IoInit( );
if( McuInitialized == false )
{
McuInitialized = true;
if( GetBoardPowerSource( ) == BATTERY_POWER )
{
CalibrateSystemWakeupTime( );
}
}
}
void BoardInitMcu( void )
{
if ( McuInitialized == false ) {
/* Initialize low level components */
low_level_init();
/*! SPI channel to be used by Semtech SX1276 */
#if defined(SX1276_BOARD_EMBED)
SpiInit(&SX1276.Spi, RADIO_MOSI, RADIO_MISO, RADIO_SCLK, NC);
SX1276IoInit();
#endif
#if defined (USE_USB_CDC)
UartInit( &UartUsb, UART_USB_CDC, NC, NC );
UartConfig( &UartUsb, RX_TX, 115200, UART_8_BIT, UART_1_STOP_BIT, NO_PARITY, NO_FLOW_CTRL );
TimerSetLowPowerEnable(false);
#elif defined(DEBUG)
#if defined(USE_SHELL)
Shell_Init();
#else
#if !defined(USE_CUSTOM_UART_HAL)
FifoInit(&Uart1.FifoRx, DbgRxBuffer, DBG_FIFO_RX_SIZE);
FifoInit(&Uart1.FifoTx, DbgTxBuffer, DBG_FIFO_TX_SIZE);
#endif
UartInit(&Uart1, UART_1, UART1_TX, UART1_RX);
UartConfig(&Uart1, RX_TX, 115200, UART_8_BIT, UART_1_STOP_BIT, NO_PARITY,
NO_FLOW_CTRL);
#endif
DbgConsole_Init(&Uart1);
TimerSetLowPowerEnable(false);
#elif( LOW_POWER_MODE_ENABLE )
TimerSetLowPowerEnable(true);
#else
TimerSetLowPowerEnable(false);
#endif
BoardUnusedIoInit();
#if !defined(USE_FREE_RTOS)
if ( TimerGetLowPowerEnable() == true ) {
RtcInit();
} else {
TimerHwInit();
}
#endif /* USE_FREE_RTOS */
McuInitialized = true;
}
}
void GpsMcuInit( void )
{
NmeaStringSize = 0;
//FifoInit( &Uart1.FifoTx, TxBuffer, FIFO_TX_SIZE );
FifoInit( &Uart1.FifoRx, RxBuffer, FIFO_RX_SIZE );
Uart1.IrqNotify = GpsMcuIrqNotify;
//GpioWrite( &GpsPowerEn, 1 ); // power down the GPS
GpioWrite( &GpsPowerEn, 0 ); // power up the GPS
GpioInit( &GpsPps, GPS_PPS, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
GpioSetInterrupt( &GpsPps, IRQ_FALLING_EDGE, IRQ_VERY_LOW_PRIORITY, &GpsMcuOnPpsSignal );
}
void SamplerInitOff(void)
{
// Chained interrupt off
SAMPLER_INTERRUPT_IE = 1;
// Block sampler from getting new data
status.streaming = 0;
// Empty fifo so streamer tasks finds no outgoing data
FifoInit(&streamerFifo, 1, STREAM_OUT_BUFFER_SIZE, streamerOutBuffer);
// Turn off the timebase
SysTimeAddRateCB(SamplerTrigger,0);
sampleCount = 0;
// sampleTicks = 0; // Left running
// Power off sensors
MultiStandby();
}
void GpsMcuInit( void )
{
NmeaStringSize = 0;
PpsTrigger = PpsTriggerIsFalling;
GpioInit( &GpsPowerEn, GPS_POWER_ON, PIN_OUTPUT, PIN_PUSH_PULL, PIN_NO_PULL, 1 );
GpioInit( &GpsPps, GPS_PPS, PIN_INPUT, PIN_PUSH_PULL, PIN_NO_PULL, 0 );
GpioSetInterrupt( &GpsPps, IRQ_FALLING_EDGE, IRQ_VERY_LOW_PRIORITY, &GpsMcuOnPpsSignal );
FifoInit( &Uart1.FifoRx, RxBuffer, FIFO_RX_SIZE );
Uart1.IrqNotify = GpsMcuIrqNotify;
GpsMcuStart( );
}
void SamplerInitOn(void)
{
// Fifos
FifoInit(&streamerFifo, 1, STREAM_OUT_BUFFER_SIZE, streamerOutBuffer);
// Turn on sensors
MultiStartup(&settings);
// Setup chained interrupts
SAMPLER_INTERRUPT_IP = SAMPLER_INT_PRIORITY;
SAMPLER_INTERRUPT_IF = 0;
SAMPLER_INTERRUPT_IE = 1;
// Turn on time base
sampleCount = 0;
sampleTicks = SysTimeTicks();
SysTimeAddRateCB(SamplerTrigger,settings.sampleRate);
// The sensors will begin filling the fifos if status.streaming is set
}
void ControlInit(void) {
control.reset = 1;
control.status_bits = 0;
control.speeds.angular_speed = 0,
control.speeds.linear_speed = 0;
control.last_finished_id = 0;
control.max_acc = ACC_MAX;
control.max_spd = SPD_MAX;
control.rot_spd_ratio = RATIO_ROT_SPD_MAX;
MotorsInit();
RobotStateInit();
FifoInit();
PIDInit(&PID_left);
PIDInit(&PID_right);
PIDSet(&PID_left, LEFT_P, LEFT_I, LEFT_D, LEFT_BIAS);
PIDSet(&PID_right, RIGHT_P, RIGHT_I, RIGHT_D, RIGHT_BIAS);
}
void UartUsbConfig( Uart_t *obj, UartMode_t mode, uint32_t baudrate, WordLength_t wordLength, StopBits_t stopBits, Parity_t parity, FlowCtrl_t flowCtrl )
{
FifoInit( &obj->FifoTx, CdcTxBuffer, FIFO_RX_SIZE );
FifoInit( &obj->FifoRx, CdcRxBuffer, FIFO_RX_SIZE );
}
void CDC_Set_Uart_Obj( Uart_t *obj )
{
UartObj = obj;
FifoInit( &obj->FifoRx, FifoRxBuffer, FIFO_RX_SIZE );
}
// Initialize the data stream, specifying the storage buffer (capacity * elementSize)
void DataStreamInit(datastream_t *dataStream, size_t elementSize, unsigned int capacity, void *buffer)
{
FifoInit(&dataStream->fifo, elementSize, capacity, buffer);
DataStreamClear(dataStream);
}
