/**
* @brief Initializes the objects factory.
*
* @init
*/
void _factory_init(void) {
#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
chMtxObjectInit(&ch_factory.mtx);
#else
chSemObjectInit(&ch_factory.sem, (cnt_t)1);
#endif
#if CH_CFG_FACTORY_OBJECTS_REGISTRY == TRUE
dyn_list_init(&ch_factory.obj_list);
chPoolObjectInit(&ch_factory.obj_pool,
sizeof (registered_object_t),
chCoreAllocAlignedI);
#endif
#if CH_CFG_FACTORY_GENERIC_BUFFERS == TRUE
dyn_list_init(&ch_factory.buf_list);
#endif
#if CH_CFG_FACTORY_SEMAPHORES == TRUE
dyn_list_init(&ch_factory.sem_list);
chPoolObjectInit(&ch_factory.sem_pool,
sizeof (dyn_semaphore_t),
chCoreAllocAlignedI);
#endif
#if CH_CFG_FACTORY_MAILBOXES == TRUE
dyn_list_init(&ch_factory.mbx_list);
#endif
#if CH_CFG_FACTORY_OBJ_FIFOS == TRUE
dyn_list_init(&ch_factory.fifo_list);
#endif
}
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 comm_usb_init(void) {
comm_usb_serial_init();
packet_init(send_packet, process_packet, PACKET_HANDLER);
chMtxObjectInit(&send_mutex);
// Threads
chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL);
chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL);
}
/*
* Initialize command module
*/
void sc_cmd_init(void)
{
uint8_t i;
chMtxObjectInit(&blob_mtx);
memset(commands, 0, sizeof(commands));
for (i = 0; i < SC_ARRAY_SIZE(cmds); ++i) {
sc_cmd_register(cmds[i].cmd, cmds[i].help, cmds[i].cmd_cb);
}
}
/**
* @brief Initializes the default heap.
*
* @notapi
*/
void _heap_init(void) {
default_heap.h_provider = chCoreAlloc;
default_heap.h_free.h.u.next = NULL;
default_heap.h_free.h.size = 0;
#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
chMtxObjectInit(&default_heap.h_mtx);
#else
chSemObjectInit(&default_heap.h_sem, (cnt_t)1);
#endif
}
/**
* @brief Initializes the standard part of a @p ILI9341Driver structure.
*
* @param[out] driverp pointer to the @p ILI9341Driver object
*
* @init
*/
void ili9341ObjectInit(ILI9341Driver *driverp) {
osalDbgCheck(driverp != NULL);
driverp->state = ILI9341_STOP;
driverp->config = NULL;
#if (TRUE == ILI9341_USE_MUTUAL_EXCLUSION)
#if (TRUE == CH_CFG_USE_MUTEXES)
chMtxObjectInit(&driverp->lock);
#else
chSemObjectInit(&driverp->lock, 1);
#endif
#endif /* (TRUE == ILI9341_USE_MUTUAL_EXCLUSION) */
}
/** Set up the tracking module. */
void track_setup(void)
{
SETTING_NOTIFY("track", "iq_output_mask", iq_output_mask, TYPE_INT,
track_iq_output_notify);
track_internal_setup();
for (u32 i=0; i<NUM_TRACKER_CHANNELS; i++) {
tracker_channels[i].state = STATE_DISABLED;
tracker_channels[i].tracker = 0;
chMtxObjectInit(&tracker_channels[i].mutex);
}
platform_track_setup();
}
/**
* @brief Initializes a memory heap from a static memory area.
* @pre Both the heap buffer base and the heap size must be aligned to
* the @p stkalign_t type size.
*
* @param[out] heapp pointer to the memory heap descriptor to be initialized
* @param[in] buf heap buffer base
* @param[in] size heap size
*
* @init
*/
void chHeapObjectInit(memory_heap_t *heapp, void *buf, size_t size) {
union heap_header *hp = buf;
chDbgCheck(MEM_IS_ALIGNED(buf) && MEM_IS_ALIGNED(size));
heapp->h_provider = NULL;
heapp->h_free.h.u.next = hp;
heapp->h_free.h.size = 0;
hp->h.u.next = NULL;
hp->h.size = size - sizeof(union heap_header);
#if (CH_CFG_USE_MUTEXES == TRUE) || defined(__DOXYGEN__)
chMtxObjectInit(&heapp->h_mtx);
#else
chSemObjectInit(&heapp->h_sem, (cnt_t)1);
#endif
}
/**
* @brief Initialize the Data Link Layer object.
* @details The function starts the DataLinkLayer serial driver
* - Check the actual state of the driver
* - Configure and start the sdSerial driver
* - Init the mutex variable used by the 'DLLSendSingleFrameSerial' function
* - Init the mailboxes which are work like a buffer
* - Creates a SyncFrame
* - Starts the 'SDReceiving' and 'SDSending' threads which are provide
* the whole DLL functionality
* - Set the DLL state to ACTIVE
*
* @param[in] dllp DataLinkLayer driver structure
* @param[in] config The config contains the speed and the ID of the serial driver
*/
void DLLStart(DLLDriver *dllp, DLLSerialConfig *config){
osalDbgCheck((dllp != NULL) && (config != NULL));
osalDbgAssert((dllp->state == DLL_UNINIT) || (dllp->state == DLL_ACTIVE),
"DLLInit(), invalid state");
dllp->config = config;
SerialDCfg.speed = dllp->config->baudrate; //Set the data rate to the given rate
sdStart(dllp->config->SDriver, &SerialDCfg); //Start the serial driver for the ESP8266
chMtxObjectInit(&dllp->DLLSerialSendMutex);
chMBObjectInit(&dllp->DLLBuffers.DLLFilledOutputBuffer,
dllp->DLLBuffers.DLLFilledOutputBufferQueue, OUTPUT_FRAME_BUFFER);
chMBObjectInit(&dllp->DLLBuffers.DLLFreeOutputBuffer,
dllp->DLLBuffers.DLLFreeOutputBufferQueue, OUTPUT_FRAME_BUFFER);
int i;
for (i = 0; i < OUTPUT_FRAME_BUFFER; i++)
(void)chMBPost(&dllp->DLLBuffers.DLLFreeOutputBuffer,
(msg_t)&dllp->DLLBuffers.DLLOutputBuffer[i], TIME_INFINITE);
DLLCreateSyncFrame(dllp);
dllp->SendingThread = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(128), NORMALPRIO+1, SDSending, (void *)dllp);
if (dllp->SendingThread == NULL)
chSysHalt("DualFramework: Starting 'SendingThread' failed - out of memory");
dllp->ReceivingThread = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(128), NORMALPRIO+1, SDReceiving, (void *)dllp);
if (dllp->ReceivingThread == NULL)
chSysHalt("DualFramework: Starting 'ReceivingThread' failed - out of memory");
dllp->state = DLL_ACTIVE;
}
/*------------------------------------------------------------------------*
* chibios_rt::Mutex *
*------------------------------------------------------------------------*/
Mutex::Mutex(void) {
chMtxObjectInit(&mutex);
}
static void test_005_006_setup(void) {
chMtxObjectInit(&m1);
}
static void test_005_003_setup(void) {
chMtxObjectInit(&m1); /* Mutex B.*/
chMtxObjectInit(&m2); /* Mutex A.*/
}
static void mtx1_setup(void) {
chMtxObjectInit(&m1);
}
void USBMutexInit(void)
{
chMtxObjectInit(&USB_write_lock);
}
static void mtx4_setup(void) {
chMtxObjectInit(&m1);
chMtxObjectInit(&m2);
}
static void mtx8_setup(void) {
chCondObjectInit(&c1);
chMtxObjectInit(&m1);
chMtxObjectInit(&m2);
}
static void mtx3_setup(void) {
chMtxObjectInit(&m1); /* Mutex B.*/
chMtxObjectInit(&m2); /* Mutex A.*/
}
static void test_005_009_setup(void) {
chCondObjectInit(&c1);
chMtxObjectInit(&m1);
chMtxObjectInit(&m2);
}
/** Application entry point. */
int main(void) {
static thread_t *shelltp = NULL;
/* Initializes a serial-over-USB CDC driver. */
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
sdStart(&SD3, NULL);
chprintf((BaseSequentialStream *)&SD3 , "\n> boot\n");
/*
* Activates the USB driver and then the USB bus pull-up on D+.
* Note, a delay is inserted in order to not have to disconnect the cable
* after a reset.
*/
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/* Shell manager initialization. */
shellInit();
/* Initialize global objects. */
config_init();
chMtxObjectInit(&robot_pose_lock);
/* Initialise timestamp module */
timestamp_stm32_init();
/* bus enumerator init */
static __attribute__((section(".ccm"))) struct bus_enumerator_entry_allocator
bus_enum_entries_alloc[MAX_NB_BUS_ENUMERATOR_ENTRIES];
bus_enumerator_init(&bus_enumerator,
bus_enum_entries_alloc,
MAX_NB_BUS_ENUMERATOR_ENTRIES);
/* allocate and init motor manager */
static __attribute__((section(".ccm"))) trajectory_t trajectory_buffer[MAX_NB_TRAJECTORY_BUFFERS];
static __attribute__((section(".ccm"))) float trajectory_points_buffer[ACTUATOR_TRAJECTORY_NB_POINTS
* ACTUATOR_TRAJECTORY_POINT_DIMENSION
* MAX_NB_TRAJECTORY_BUFFERS];
static __attribute__((section(".ccm"))) motor_driver_t motor_driver_buffer[MAX_NB_MOTOR_DRIVERS];
motor_manager_init(&motor_manager,
trajectory_buffer,
MAX_NB_TRAJECTORY_BUFFERS,
trajectory_points_buffer,
MAX_NB_TRAJECTORY_BUFFERS,
motor_driver_buffer,
MAX_NB_MOTOR_DRIVERS,
&bus_enumerator);
differential_base_init();
/* Checks if there is any log message from a previous boot */
if (panic_log_read() != NULL) {
/* Turns on the user LED if yes */
palClearPad(GPIOC, GPIOC_LED);
/* Turn on all LEDs on the front panel. */
palSetPad(GPIOF, GPIOF_LED_READY);
palSetPad(GPIOF, GPIOF_LED_DEBUG);
palSetPad(GPIOF, GPIOF_LED_ERROR);
palSetPad(GPIOF, GPIOF_LED_POWER_ERROR);
palSetPad(GPIOF, GPIOF_LED_PC_ERROR);
palSetPad(GPIOF, GPIOF_LED_BUS_ERROR);
palSetPad(GPIOF, GPIOF_LED_YELLOW_1);
palSetPad(GPIOF, GPIOF_LED_YELLOW_2);
palSetPad(GPIOF, GPIOF_LED_GREEN_1);
palSetPad(GPIOF, GPIOF_LED_GREEN_2);
} else {
struct netif *ethernet_if;
differential_base_tracking_start(); // tracy
ip_thread_init();
chThdSleepMilliseconds(1000);
ethernet_if = netif_find("ms0");
if (ethernet_if) {
dhcp_start(ethernet_if);
}
sntp_init();
can_bridge_init();
uavcan_node_start(10);
rpc_server_init();
message_server_init();
interface_panel_init();
odometry_publisher_init();
#ifdef ENABLE_STREAM
#warning "Enabling robot stream can lead to lwip crash. Do not use in match until fixed."
stream_init();
#endif
}
/* main thread, spawns a shell on USB connection. */
while (1) {
if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE)) {
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, USB_SHELL_PRIO);
} else if (chThdTerminatedX(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chThdSleepMilliseconds(500);
}
}
void mcucom_port_mutex_init(mcucom_port_mutex_t *mutex)
{
chMtxObjectInit(mutex);
}
void initHardware(Logging *l) {
efiAssertVoid(CUSTOM_IH_STACK, getRemainingStack(chThdGetSelfX()) > 256, "init h");
sharedLogger = l;
engine_configuration_s *engineConfiguration = engine->engineConfigurationPtr;
efiAssertVoid(CUSTOM_EC_NULL, engineConfiguration!=NULL, "engineConfiguration");
board_configuration_s *boardConfiguration = &engineConfiguration->bc;
printMsg(sharedLogger, "initHardware()");
// todo: enable protection. it's disabled because it takes
// 10 extra seconds to re-flash the chip
//flashProtect();
chMtxObjectInit(&spiMtx);
#if EFI_HISTOGRAMS
/**
* histograms is a data structure for CPU monitor, it does not depend on configuration
*/
initHistogramsModule();
#endif /* EFI_HISTOGRAMS */
/**
* We need the LED_ERROR pin even before we read configuration
*/
initPrimaryPins();
if (hasFirmwareError()) {
return;
}
#if EFI_INTERNAL_FLASH
palSetPadMode(CONFIG_RESET_SWITCH_PORT, CONFIG_RESET_SWITCH_PIN, PAL_MODE_INPUT_PULLUP);
initFlash(sharedLogger);
/**
* this call reads configuration from flash memory or sets default configuration
* if flash state does not look right.
*/
if (SHOULD_INGORE_FLASH()) {
engineConfiguration->engineType = DEFAULT_ENGINE_TYPE;
resetConfigurationExt(sharedLogger, engineConfiguration->engineType PASS_ENGINE_PARAMETER_SUFFIX);
writeToFlashNow();
} else {
readFromFlash();
}
#else
engineConfiguration->engineType = DEFAULT_ENGINE_TYPE;
resetConfigurationExt(sharedLogger, engineConfiguration->engineType PASS_ENGINE_PARAMETER_SUFFIX);
#endif /* EFI_INTERNAL_FLASH */
#if EFI_HD44780_LCD
// initI2Cmodule();
lcd_HD44780_init(sharedLogger);
if (hasFirmwareError())
return;
lcd_HD44780_print_string(VCS_VERSION);
#endif /* EFI_HD44780_LCD */
if (hasFirmwareError()) {
return;
}
#if EFI_SHAFT_POSITION_INPUT || defined(__DOXYGEN__)
initTriggerDecoder();
#endif
bool isBoardTestMode_b;
if (CONFIGB(boardTestModeJumperPin) != GPIO_UNASSIGNED) {
efiSetPadMode("board test", CONFIGB(boardTestModeJumperPin),
PAL_MODE_INPUT_PULLUP);
isBoardTestMode_b = (!efiReadPin(CONFIGB(boardTestModeJumperPin)));
// we can now relese this pin, it is actually used as output sometimes
unmarkPin(CONFIGB(boardTestModeJumperPin));
} else {
isBoardTestMode_b = false;
}
#if HAL_USE_ADC || defined(__DOXYGEN__)
initAdcInputs(isBoardTestMode_b);
#endif
if (isBoardTestMode_b) {
// this method never returns
initBoardTest();
}
initRtc();
initOutputPins();
#if EFI_MAX_31855
initMax31855(sharedLogger, getSpiDevice(CONFIGB(max31855spiDevice)), CONFIGB(max31855_cs));
#endif /* EFI_MAX_31855 */
#if EFI_CAN_SUPPORT
initCan();
#endif /* EFI_CAN_SUPPORT */
// init_adc_mcp3208(&adcState, &SPID2);
// requestAdcValue(&adcState, 0);
#if EFI_SHAFT_POSITION_INPUT || defined(__DOXYGEN__)
// todo: figure out better startup logic
initTriggerCentral(sharedLogger);
#endif /* EFI_SHAFT_POSITION_INPUT */
turnOnHardware(sharedLogger);
#if HAL_USE_SPI || defined(__DOXYGEN__)
initSpiModules(boardConfiguration);
#endif
#if EFI_HIP_9011 || defined(__DOXYGEN__)
initHip9011(sharedLogger);
#endif /* EFI_HIP_9011 */
#if EFI_FILE_LOGGING || defined(__DOXYGEN__)
initMmcCard();
#endif /* EFI_FILE_LOGGING */
#if EFI_MEMS || defined(__DOXYGEN__)
initAccelerometer(PASS_ENGINE_PARAMETER_SIGNATURE);
#endif
// initFixedLeds();
#if EFI_BOSCH_YAW || defined(__DOXYGEN__)
initBoschYawRateSensor();
#endif /* EFI_BOSCH_YAW */
// initBooleanInputs();
#if EFI_UART_GPS || defined(__DOXYGEN__)
initGps();
#endif
#if EFI_SERVO
initServo();
#endif
#if ADC_SNIFFER || defined(__DOXYGEN__)
initAdcDriver();
#endif
#if HAL_USE_I2C || defined(__DOXYGEN__)
addConsoleActionII("i2c", sendI2Cbyte);
#endif
// USBMassStorageDriver UMSD1;
// while (true) {
// for (int addr = 0x20; addr < 0x28; addr++) {
// sendI2Cbyte(addr, 0);
// int err = i2cGetErrors(&I2CD1);
// print("I2C: err=%x from %d\r\n", err, addr);
// chThdSleepMilliseconds(5);
// sendI2Cbyte(addr, 255);
// chThdSleepMilliseconds(5);
// }
// }
#if EFI_VEHICLE_SPEED || defined(__DOXYGEN__)
initVehicleSpeed(sharedLogger);
#endif
#if EFI_CDM_INTEGRATION
cdmIonInit();
#endif
#if HAL_USE_EXT || defined(__DOXYGEN__)
initJoystick(sharedLogger);
#endif
calcFastAdcIndexes();
printMsg(sharedLogger, "initHardware() OK!");
}
