int main(void)
{
halInit();
chSysInit();
timestamp_stm32_init();
board_io_pwr_en(true);
board_sensor_pwr_en(true);
error_init();
// standard output
sdStart(&UART_CONN1, NULL);
stdout = (BaseSequentialStream*)&UART_CONN1;
chprintf(stdout, "\n\n\n");
log_init();
log_handler_register(&log_handler_stdout, LOG_LVL_DEBUG, log_handler_stdout_cb);
log_info("=== boot ===");
led_start();
// mount SD card
board_power_cycle_sdcard();
sdcStart(&SDCD1, NULL);
sdcard_mount();
static char logdir[100];
if (sdcard_find_next_file_name_with_prefix("/", "log_", logdir, sizeof(logdir)) < 0) {
log_error("could not determine log file directory");
}
FRESULT res = f_mkdir(logdir);
if (!(res == FR_OK || res == FR_EXIST)) {
log_warning("could not create log directory %s", logdir);
}
size_t logdir_strlen = strlen(logdir);
if (sdcard_is_mounted()) {
// add .txt to logdir
strncpy(&logdir[logdir_strlen], "/log.txt", sizeof(logdir) - logdir_strlen);
logdir[sizeof(logdir)-1] = '\0';
sdcard_log_handler_init(logdir, LOG_LVL_INFO);
logdir[logdir_strlen] = '\0'; // reset log dir string
}
log_boot_message();
// initialization
parameter_namespace_declare(¶meters, NULL, NULL); // root namespace
parameter_string_declare_with_default(&board_name,
¶meters,
"name",
board_name_p_buf,
sizeof(board_name_p_buf),
"ins-board");
msgbus_init(&bus);
services_init();
// load parameters from SD card
log_info("loading parameters from sd card");
sdcard_read_parameter(¶meters, "/config.msgpack");
chprintf(stdout, "current parameters:");
parameter_print(¶meters, (parameter_printfn_t)chprintf, stdout);
// UART driver
io_setup();
// USB serial driver
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(100);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
// start all services
services_start(logdir);
// shellInit();
// char buf[STREAM_DEV_STR_SIZE];
// parameter_string_get(&shell_port, buf, sizeof(buf));
// BaseSequentialStream* shell_dev = get_base_seq_stream_device_from_str(buf);
// static thread_t *shelltp = NULL;
// static ShellConfig shell_cfg;
// shell_cfg.sc_channel = shell_dev;
// shell_cfg.sc_commands = shell_commands;
chThdSleepMilliseconds(500);
while (true) {
// if (shelltp == NULL && shell_dev != NULL) {
// static THD_WORKING_AREA(shell_wa, 2048);
// shelltp = shellCreateStatic(&shell_cfg, shell_wa, sizeof(shell_wa), THD_PRIO_SHELL);
// } else if (shelltp != NULL && chThdTerminatedX(shelltp)) {
// shelltp = NULL;
// }
log_debug("VCC %f", analog_get_vcc());
log_debug("Temp %f", analog_get_cpu_temp());
log_debug("V_DC %f", analog_get_vdc());
log_debug("CONN2_TX %f", analog_get_voltage(ANALOG_CH_CONN2_TX));
log_debug("CONN2_RX %f", analog_get_voltage(ANALOG_CH_CONN2_RX));
log_debug("CONN3_TX %f", analog_get_voltage(ANALOG_CH_CONN3_TX));
log_debug("CONN3_RX %f", analog_get_voltage(ANALOG_CH_CONN3_RX));
chThdSleepMilliseconds(500);
}
}
/** 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);
}
}
