// Initialise the EXT driver and register the buttons events to the current
// thread.
void exti_init(void)
{
chEvtRegisterMask(&pc0_sw_evt, &pc0_sw_listener, PC0_SW_MSK);
chEvtRegisterMask(&pc1_sw_evt, &pc1_sw_listener, PC1_SW_MSK);
chEvtRegisterMask(&pc2_sw_evt, &pc2_sw_listener, PC2_SW_MSK);
chEvtRegisterMask(&pc3_sw_evt, &pc3_sw_listener, PC3_SW_MSK);
extStart(&EXTD1, &extcfg);
}
static void cmdIr(BaseSequentialStream *chp, int argc, char *argv[])
{
(void)argv;
(void)argc;
chEvtRegisterMask(&esSensorEvents , &el, SENSOR_EVENT_IR_END);
chprintf(chp, "Pinging with IR...\r\n");
chEvtBroadcastFlags(&esSensorEvents, SENSOR_EVENT_IR_START);
chEvtWaitOne(SENSOR_EVENT_IR_END);
chThdSleepMilliseconds(50);
ir_index_t i;
ir_angle_t angle;
chSysLock();
for (i = 0; i < IR_RX_COUNT; i++) {
angle = irDetections[i];
chprintf(chp, "IR Angle: %D\r\n", angle);
}
chSysUnlock();
chEvtUnregister(&esSensorEvents, &el);
}
static THD_FUNCTION(ThreadTestEvents, arg)
{
(void)arg;
event_listener_t el;
eventmask_t events;
int i = 0;
chEvtRegisterMask( &hmc5983cfg.data_holder->es,
&el,
HMC5983_DATA_AVAILABLE_EVENTMASK);
while(1)
{
events = chEvtWaitOne(HMC5983_DATA_AVAILABLE_EVENTMASK);
if (events == HMC5983_DATA_AVAILABLE_EVENTMASK)
{
if (i++ > 10)
{
palTogglePad(GPIOC, GPIOC_LED_USR);
i = 0;
}
}
}
return MSG_OK;
}
static THD_FUNCTION(thSerEcho, arg)
{
(void)arg;
chRegSetThreadName("SerEcho");
event_listener_t elSerData;
eventflags_t flags;
chEvtRegisterMask((event_source_t *)chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));
while (!chThdShouldTerminateX())
{
chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
flags = chEvtGetAndClearFlags(&elSerData);
if (flags & CHN_INPUT_AVAILABLE)
{
msg_t charbuf;
do
{
charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
if ( charbuf != Q_TIMEOUT )
{
chSequentialStreamPut(&SD1, charbuf);
}
}
while (charbuf != Q_TIMEOUT);
}
}
}
CCM_FUNC static THD_FUNCTION(ThreadBDU, arg)
{
event_listener_t el1;
eventmask_t flags;
(void)arg;
chRegSetThreadName("BDU");
chEvtRegisterMask(chnGetEventSource(&SDU2), &el1, ALL_EVENTS);
while(USBD1.state != USB_READY) chThdSleepMilliseconds(10);
while(SDU2.state != SDU_READY) chThdSleepMilliseconds(10);
while (TRUE)
{
chEvtWaitAnyTimeout(ALL_EVENTS, TIME_IMMEDIATE);
flags = chEvtGetAndClearFlags(&el1);
pwmEnableChannel(&PWMD_LED2, CHN_LED2, PWM_PERCENTAGE_TO_WIDTH(&PWMD_LED2, 8000));
if (flags & CHN_INPUT_AVAILABLE)
{
pwmEnableChannel(&PWMD_LED2, CHN_LED2, PWM_PERCENTAGE_TO_WIDTH(&PWMD_LED2, 1000));
readCommand((BaseChannel *)&SDU2);
}
else
chThdSleepMilliseconds(2);
}
return;
}
void startConsole(Logging *sharedLogger, CommandHandler console_line_callback_p) {
logger = sharedLogger;
console_line_callback = console_line_callback_p;
#if (defined(EFI_CONSOLE_UART_DEVICE) && ! EFI_SIMULATOR) || defined(__DOXYGEN__)
palSetPadMode(CONSOLE_MODE_SWITCH_PORT, CONSOLE_MODE_SWITCH_PIN, PAL_MODE_INPUT_PULLUP);
b_isCommandLineConsoleOverTTL = GET_CONSOLE_MODE_VALUE() == EFI_USE_UART_FOR_CONSOLE;
if (isCommandLineConsoleOverTTL()) {
/*
* Activates the serial
* it is important to set 'NONE' as flow control! in terminal application on the PC
*/
serialConfig.speed = engineConfiguration->uartConsoleSerialSpeed;
sdStart(EFI_CONSOLE_UART_DEVICE, &serialConfig);
// cannot use pin repository here because pin repository prints to console
palSetPadMode(EFI_CONSOLE_RX_PORT, EFI_CONSOLE_RX_PIN, PAL_MODE_ALTERNATE(EFI_CONSOLE_AF));
palSetPadMode(EFI_CONSOLE_TX_PORT, EFI_CONSOLE_TX_PIN, PAL_MODE_ALTERNATE(EFI_CONSOLE_AF));
isSerialConsoleStarted = true;
chEvtRegisterMask((event_source_t *) chnGetEventSource(EFI_CONSOLE_UART_DEVICE), &consoleEventListener, 1);
}
#else
b_isCommandLineConsoleOverTTL = false;
#endif /* EFI_PROD_CODE */
chThdCreateStatic(consoleThreadStack, sizeof(consoleThreadStack), NORMALPRIO, (tfunc_t)consoleThreadThreadEntryPoint, NULL);
addConsoleAction(SWITCH_TO_BINARY_COMMAND, switchToBinaryProtocol);
}
static void cmd_gps_passthrough(BaseSequentialStream *chp, int argc, char *argv[]) {
(void)argc;
(void)argv;
static const SerialConfig sc = {
9600, 0, USART_CR2_STOP1_BITS | USART_CR2_LINEN, 0};
sdStart(&SD1, &sc);
EventListener elSerData;
flagsmask_t flags;
chEvtRegisterMask(chnGetEventSource(&SD1), &elSerData, EVENT_MASK(1));
while (TRUE)
{
chEvtWaitOneTimeout(EVENT_MASK(1), MS2ST(10));
flags = chEvtGetAndClearFlags(&elSerData);
if (flags & CHN_INPUT_AVAILABLE)
{
msg_t charbuf;
do
{
charbuf = chnGetTimeout(&SD1, TIME_IMMEDIATE);
if ( charbuf != Q_TIMEOUT )
{
chSequentialStreamPut(chp, charbuf);
}
}
while (charbuf != Q_TIMEOUT);
}
}
}
/**
* Command processing thread.
*/
msg_t CmdExecutor::main(void){
this->setName("CmdExecutor");
eventmask_t evt = 0;
struct EventListener el_command_long;
chEvtRegisterMask(&event_mavlink_in_command_long, &el_command_long, EVMSK_MAVLINK_IN_COMMAND_LONG);
/* wait modems */
while(GlobalFlags.messaging_ready == 0)
chThdSleepMilliseconds(50);
/* main loop */
while(!chThdShouldTerminate()){
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_COMMAND_LONG, MS2ST(50));
switch (evt){
case EVMSK_MAVLINK_IN_COMMAND_LONG:
executeCmd(&mavlink_in_command_long_struct);
break;
default:
break;
}
}
chEvtUnregister(&event_mavlink_in_command_long, &el_command_long);
chThdExit(0);
return 0;
}
/**
* @brief Thread for the flash save operation.
*
* @param[in] arg Unused.
* @return Unused.
*/
static THD_FUNCTION(ThreadSysInfoFlashSave, arg)
{
(void)arg;
/* Event registration for new estimation */
event_listener_t el;
/* Set thread name */
chRegSetThreadName("SysInfo FlashSave");
/* Register to new estimation */
chEvtRegisterMask(ptrGetFlashSaveEventSource(), &el,
FLASHSAVE_SAVE_EVENTMASK);
while (1)
{
/* Wait for new estimation */
chEvtWaitOne(FLASHSAVE_SAVE_EVENTMASK);
/* Save settings to flash */
FlashSave_Write(FlashSave_STR2ID("SIVN"), true,
(uint8_t *)system_strings.vehicle_name, VEHICLE_NAME_SIZE);
FlashSave_Write(FlashSave_STR2ID("SIVT"), true,
(uint8_t *)system_strings.vehicle_type, VEHICLE_TYPE_SIZE);
}
}
static msg_t MmcReaderDmThread(void *sdp){
chRegSetThreadName("MmcReaderDm");
struct EventListener el_storage_request_count_dm;
struct EventListener el_storage_request_dm;
chEvtRegisterMask(&event_mavlink_oblique_storage_request_count_dm,
&el_storage_request_count_dm,
EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM);
chEvtRegisterMask(&event_mavlink_oblique_storage_request_dm,
&el_storage_request_dm,
EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM);
eventmask_t evt = 0;
while (!chThdShouldTerminate()){
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM | EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM, MS2ST(50));
if (!mmcIsCardInserted(&MMCD1))
continue;
else{
switch (evt){
case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_DM):
bnapStorageAcquire(&Storage);
mavlink_oblique_storage_count_struct.count = Storage.used;
bnapStorageRelease(&Storage);
chEvtBroadcastFlags(&event_mavlink_oblique_storage_count, EVMSK_MAVLINK_OBLIQUE_STORAGE_COUNT);
break;
case(EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_DM):
_oblique_storage_request_handler_dm(sdp);
break;
default:
break;
}
}
}
chEvtUnregister(&event_mavlink_oblique_storage_request_count_dm,
&el_storage_request_count_dm);
chEvtUnregister(&event_mavlink_oblique_storage_request_dm,
&el_storage_request_dm);
return 0;
}
static void evt1_execute(void) {
event_listener_t el1, el2;
/*
* Testing chEvtRegisterMask() and chEvtUnregister().
*/
chEvtObjectInit(&es1);
chEvtRegisterMask(&es1, &el1, 1);
chEvtRegisterMask(&es1, &el2, 2);
test_assert(1, chEvtIsListeningI(&es1), "no listener");
chEvtUnregister(&es1, &el1);
test_assert(2, chEvtIsListeningI(&es1), "no listener");
chEvtUnregister(&es1, &el2);
test_assert(3, !chEvtIsListeningI(&es1), "stuck listener");
/*
* Testing chEvtDispatch().
*/
chEvtDispatch(evhndl, 7);
test_assert_sequence(4, "ABC");
}
void
vexAudioPlaySoundWait( int freq, int amplitude, int timems )
{
EventListener el;
// register event
chEvtRegisterMask(&sound_done, &el, 1);
// play sound
vexAudioPlaySound( freq, amplitude, timems );
// wait for event, 10 second timeout
chEvtWaitAnyTimeout( ALL_EVENTS, MS2ST(10000));
// unregister
chEvtUnregister( &sound_done, &el );
}
static msg_t fnet_receive_thread(void *arg) {
(void) arg;
EventListener el1;
chRegSetThreadName("FNET receive thread");
chEvtRegisterMask(macGetReceiveEventSource(ÐD1), &el1, FRAME_RECEIVED_ID);
chEvtAddEvents(FRAME_RECEIVED_ID);
while (TRUE) {
eventmask_t mask = chEvtWaitAny(ALL_EVENTS );
if (mask & FRAME_RECEIVED_ID) {
fnet_stm32_input();
}
}
return RDY_OK;
}
static msg_t fnet_timer_thread(void *period_ms) {
EventListener el0;
chRegSetThreadName("FNET timer thread");
evtInit(&fnetEventTimer, MS2ST(period_ms) );
evtStart(&fnetEventTimer);
chEvtRegisterMask(&fnetEventTimer.et_es, &el0, PERIODIC_TIMER_ID);
chEvtAddEvents(PERIODIC_TIMER_ID);
while (TRUE) {
eventmask_t mask = chEvtWaitAny(ALL_EVENTS );
if (mask & PERIODIC_TIMER_ID) {
fnet_timer_ticks_inc();
fnet_timer_handler_bottom(NULL );
}
}
return RDY_OK;
}
static msg_t MmcWriterThread(void *arg){
chRegSetThreadName("MmcWriter");
(void)arg;
struct EventListener el_gps_raw_int;
chEvtRegisterMask(&event_mavlink_gps_raw_int, &el_gps_raw_int, EVMSK_MAVLINK_GPS_RAW_INT);
eventmask_t evt = 0;
/* wait until card not ready */
NOT_READY:
while (!mmcIsCardInserted(&MMCD1))
chThdSleep(SDC_POLLING_INTERVAL);
chThdSleep(SDC_POLLING_INTERVAL);
if (!mmcIsCardInserted(&MMCD1))
goto NOT_READY;
else
_insert_handler();
/* main work cycle */
while (!chThdShouldTerminate()){
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_GPS_RAW_INT, WRITE_TMO);
if (!mmcIsCardInserted(&MMCD1)){
_remove_handler();
goto NOT_READY;
}
else{
bnapStorageAcquire(&Storage);
bnapStorageDoRecord(&Storage);
bnapStorageRelease(&Storage);
}
}
chEvtUnregister(&event_mavlink_gps_raw_int, &el_gps_raw_int);
chThdExit(0);
(void)evt;
return 0;
}
static void evt2_execute(void) {
eventmask_t m;
event_listener_t el1, el2;
systime_t target_time;
/*
* Test on chEvtWaitOne() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitOne(ALL_EVENTS);
test_assert(1, m == 1, "single event error");
m = chEvtWaitOne(ALL_EVENTS);
test_assert(2, m == 4, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(3, m == 0, "stuck event");
/*
* Test on chEvtWaitOne() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitOne(ALL_EVENTS);
test_assert_time_window(4, target_time, target_time + ALLOWED_DELAY);
test_assert(5, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(6, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAny() without wait.
*/
chEvtAddEvents(5);
m = chEvtWaitAny(ALL_EVENTS);
test_assert(7, m == 5, "unexpected pending bit");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(8, m == 0, "stuck event");
/*
* Test on chEvtWaitAny() with wait.
*/
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread1, chThdGetSelfX());
m = chEvtWaitAny(ALL_EVENTS);
test_assert_time_window(9, target_time, target_time + ALLOWED_DELAY);
test_assert(10, m == 1, "single event error");
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(11, m == 0, "stuck event");
test_wait_threads();
/*
* Test on chEvtWaitAll().
*/
chEvtObjectInit(&es1);
chEvtObjectInit(&es2);
chEvtRegisterMask(&es1, &el1, 1);
chEvtRegisterMask(&es2, &el2, 4);
test_wait_tick();
target_time = chVTGetSystemTime() + MS2ST(50);
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX() - 1,
thread2, "A");
m = chEvtWaitAll(5);
test_assert_time_window(12, target_time, target_time + ALLOWED_DELAY);
m = chEvtGetAndClearEvents(ALL_EVENTS);
test_assert(13, m == 0, "stuck event");
test_wait_threads();
chEvtUnregister(&es1, &el1);
chEvtUnregister(&es2, &el2);
test_assert(14, !chEvtIsListeningI(&es1), "stuck listener");
test_assert(15, !chEvtIsListeningI(&es2), "stuck listener");
}
/**
* @brief LWIP handling thread.
*
* @param[in] p pointer to a @p lwipthread_opts structure or @p NULL
* @return The function does not return.
*/
msg_t lwip_thread(void *p) {
event_timer_t evt;
event_listener_t el0, el1;
struct ip_addr ip, gateway, netmask;
static struct netif thisif;
static const MACConfig mac_config = {thisif.hwaddr};
chRegSetThreadName("lwipthread");
/* Initializes the thing.*/
tcpip_init(NULL, NULL);
/* TCP/IP parameters, runtime or compile time.*/
if (p) {
struct lwipthread_opts *opts = p;
unsigned i;
for (i = 0; i < 6; i++)
thisif.hwaddr[i] = opts->macaddress[i];
ip.addr = opts->address;
gateway.addr = opts->gateway;
netmask.addr = opts->netmask;
}
else {
thisif.hwaddr[0] = LWIP_ETHADDR_0;
thisif.hwaddr[1] = LWIP_ETHADDR_1;
thisif.hwaddr[2] = LWIP_ETHADDR_2;
thisif.hwaddr[3] = LWIP_ETHADDR_3;
thisif.hwaddr[4] = LWIP_ETHADDR_4;
thisif.hwaddr[5] = LWIP_ETHADDR_5;
LWIP_IPADDR(&ip);
LWIP_GATEWAY(&gateway);
LWIP_NETMASK(&netmask);
}
macStart(ÐD1, &mac_config);
netif_add(&thisif, &ip, &netmask, &gateway, NULL, ethernetif_init, tcpip_input);
netif_set_default(&thisif);
netif_set_up(&thisif);
/* Setup event sources.*/
evtObjectInit(&evt, LWIP_LINK_POLL_INTERVAL);
evtStart(&evt);
chEvtRegisterMask(&evt.et_es, &el0, PERIODIC_TIMER_ID);
chEvtRegisterMask(macGetReceiveEventSource(ÐD1), &el1, FRAME_RECEIVED_ID);
chEvtAddEvents(PERIODIC_TIMER_ID | FRAME_RECEIVED_ID);
/* Goes to the final priority after initialization.*/
chThdSetPriority(LWIP_THREAD_PRIORITY);
while (TRUE) {
eventmask_t mask = chEvtWaitAny(ALL_EVENTS);
if (mask & PERIODIC_TIMER_ID) {
bool_t current_link_status = macPollLinkStatus(ÐD1);
if (current_link_status != netif_is_link_up(&thisif)) {
if (current_link_status)
tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_up,
&thisif, 0);
else
tcpip_callback_with_block((tcpip_callback_fn) netif_set_link_down,
&thisif, 0);
}
}
if (mask & FRAME_RECEIVED_ID) {
struct pbuf *p;
while ((p = low_level_input(&thisif)) != NULL) {
struct eth_hdr *ethhdr = p->payload;
switch (htons(ethhdr->type)) {
/* IP or ARP packet? */
case ETHTYPE_IP:
case ETHTYPE_ARP:
#if PPPOE_SUPPORT
/* PPPoE packet? */
case ETHTYPE_PPPOEDISC:
case ETHTYPE_PPPOE:
#endif /* PPPOE_SUPPORT */
/* full packet send to tcpip_thread to process */
if (thisif.input(p, &thisif) == ERR_OK)
break;
LWIP_DEBUGF(NETIF_DEBUG, ("ethernetif_input: IP input error\n"));
default:
pbuf_free(p);
}
}
}
}
return 0;
}
void PillRegisterEvtChange(EventListener *PEvtLstnr, uint8_t EvtMask) {
chEvtRegisterMask(&IEvtPillChange, PEvtLstnr, EvtMask);
}
static msg_t PlannerThread(void* arg){
chRegSetThreadName("Planner");
(void)arg;
while(GlobalFlags.messaging_ready == 0)
chThdSleepMilliseconds(50);
struct EventListener el_mission_request_list;
struct EventListener el_mission_count;
struct EventListener el_mission_clear_all;
struct EventListener el_mission_item;
chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
chEvtRegisterMask(&event_mavlink_in_mission_count, &el_mission_count, EVMSK_MAVLINK_IN_MISSION_COUNT);
chEvtRegisterMask(&event_mavlink_in_mission_clear_all, &el_mission_clear_all, EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL);
chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
eventmask_t evt = 0;
while (!chThdShouldTerminate()) {
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST |
EVMSK_MAVLINK_IN_MISSION_COUNT |
EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL |
EVMSK_MAVLINK_IN_MISSION_ITEM,
MS2ST(100));
switch (evt){
/* ground want to know how many items we have */
case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST:
chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
mav2gcs();
chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
break;
/* ground says how many items it wants to send here */
case EVMSK_MAVLINK_IN_MISSION_COUNT:
/* this event now will be handled inside write loop */
chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
gcs2mav(mavlink_in_mission_count_struct.count);
/* register event back to main cycle */
chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
break;
/* ground wants erase all wps */
case EVMSK_MAVLINK_IN_MISSION_CLEAR_ALL:
mission_clear_all();
break;
case EVMSK_MAVLINK_IN_MISSION_ITEM:
/* If a waypoint planner component receives WAYPOINT messages outside
* of transactions it answers with a WAYPOINT_ACK message. */
send_ack(MAV_MISSION_DENIED);
break;
default:
//chDbgPanic("unimplemented");
break;
}
}
chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
chEvtUnregister(&event_mavlink_in_mission_count, &el_mission_count);
chEvtUnregister(&event_mavlink_in_mission_clear_all, &el_mission_clear_all);
chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
chThdExit(0);
return 0;
}
/**
* @details When the last waypoint was successfully received
* the requesting component sends a WAYPOINT_ACK message
* to the targeted component. This finishes the transaction.
* Notice that the targeted component has to listen to
* WAYPOINT_REQUEST messages of the last waypoint until it
* gets the WAYPOINT_ACK or another message that starts
* a different transaction or a timeout happens.
*/
static bool mav2gcs(void){
eventmask_t evt = 0;
bool status = CH_FAILED;
uint32_t retry_cnt = PLANNER_RETRY_CNT;
struct EventListener el_mission_request;
struct EventListener el_mission_ack;
struct EventListener el_mission_request_list;
chEvtRegisterMask(&event_mavlink_in_mission_request_list, &el_mission_request_list, EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST);
chEvtRegisterMask(&event_mavlink_in_mission_request, &el_mission_request, EVMSK_MAVLINK_IN_MISSION_REQUEST);
chEvtRegisterMask(&event_mavlink_in_mission_ack, &el_mission_ack, EVMSK_MAVLINK_IN_MISSION_ACK);
START:
mavlink_out_mission_count_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_out_mission_count_struct.target_system = GROUND_STATION_ID;
mavlink_out_mission_count_struct.count = wpdb.getCount();
chEvtBroadcastFlags(&event_mavlink_out_mission_count, EVMSK_MAVLINK_OUT_MISSION_COUNT);
if (0 == mavlink_out_mission_count_struct.count){
status = CH_SUCCESS;
goto EXIT;
}
/* теперь нам надо понять, дошло сообщение с количеством вейпоинтов, или нет.
* Если нет - земля пришле повторный запрос MISSION_REQUEST_LIST */
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST |
EVMSK_MAVLINK_IN_MISSION_REQUEST |
EVMSK_MAVLINK_IN_MISSION_ACK,
PLANNER_RETRY_TMO * PLANNER_RETRY_CNT);
switch(evt){
case EVMSK_MAVLINK_IN_MISSION_REQUEST_LIST:
/* надобно повторить */
goto START;
/* maint loooong loop */
case EVMSK_MAVLINK_IN_MISSION_REQUEST:
do{
wpdb.load(&mavlink_out_mission_item_struct, mavlink_in_mission_request_struct.seq);
mavlink_out_mission_item_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_out_mission_item_struct.target_system = GROUND_STATION_ID;
chEvtBroadcastFlags(&event_mavlink_out_mission_item, EVMSK_MAVLINK_OUT_MISSION_ITEM);
/* wait next request or ack */
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_REQUEST |
EVMSK_MAVLINK_IN_MISSION_ACK,
PLANNER_RETRY_TMO);
switch(evt){
case EVMSK_MAVLINK_IN_MISSION_REQUEST:
continue;
case EVMSK_MAVLINK_IN_MISSION_ACK:
status = CH_SUCCESS;
goto EXIT;
break;
default:
retry_cnt--;
status = CH_FAILED;
break;
}
}while(retry_cnt);
break; /* case EVMSK_MAVLINK_IN_MISSION_REQUEST */
case EVMSK_MAVLINK_IN_MISSION_ACK:
status = CH_SUCCESS;
break;
default:
status = CH_FAILED;
break;
}
EXIT:
chEvtUnregister(&event_mavlink_in_mission_request, &el_mission_request);
chEvtUnregister(&event_mavlink_in_mission_ack, &el_mission_ack);
chEvtUnregister(&event_mavlink_in_mission_request_list, &el_mission_request_list);
return status;
}
static bool gcs2mav(uint16_t N){
uint32_t seq = 0;
uint32_t retry_cnt = PLANNER_RETRY_CNT;
eventmask_t evt = 0;
mavlink_mission_item_t mi; /* local copy */
uint8_t status = MAV_MISSION_ERROR;
/* check available space */
if (N > wpdb.getCapacity()){
send_ack(MAV_MISSION_NO_SPACE);
return CH_FAILED;
}
/* prepare to harvest waypoints */
struct EventListener el_mission_item;
chEvtRegisterMask(&event_mavlink_in_mission_item, &el_mission_item, EVMSK_MAVLINK_IN_MISSION_ITEM);
chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, 1);/* fake wait to clear "queue" */
if (CH_FAILED == wpdb.clear())
chDbgPanic("");
for (seq=0; seq<N; seq++){
/* prepare request to ground */
mavlink_out_mission_request_struct.target_component = MAV_COMP_ID_MISSIONPLANNER;
mavlink_out_mission_request_struct.target_system = GROUND_STATION_ID;
mavlink_out_mission_request_struct.seq = seq;
retry_cnt = PLANNER_RETRY_CNT;
chThdSleep(PLANNER_ADDITIONAL_TMO);
do{
/* drop message */
chEvtBroadcastFlags(&event_mavlink_out_mission_request, EVMSK_MAVLINK_OUT_MISSION_REQUEST);
/* wait answer */
evt = chEvtWaitOneTimeout(EVMSK_MAVLINK_IN_MISSION_ITEM, PLANNER_RETRY_TMO);
if (EVMSK_MAVLINK_IN_MISSION_ITEM == evt){
chSysLock();
mi = mavlink_in_mission_item_struct;
chSysUnlock();
/* check waypoint cosherness and write it if cosher */
status = check_wp(&mi, seq);
if (status != MAV_MISSION_ACCEPTED)
goto EXIT;
else{
if (CH_FAILED == wpdb.save(&mi, seq))
chDbgPanic("");
break; /* do-while */
}
}
retry_cnt--;
if(0 == retry_cnt)
goto EXIT;
}while(retry_cnt);
}
/* save waypoint count in eeprom only in the very end of transaction */
if (CH_FAILED == wpdb.finalize())
chDbgPanic("");
/* final stuff */
EXIT:
chEvtUnregister(&event_mavlink_in_mission_item, &el_mission_item);
send_ack(status);
if (0 == retry_cnt)
return CH_FAILED;
else
return CH_SUCCESS;
}
void EvtSource::registerMask(chibios_rt::EvtListener *elp,
eventmask_t emask) {
chEvtRegisterMask(&ev_source, &elp->ev_listener, emask);
}
void Event::RegisterMask(EventListener *elp, eventmask_t emask) {
chEvtRegisterMask(&event,elp, emask);
}
int main(void)
{
// copy vector table
memcpy((char *)0x20000000, (const char *)&_vectors, 0x200);
// remap SRAM1 to 0x00000000
SYSCFG->MEMRMP |= 0x03;
/* system & hardware initialization */
halInit();
// float usb inputs, hope the host notices detach...
palSetPadMode(GPIOA, 11, PAL_MODE_INPUT);
palSetPadMode(GPIOA, 12, PAL_MODE_INPUT);
// setup LEDs
palSetPadMode(LED1_PORT,LED1_PIN,PAL_MODE_OUTPUT_PUSHPULL);
palSetPadMode(LED2_PORT, LED2_PIN, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(LED1_PORT,LED1_PIN);
palSetPad(LED2_PORT,LED2_PIN);
chSysInit();
palSetPadMode(GPIOA, 11, PAL_MODE_ALTERNATE(10));
palSetPadMode(GPIOA, 12, PAL_MODE_ALTERNATE(10));
palSetPadMode(GPIOC, 8, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOC, 9, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOC, 10, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOC, 11, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOC, 12, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(GPIOD, 2, PAL_MODE_ALTERNATE(12) | PAL_STM32_OSPEED_HIGHEST);
chThdSleepMilliseconds(50);
/* initialize the SD card */
sdcStart(&SDCD1, NULL);
sdcConnect(&SDCD1);
/* initialize the USB mass storage driver */
msdInit(&UMSD1);
/* turn off green LED, turn on red LED */
palSetPadMode(LED1_PORT, LED1_PIN, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(LED1_PORT, LED1_PIN);
palSetPadMode(LED2_PORT, LED2_PIN, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad(LED2_PORT, LED2_PIN);
/* start the USB mass storage service */
int ret = msdStart(&UMSD1, &msdConfig);
if (ret != 0) {
/* no media found : bye bye !*/
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(1000);
NVIC_SystemReset();
}
/* watch the mass storage events */
EventListener connected;
EventListener ejected;
chEvtRegisterMask(&UMSD1.evt_connected, &connected, EVENT_MASK(1));
chEvtRegisterMask(&UMSD1.evt_ejected, &ejected, EVENT_MASK(2));
/* start the USB driver */
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(1000);
usbStart(&USBD1, &usbConfig);
usbConnectBus(&USBD1);
while (TRUE)
{
eventmask_t event = chEvtWaitOne(EVENT_MASK(1) | EVENT_MASK(2));
if (event == EVENT_MASK(1))
{
/* media connected */
}
else if (event == EVENT_MASK(2))
{
/* media ejected : bye bye !*/
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(1000);
NVIC_SystemReset();
}
}
return 0;
}
static msg_t thdUsbStorage(void *arg)
{
(void) arg; // unused
chRegSetThreadName("UsbStorage:polling");
uint antiBounce=5;
EventListener connected;
// Should use EXTI interrupt instead of active polling,
// but in the chibios_opencm3 implementation, since EXTI is
// used via libopencm3, ISR are routed on pprz/opencm3 and cannot
// be used concurrently by chibios api
// Should be fixed when using chibios-rt branch
while (!chThdShouldTerminate() && antiBounce) {
const bool_t usbConnected = palReadPad (GPIOA, GPIOA_OTG_FS_VBUS);
if (usbConnected)
antiBounce--;
else
antiBounce=5;
chThdSleepMilliseconds(20);
}
isRunning = true;
chRegSetThreadName("UsbStorage:connected");
/* Stop the logs*/
chibios_logFinish (true);
/* connect sdcard sdc interface sdio */
if (sdioConnect () == false)
chThdExit (RDY_TIMEOUT);
/* initialize the USB mass storage driver */
msdInit(&UMSD1);
/* start the USB mass storage service */
msdStart(&UMSD1, &msdConfig);
/* start the USB driver */
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(1000);
usbStart(&USBD1, &usbConfig);
usbConnectBus(&USBD1);
/* wait for a real usb storage connexion before shutting down autopilot */
chEvtRegisterMask(&UMSD1.evt_connected, &connected, EVENT_MASK(1));
chEvtWaitOne(EVENT_MASK(1));
/* stop autopilot */
if (pprzThdPtr != NULL) {
chThdTerminate (pprzThdPtr);
chThdWait (pprzThdPtr);
pprzThdPtr = NULL;
}
/* wait until usb-storage is unmount and usb cable is unplugged*/
while (!chThdShouldTerminate() && palReadPad (GPIOA, GPIOA_OTG_FS_VBUS)) {
chThdSleepMilliseconds(10);
}
/* then close open descriptors and reboot autopilot */
usbDisconnectBus(&USBD1);
chThdSleepMilliseconds(500);
msdStop(&UMSD1);
sdioDisconnect ();
MCU_RESTART();
return RDY_OK;
}
static msg_t control_thread(void* arg)
{
(void)arg;
chRegSetThreadName("motor");
EventListener listener;
chEvtRegisterMask(&_setpoint_update_event, &listener, ALL_EVENTS);
uint64_t timestamp_hnsec = motor_rtctl_timestamp_hnsec();
while (1) {
/*
* Control loop period adapts to comm period.
*/
const uint32_t comm_period = motor_rtctl_get_comm_period_hnsec();
unsigned control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
if (comm_period > 0) {
control_period_ms = comm_period / HNSEC_PER_MSEC;
}
if (control_period_ms < 1) {
control_period_ms = 1;
} else if (control_period_ms > IDLE_CONTROL_PERIOD_MSEC) {
control_period_ms = IDLE_CONTROL_PERIOD_MSEC;
}
/*
* Thread priority - maximum if the motor is running, normal otherwise
*/
const tprio_t desired_thread_priority = (comm_period > 0) ? HIGHPRIO : NORMALPRIO;
if (desired_thread_priority != chThdGetPriority()) {
const tprio_t old = chThdSetPriority(desired_thread_priority);
lowsyslog("Motor: Priority changed: %i --> %i\n", (int)old, (int)desired_thread_priority);
}
/*
* The event must be set only when the mutex is unlocked.
* Otherwise this thread will take control, stumble upon the locked mutex, return the control
* to the thread that holds the mutex, unlock the mutex, then proceed.
*/
chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(control_period_ms));
chMtxLock(&_mutex);
const uint64_t new_timestamp_hnsec = motor_rtctl_timestamp_hnsec();
const uint32_t dt_hnsec = new_timestamp_hnsec - timestamp_hnsec;
const float dt = dt_hnsec / (float)HNSEC_PER_SEC;
timestamp_hnsec = new_timestamp_hnsec;
assert(dt > 0);
update_filters(dt);
update_setpoint_ttl(dt_hnsec / HNSEC_PER_MSEC);
update_control(comm_period, dt);
poll_beep();
chMtxUnlock();
watchdog_reset(_watchdog_id);
}
assert_always(0);
return 0;
}
