/*------------------------------------------------------------------------*
* Simulator main. *
*------------------------------------------------------------------------*/
int main(void) {
EventListener sd1fel, sd2fel, tel;
/*
* HAL initialization.
*/
halInit();
/*
* ChibiOS/RT initialization.
*/
chSysInit();
/*
* Serial ports (simulated) initialization.
*/
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/*
* Shell manager initialization.
*/
shellInit();
chEvtRegister(&shell_terminated, &tel, 0);
/*
* Console thread started.
*/
cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
console_thread, NULL);
/*
* Initializing connection/disconnection events.
*/
cputs("Shell service started on SD1, SD2");
cputs(" - Listening for connections on SD1");
(void) sdGetAndClearFlags(&SD1);
chEvtRegister(&SD1.sevent, &sd1fel, 1);
cputs(" - Listening for connections on SD2");
(void) sdGetAndClearFlags(&SD2);
chEvtRegister(&SD2.sevent, &sd2fel, 2);
/*
* Events servicing loop.
*/
while (!chThdShouldTerminate())
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
/*
* Clean simulator exit.
*/
chEvtUnregister(&SD1.sevent, &sd1fel);
chEvtUnregister(&SD2.sevent, &sd2fel);
return 0;
}
/*------------------------------------------------------------------------*
* Simulator main. *
*------------------------------------------------------------------------*/
int main(void) {
EventListener sd1fel, sd2fel, tel;
/*
* 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();
/*
* Serial ports (simulated) initialization.
*/
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/*
* Shell manager initialization.
*/
shellInit();
chEvtRegister(&shell_terminated, &tel, 0);
/*
* Console thread started.
*/
cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1,
console_thread, NULL);
/*
* Initializing connection/disconnection events.
*/
cputs("Shell service started on SD1, SD2");
cputs(" - Listening for connections on SD1");
(void) chIOGetAndClearFlags(&SD1);
chEvtRegister(chIOGetEventSource(&SD1), &sd1fel, 1);
cputs(" - Listening for connections on SD2");
(void) chIOGetAndClearFlags(&SD2);
chEvtRegister(chIOGetEventSource(&SD2), &sd2fel, 2);
/*
* Events servicing loop.
*/
while (!chThdShouldTerminate())
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
/*
* Clean simulator exit.
*/
chEvtUnregister(chIOGetEventSource(&SD1), &sd1fel);
chEvtUnregister(chIOGetEventSource(&SD2), &sd2fel);
return 0;
}
/*------------------------------------------------------------------------*
* Simulator main. *
*------------------------------------------------------------------------*/
int main(void) {
initTestStream(&testStream);
/*
* 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();
/*
* Serial ports (simulated) initialization.
*/
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/*
* Console thread started.
*/
cdtp = chThdCreateFromHeap(NULL, CONSOLE_WA_SIZE, NORMALPRIO + 1, console_thread, NULL);
/*
* Initializing connection/disconnection events.
*/
cputs("Shell service started on SD1, SD2");
cputs(" - Listening for connections on SD1");
chEvtRegister(chnGetEventSource(&SD1), &sd1fel, 1);
cputs(" - Listening for connections on SD2");
chEvtRegister(chnGetEventSource(&SD2), &sd2fel, 2);
rusEfiFunctionalTest();
/*
* Events servicing loop.
*/
while (!chThdShouldTerminate()) {
chEvtDispatch(fhandlers, chEvtWaitOne(ALL_EVENTS));
printPendingMessages();
chThdSleepMilliseconds(100);
}
/*
* Clean simulator exit.
*/
chEvtUnregister(chnGetEventSource(&SD1), &sd1fel);
chEvtUnregister(chnGetEventSource(&SD2), &sd2fel);
return 0;
}
/**
* 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;
}
static THD_FUNCTION(cancom_thread, arg) {
(void)arg;
chRegSetThreadName("CAN");
event_listener_t el;
CANRxFrame rxmsg;
uint8_t buffer[9];
chEvtRegister(&CANDx.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0) {
continue;
}
while (canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
buffer[0] = rxmsg.SID;
for (int i = 0;i < rxmsg.DLC;i++) {
buffer[i + 1] = rxmsg.data8[i];
}
packet_handler_int_process_packet(buffer, rxmsg.DLC + 1);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
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 msg_t can_rx(void *p) {
struct can_instance *cip = p;
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
#if SPC5_CAN_USE_FILTERS
rxFlag = chEvtGetAndClearFlagsI(&el);
#endif
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
#if !SPC5_CAN_USE_FILTERS
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#else
while (canReceive(cip->canp, rxFlag,
&rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(PORT_D, cip->led);
}
#endif
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
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 THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palTogglePad(GPIOE, GPIOE_LED3_RED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static msg_t can_rx(void *p) {
EventListener el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(!chThdShouldTerminate()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == RDY_OK) {
/* Process message.*/
palTogglePad(IOPORT3, GPIOC_LED);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return 0;
}
static THD_FUNCTION(can_rx, p) {
struct can_instance *cip = p;
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&cip->canp->rxfull_event, &el, 0);
while(!chThdShouldTerminateX()) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0)
continue;
while (canReceive(cip->canp, CAN_ANY_MAILBOX,
&rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
palToggleLine(cip->led);
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
static THD_FUNCTION(can_rx, p) {
event_listener_t el;
CANRxFrame rxmsg;
(void)p;
chRegSetThreadName("receiver");
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while (true) {
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(100)) == 0)
continue;
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
oca_led_toggle(oca_led_act);
CanMessage UsbMessage = CanMessage_init_default;
UsbMessage.DLC = rxmsg.DLC;
UsbMessage.RTR = rxmsg.DLC;
UsbMessage.IDE = rxmsg.IDE;
UsbMessage.ID = rxmsg.EID;
UsbMessage.Data1 = rxmsg.data32[0];
UsbMessage.Data1 = rxmsg.data32[1];
uint8_t buffer[30];
pb_ostream_t stream = pb_ostream_from_buffer(buffer, sizeof(buffer));
pb_encode(&stream, CanMessage_fields, &UsbMessage);
/*if(serusbcfg.usbp->state == USB_ACTIVE)
{
if(SDU1.state == SDU_READY)
chnWrite(&SDU1, buffer, stream.bytes_written);
}*/
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
}
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;
}
CCM_FUNC static THD_FUNCTION(ThreadCAN, arg)
{
event_listener_t el;
CANTxFrame txmsg;
CANRxFrame rxmsg;
uint16_t i;
bool pidserved;
(void)arg;
chRegSetThreadName("CAN Bus");
while(CAND1.state != CAN_READY) chThdSleepMilliseconds(10);
while(SDU1.state != SDU_READY) chThdSleepMilliseconds(10);
chEvtRegister(&CAND1.rxfull_event, &el, 0);
while(TRUE) {
// Are we using coms for sensor data? If not just sleep.
if (settings.sensorsInput != SENSORS_INPUT_COM && (settings.functions & FUNC_OBD_SERVER) == 0) {
chThdSleepMilliseconds(100);
continue;
}
//checkCanFilters(&CAND1, &cancfg);
if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(10)) == 0) {
continue;
}
while (canReceive(&CAND1, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE) == MSG_OK) {
/* Process message.*/
if (dbg_can) {
chprintf(DBG_STREAM, "->[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), rxmsg.SID, rxmsg.RTR);
for(i = 0; i < rxmsg.DLC; i++) {
chprintf(DBG_STREAM, ":%02x", rxmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
if (settings.functions & FUNC_OBD_SERVER) {
pidserved = serveCanOBDPidRequest(&CAND1, &txmsg, &rxmsg);
if (dbg_can && pidserved) {
chprintf(DBG_STREAM, "<-[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), txmsg.SID, txmsg.RTR);
for(i = 0; i < txmsg.DLC; i++) {
chprintf(DBG_STREAM, ":%02x", txmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
else if (dbg_can) {
chprintf(DBG_STREAM, "!![CANBUS][%08x][ignored sender %03x]\r\n", chSysGetRealtimeCounterX(), rxmsg.SID);
}
}
if ((settings.functions & FUNC_OBD_SERVER) == 0) {
if (settings.sensorsInput == SENSORS_INPUT_OBD_CAN) {
readCanOBDPidResponse(&rxmsg);
}
else if (settings.sensorsInput == SENSORS_INPUT_YAMAHA_CAN) {
readCanYamahaPid(&rxmsg);
}
}
}
if (settings.sensorsInput == SENSORS_INPUT_OBD_CAN
&& (settings.functions & FUNC_OBD_SERVER) == 0) {
// Request OBD PIDs
sendCanOBDFrames(&CAND1, &txmsg);
if (dbg_can) {
chprintf(DBG_STREAM, "<-[CANBUS][%08x][SID:%03x][RTR:%01x]", chSysGetRealtimeCounterX(), txmsg.SID, txmsg.RTR);
for(i = 0; i < 8; i++) {
chprintf(DBG_STREAM, ":%02x", txmsg.data8[i]);
}
chprintf(DBG_STREAM, "\r\n");
}
chThdSleepMilliseconds(100); // ~10Hz
}
}
chEvtUnregister(&CAND1.rxfull_event, &el);
return;
}
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");
}
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::unregister(chibios_rt::EvtListener *elp) {
chEvtUnregister(&ev_source, &elp->ev_listener);
}
void Event::Unregister(EventListener *elp) {
chEvtUnregister(&event, elp);
}