/*! \brief sdlog thread.
*
* Test logging to microSD card on e407.
*
*/
msg_t sdlog_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
static const evhandler_t evhndl_sdclog[] = {
sdc_log_data,
sdc_log_data,
sdc_log_data,
sdc_log_data,
sdc_log_data
};
struct EventListener el0, el1, el2, el3, el4;
chRegSetThreadName("sdlog_thread");
#ifdef DEBUG_SDCLOG
/*chThdSleepMilliseconds(1000);*/
#endif
SDCLOGDBG("Start sdlog thread\r\n");
// init structure
datafile_state.log_sequence = 0;
datafile_state.write_errors = 0;
datafile_state.sd_log_opened = false;
sdc_reset_fp_index();
sdc_init_eod((uint8_t)0xa5);
// Assert data is halfword aligned
if(((sizeof(GENERIC_message)*8) % 16) != 0) {
SDCLOGDBG("%s: GENERIC message is not halfword aligned.\r\n", __func__);
return (SDC_ASSERT_ERROR);
}
// Assert we will not overflow Payload
if( (sizeof(MPU9150_read_data) > (sizeof(datafile_state.log_data.data)-1)) ||
(sizeof(MPL3115A2_read_data) > (sizeof(datafile_state.log_data.data)-1)) ||
(sizeof(ADIS16405_burst_data) > (sizeof(datafile_state.log_data.data)-1))) {
SDCLOGDBG("%s: DATA size is too large\r\n");
return (SDC_ASSERT_ERROR);
}
chEvtRegister(&mpl3115a2_data_event , &el0, MPL3115A2);
chEvtRegister(&adis_spi_burst_data_captured, &el1, ADIS16405);
chEvtRegister(&mpu9150_data_event , &el2, MPU9150);
chEvtRegister(&sdc_halt_event , &el3, SENSOR_LOG_HALT);
chEvtRegister(&sdc_start_event , &el4, SENSOR_LOG_START);
while(1) {
if(!fs_stop) {
chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(50)));
} else {
chEvtDispatch(evhndl_sdclog, chEvtWaitOneTimeout((1<<SENSOR_LOG_START), MS2ST(50)));
}
}
return -1;
}
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);
}
}
}
int
main(void)
{
halInit();
chSysInit();
static const evhandler_t evhndl[] = { };
// CDC.
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
usbDisconnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(1500);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
chThdSleepMilliseconds(4000);
uart_init();
chThdCreateStatic(uart_thread_wa, sizeof(uart_thread_wa), NORMALPRIO, uart_thread, NULL);
while (true)
{
palTogglePad(GPIOC, GPIOC_LED);
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100)));
}
}
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;
}
static void evt3_execute(void) {
eventmask_t m;
/*
* Tests various timeout situations.
*/
m = chEvtWaitOneTimeout(ALL_EVENTS, TIME_IMMEDIATE);
test_assert(1, m == 0, "spurious event");
m = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_IMMEDIATE);
test_assert(2, m == 0, "spurious event");
m = chEvtWaitAllTimeout(ALL_EVENTS, TIME_IMMEDIATE);
test_assert(3, m == 0, "spurious event");
m = chEvtWaitOneTimeout(ALL_EVENTS, 10);
test_assert(4, m == 0, "spurious event");
m = chEvtWaitAnyTimeout(ALL_EVENTS, 10);
test_assert(5, m == 0, "spurious event");
m = chEvtWaitAllTimeout(ALL_EVENTS, 10);
test_assert(6, m == 0, "spurious event");
}
static msg_t Thread_mpu9150_int(void* arg) {
(void) arg;
static const evhandler_t evhndl_mpu9150[] = {
mpu9150_int_event_handler
};
struct EventListener evl_mpu9150;
chRegSetThreadName("mpu9150_int");
chEvtRegister(&mpu9150_int_event, &evl_mpu9150, 0);
while (TRUE) {
chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
}
return -1;
}
/*! \brief ADIS Newdata Thread
*/
static msg_t Thread_adis_newdata(void *arg) {
(void)arg;
chRegSetThreadName("adis_newdata");
static const evhandler_t evhndl_newdata[] = {
adis_newdata_handler
};
struct EventListener evl_spi_cb2;
chEvtRegister(&adis_spi_cb_newdata, &evl_spi_cb2, 0);
while (TRUE) {
chEvtDispatch(evhndl_newdata, chEvtWaitOneTimeout((eventmask_t)1, US2ST(50)));
}
return -1;
}
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;
}
msg_t data_udp_send_thread(void *p) {
void * arg __attribute__ ((unused)) = p;
static const evhandler_t evhndl_mpu9150[] = {
data_udp_send_mpu9150_data
};
struct EventListener evl_mpu9150;
err_t err;
ip_addr_t ip_addr_sensor;
ip_addr_t ip_addr_fc;
chRegSetThreadName("data_udp_send_thread");
chEvtRegister(&mpu9150_data_event, &evl_mpu9150, 0);
IMU_A_IP_ADDR(&ip_addr_sensor);
IP_PSAS_FC(&ip_addr_fc);
mpu9150_mac_info.conn = netconn_new( NETCONN_UDP );
/* Bind to the local address, or to ANY address */
// netconn_bind(conn, NULL, DATA_UDP_TX_THREAD_PORT ); //local port, NULL is bind to ALL ADDRESSES! (IP_ADDR_ANY)
err = netconn_bind(mpu9150_mac_info.conn, &ip_addr_sensor, IMU_A_TX_PORT ); //local port
if (err == ERR_OK) {
/* Connect to specific address or a broadcast address */
/*
* \todo Understand why a UDP needs a connect...
* This may be a LwIP thing that chooses between tcp_/udp_/raw_ connections internally.
*
*/
// netconn_connect(conn, IP_ADDR_BROADCAST, DATA_UDP_TX_THREAD_PORT );
err = netconn_connect(mpu9150_mac_info.conn, &ip_addr_fc, FC_LISTEN_PORT_IMU_A );
if(err == ERR_OK) {
while (TRUE) {
chEvtDispatch(evhndl_mpu9150, chEvtWaitOneTimeout(EVENT_MASK(0), MS2ST(50)));
}
}
return RDY_RESET;
}
return RDY_RESET;
}
/*! \brief ADIS DIO1 thread
*
* For burst mode transactions t_readrate is 1uS
*
*/
static msg_t Thread_adis_dio1(void *arg) {
(void)arg;
static const evhandler_t evhndl_dio1[] = {
adis_burst_read_handler,
//adis_read_id_handler,
adis_spi_cb_txdone_handler,
adis_release_bus
};
struct EventListener evl_dio;
struct EventListener evl_spi_ev;
struct EventListener evl_spi_release;
chRegSetThreadName("adis_dio");
chEvtRegister(&adis_dio1_event, &evl_dio, 0);
chEvtRegister(&adis_spi_cb_txdone_event, &evl_spi_ev, 1);
chEvtRegister(&adis_spi_cb_releasebus, &evl_spi_release, 2);
while (TRUE) {
chEvtDispatch(evhndl_dio1, chEvtWaitOneTimeout((EVENT_MASK(2)|EVENT_MASK(1)|EVENT_MASK(0)), US2ST(50)));
}
return -1;
}
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;
}
eventmask_t BaseThread::waitOneEventTimeout(eventmask_t ewmask,
systime_t time) {
return chEvtWaitOneTimeout(ewmask, time);
}
int main(void) {
static Thread *shelltp = NULL;
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler,
packetReceivedHandler
};
struct EventListener el0, el1, el2;
chEvtRegister(&packet_event, &el2, 0);
chEvtInit(&packet_event);
/*
* 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();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Activates the serial driver 6 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD6, NULL);
sdcStart(&SDCD1, NULL);
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Creates the LWIP threads (it changes priority internally).
*/
chThdCreateStatic(wa_lwip_thread, LWIP_THREAD_STACK_SIZE, NORMALPRIO + 2,
lwip_thread, NULL);
/*
* Creates the HTTP thread (it changes priority internally).
*/
/*
chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1,
http_server, NULL);
*/
WORKING_AREA(wa_udp_receive_server, PWM_REC_STACK_SIZE);
chThdCreateStatic(wa_udp_receive_server, sizeof(wa_udp_receive_server), NORMALPRIO +1,
udp_receive_server_init, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
while (TRUE) {
if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
if (palReadPad(GPIOA, GPIOA_BUTTON_WKUP) != 0) {
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
}
int Mp3Decode(const char* pszFile)
{
int nResult = 0;
BYTE* pInData = g_Mp3InBuffer;
UINT unInDataLeft = 0;
FIL fIn;
UINT bEof = FALSE;
UINT bOutOfData = FALSE;
MP3FrameInfo mp3FrameInfo;
uint32_t unDmaBufMode = 0;
g_pMp3DmaBufferPtr = g_pMp3DmaBuffer;
g_pMp3DecoderThread = chThdSelf();
FRESULT errFS = f_open(&fIn, pszFile, FA_READ);
if(errFS != FR_OK)
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: Failed to open file \"%s\" for reading, err=%d\r\n", pszFile, errFS);
return -1;
}
HMP3Decoder hMP3Decoder = MP3InitDecoder();
if(hMP3Decoder == NULL)
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: Failed to initialize mp3 decoder engine\r\n");
return -2;
}
chprintf((BaseChannel*)&SD2, "Mp3Decode: Start decoding \"%s\"\r\n", pszFile);
char szArtist[80];
char szTitle[80];
palSetPad(GPIOD, 12); // green LED
Mp3ReadId3V2Tag(&fIn, szArtist, sizeof(szArtist), szTitle, sizeof(szTitle));
palClearPad(GPIOD, 12); // green LED
if(szArtist[0] != 0 || szTitle[0] != 0)
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: Now playing (ID3v2): %s - %s\r\n", szArtist, szTitle);
}
int nDecodeRes = ERR_MP3_NONE;
UINT unFramesDecoded = 0;
do
{
if(unInDataLeft < (2 * MAINBUF_SIZE) && (!bEof))
{
UINT unRead = Mp3FillReadBuffer(pInData, unInDataLeft, &fIn);
unInDataLeft += unRead;
pInData = g_Mp3InBuffer;
if(unRead == 0)
{
bEof = 1;
}
}
// find start of next MP3 frame - assume EOF if no sync found
int nOffset = MP3FindSyncWord(pInData, unInDataLeft);
if(nOffset < 0)
{
bOutOfData = TRUE;
break;
}
pInData += nOffset;
unInDataLeft -= nOffset;
// decode one MP3 frame - if offset < 0 then bytesLeft was less than a full frame
nDecodeRes = MP3Decode(hMP3Decoder, &pInData, (int*)&unInDataLeft, (short*)g_pMp3OutBuffer, 0);
switch(nDecodeRes)
{
case ERR_MP3_NONE:
{
MP3GetLastFrameInfo(hMP3Decoder, &mp3FrameInfo);
if(unFramesDecoded == 0)
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: %d Hz %d Bit %d Channels\r\n",
mp3FrameInfo.samprate, mp3FrameInfo.bitsPerSample, mp3FrameInfo.nChans);
if((mp3FrameInfo.samprate > 48000) || (mp3FrameInfo.bitsPerSample != 16) || (mp3FrameInfo.nChans != 2))
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: incompatible MP3 file.\r\n");
nResult = -5;
break;
}
}
if((unFramesDecoded) % 100 == 0)
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: frame %u, bitrate=%d\r\n", unFramesDecoded, mp3FrameInfo.bitrate);
}
unFramesDecoded++;
g_pMp3OutBufferPtr = g_pMp3OutBuffer;
uint32_t unOutBufferAvail= mp3FrameInfo.outputSamps;
while(unOutBufferAvail > 0)
{
// fill up the whole dma buffer
uint32_t unDmaBufferSpace = 0;
if(unDmaBufMode == 0)
{
// fill the whole buffer
// dma buf ptr was reset to beginning of the buffer
unDmaBufferSpace = g_pMp3DmaBuffer + MP3_DMA_BUFFER_SIZE - g_pMp3DmaBufferPtr;
}
else if(unDmaBufMode == 1)
{
// fill the first half of the buffer
// dma buf ptr was reset to beginning of the buffer
unDmaBufferSpace = g_pMp3DmaBuffer + (MP3_DMA_BUFFER_SIZE / 2) - g_pMp3DmaBufferPtr;
}
else
{
// fill the last half of the buffer
// dma buf ptr was reset to middle of the buffer
unDmaBufferSpace = g_pMp3DmaBuffer + MP3_DMA_BUFFER_SIZE - g_pMp3DmaBufferPtr;
}
uint32_t unCopy = unDmaBufferSpace > unOutBufferAvail ? unOutBufferAvail : unDmaBufferSpace;
if(unCopy > 0)
{
memcpy(g_pMp3DmaBufferPtr, g_pMp3OutBufferPtr, unCopy * sizeof(uint16_t));
unOutBufferAvail -= unCopy;
g_pMp3OutBufferPtr += unCopy;
unDmaBufferSpace -= unCopy;
g_pMp3DmaBufferPtr += unCopy;
}
if(unDmaBufferSpace == 0)
{
// dma buffer full
// see if this was the first run
if(unDmaBufMode == 0)
{
// on the first buffer fill up,
// start the dma transfer
if(EVAL_AUDIO_Init(OUTPUT_DEVICE_HEADPHONE, 80, (uint32_t)mp3FrameInfo.samprate))
{
chprintf((BaseChannel *) &SD2, "Mp3Decode: audio init failed\r\n");
nResult = -4;
break;
}
EVAL_AUDIO_Play(g_pMp3DmaBuffer, MP3_DMA_BUFFER_SIZE * sizeof(uint16_t));
}
// we must wait for the dma stream tx interrupt here
eventmask_t em = chEvtWaitAny((eventmask_t)2 | 4 | 8);
if(em & 8)
{
// stop requested
chprintf((BaseChannel*)&SD2, "Mp3Decode: Stop requested\r\n");
nResult = 1;
break;
}
if((em & 2) && (em & 4))
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: DMA out of sync (HT and TC both set)\r\n");
nResult = -3;
break;
}
if(unDmaBufMode == 0 || unDmaBufMode == 2)
{
// the dma event we expect is "half transfer" (=2)
if(em & 2)
{
// set up first half mode
unDmaBufMode = 1;
g_pMp3DmaBufferPtr = g_pMp3DmaBuffer;
}
else
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: DMA out of sync (expected HT, got TC)\r\n");
nResult = -3;
break;
}
}
else
{
// the dma event we expect is "transfer complete" (=4)
if(em & 4)
{
// set up last half mode
unDmaBufMode = 2;
g_pMp3DmaBufferPtr = g_pMp3DmaBuffer + (MP3_DMA_BUFFER_SIZE / 2);
}
else
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: DMA out of sync (expected TC, got HT)\r\n");
nResult = -3;
}
}
}
}
break;
}
case ERR_MP3_MAINDATA_UNDERFLOW:
{
// do nothing - next call to decode will provide more mainData
break;
}
case ERR_MP3_FREE_BITRATE_SYNC:
{
break;
}
case ERR_MP3_INDATA_UNDERFLOW:
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: Decoding error ERR_MP3_INDATA_UNDERFLOW\r\n");
bOutOfData = TRUE;
break;
}
default:
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: Decoding error %d\r\n", nDecodeRes);
bOutOfData = TRUE;
break;
}
}
}
while((!bOutOfData) && (nResult == 0));
chprintf((BaseChannel*)&SD2, "Mp3Decode: Finished decoding\r\n");
MP3FreeDecoder(hMP3Decoder);
if(EVAL_AUDIO_Stop(CODEC_PDWN_HW))
{
chprintf((BaseChannel*)&SD2, "Mp3Decode: Failed to stop audio\r\n");
}
EVAL_AUDIO_DeInit();
f_close(&fIn);
// this is the only legit way I know
// to remvove still pending event flags
// from the thread
chEvtWaitOneTimeout(2, 50);
chEvtWaitOneTimeout(4, 50);
return nResult;
}
eventmask_t Event::WaitOneTimeout(eventmask_t ewmask, systime_t time) {
return chEvtWaitOneTimeout(ewmask, time);
}
/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler
};
Thread *shelltp = NULL;
struct EventListener el0, el1;
/*
* 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();
/*
* Activates the serial driver 1 using the driver default configuration.
*/
sdStart(&SD1, NULL);
/*
* Shell manager initialization.
*/
shellInit();
/*
* Initializes the MMC driver to work with SPI.
*/
palSetPadMode(IOPORT1, PIOA_CS_MMC, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad(IOPORT1, PIOA_CS_MMC);
mmcObjectInit(&MMCD1);
mmcStart(&MMCD1, &mmccfg);
/*
* Activates the card insertion monitor.
*/
tmr_init(&MMCD1);
/*
* Creates the blinker threads.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
while (TRUE) {
if (!shelltp)
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
return 0;
}
/*
* Application entry point.
*/
int main(void)
{
enum led_status lstat = LST_INIT;
EventListener el0;
alert_status_t proto_st = ALST_INIT;
alert_status_t bmp085_st = ALST_INIT;
alert_status_t mpu6050_st = ALST_INIT;
alert_status_t hmc5883_st = ALST_INIT;
/*
* 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();
#ifdef BOARD_IMU_AHRF
/* Clear DRDY pad */
palClearPad(GPIOA, GPIOA_DRDY);
/* Activates serial */
sdStart(&SD1, NULL);
sdStart(&SD2, NULL);
/* Activate pwm */
pwmStart(&PWMD1, &pwm1cfg);
/* Activate i2c */
i2cStart(&I2CD1, &i2c1cfg);
/* Activate exti */
extStart(&EXTD1, &extcfg);
#endif /* BOARD_IMU_AHRF */
#ifdef BOARD_CAPTAIN_PRO2
/* Activates serial */
sdStart(&SD3, NULL);
sdStart(&SD4, NULL);
/* Activate pwm */
pwmStart(&PWMD3, &pwm3cfg);
pwmStart(&PWMD4, &pwm4cfg);
pwmStart(&PWMD5, &pwm5cfg);
/* Activate i2c */
i2cStart(&I2CD1, &i2c1cfg);
/* Activate exti */
extStart(&EXTD1, &extcfg);
/* Activate adc */
adcStart(&ADCD1, NULL);
#endif /* BOARD_CAPTAIN_PRO2 */
/* alert subsys */
chEvtInit(&alert_event_source);
chEvtRegister(&alert_event_source, &el0, 0);
/* init devices */
pt_init();
chThdSleepMilliseconds(10); /* power on delay */
#ifdef HAS_DEV_BMP085
bmp085_init();
chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MS5611
ms5611_init(&ms5611cfg);
chThdSleepMilliseconds(50); /* init delay */
#endif
#ifdef HAS_DEV_MPU6050
mpu6050_init(&mpu6050cfg);
chThdSleepMilliseconds(250); /* give some time for mpu6050 configuration */
#endif
#ifdef HAS_DEV_HMC5883
hmc5883_init(&hmc5883cfg);
#endif
#ifdef HAS_DEV_SERVOPWM
servopwm_init(&servopwmcfg);
#endif
#ifdef HAS_DEV_NTC10K
ntc10k_init();
#endif
#ifdef HAS_DEV_RPM
rpm_init();
#endif
#ifdef BOARD_IMU_AHRF
/* Set DRDY pad */
palSetPad(GPIOA, GPIOA_DRDY);
#endif
while (TRUE) {
eventmask_t msk = chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(100));
if (msk & EVENT_MASK(0)) {
flagsmask_t fl = chEvtGetAndClearFlags(&el0);
if (fl & ALERT_FLAG_PROTO)
proto_st = pt_get_status();
#ifdef HAS_DEV_MPU6050
if (fl & ALERT_FLAG_MPU6050)
mpu6050_st = mpu6050_get_status();
#endif
#ifdef HAS_DEV_HMC5883
if (fl & ALERT_FLAG_HMC5883)
hmc5883_st = hmc5883_get_status();
#endif
#ifdef HAS_DEV_BMP085
if (fl & ALERT_FLAG_BMP085)
bmp085_st = bmp085_get_status();
#endif
#ifdef HAS_DEV_MS5611
if (fl & ALERT_FLAG_BMP085)
bmp085_st = ms5611_get_status();
#endif
pt_set_sens_state(mpu6050_st, hmc5883_st, bmp085_st);
}
if (proto_st == ALST_FAIL || mpu6050_st == ALST_FAIL || hmc5883_st == ALST_FAIL || bmp085_st == ALST_FAIL)
lstat = LST_FAIL;
else if (proto_st == ALST_INIT || mpu6050_st == ALST_INIT || hmc5883_st == ALST_INIT || bmp085_st == ALST_INIT)
lstat = LST_INIT;
else if (proto_st == ALST_NORMAL && mpu6050_st == ALST_NORMAL && hmc5883_st == ALST_NORMAL && bmp085_st == ALST_NORMAL)
lstat = LST_NORMAL;
led_update(lstat);
}
}
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;
}
/*
* Application entry point.
*/
int main(void) {
static thread_t *shelltp = NULL;
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler
};
event_listener_t el0, el1;
/*
* 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.
* - lwIP subsystem initialization using the default configuration.
*/
halInit();
chSysInit();
lwipInit(NULL);
/*
* Initialize board LED.
*/
palSetLineMode(LINE_ARD_D13, PAL_MODE_OUTPUT_PUSHPULL);
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU2);
sduStart(&SDU2, &serusbcfg);
/*
* 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();
/*
* Activates the serial driver 1 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD1, NULL);
sdcStart(&SDCD1, NULL);
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Creates the HTTP thread (it changes priority internally).
*/
chThdCreateStatic(wa_http_server, sizeof(wa_http_server), NORMALPRIO + 1,
http_server, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
while (true) {
if (!shelltp && (SDU2.config->usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminatedX(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
if (palReadPad(GPIOI, GPIOI_BUTTON_USER) != 0) {
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
}
/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler,
ShellHandler
};
event_listener_t el0, el1, el2;
/*
* 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();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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();
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, handling SD card events and shell
* start/exit.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
chEvtRegister(&shell_terminated, &el2, 2);
while (true) {
if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE)) {
shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
"shell", NORMALPRIO + 1,
shellThread, (void *)&shell_cfg1);
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
}
int main(void) {
static Thread *shelltp = NULL;
static const evhandler_t evhndl_main[] = {
extdetail_WKUP_button_handler
};
struct EventListener el0;
/*
* 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();
extdetail_init();
palSetPad(GPIOC, GPIOC_LED);
palSetPad(GPIOA, GPIOA_SPI1_SCK);
palSetPad(GPIOA, GPIOA_SPI1_NSS);
/*
* SPI1 I/O pins setup.
*/
palSetPadMode(adis_connections.spi_sck_port, adis_connections.spi_sck_pad,
PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST);
palSetPadMode(adis_connections.spi_miso_port, adis_connections.spi_miso_pad,
PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST| PAL_STM32_PUDR_FLOATING);
palSetPadMode(adis_connections.spi_mosi_port, adis_connections.spi_mosi_pad,
PAL_MODE_ALTERNATE(5) |
PAL_STM32_OSPEED_HIGHEST );
palSetPadMode(adis_connections.spi_cs_port, adis_connections.spi_cs_pad,
PAL_MODE_OUTPUT_PUSHPULL |
PAL_STM32_OSPEED_HIGHEST);
palSetPad(GPIOA, GPIOA_SPI1_SCK);
palSetPad(GPIOA, GPIOA_SPI1_NSS);
/*!
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU_PSAS);
sduStart(&SDU_PSAS, &serusbcfg);
/*!
* 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(1000);
usbStart(serusbcfg.usbp, &usbcfg);
usbConnectBus(serusbcfg.usbp);
shellInit();
iwdg_begin();
/*!
* Activates the serial driver 6 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD6, NULL);
spiStart(&SPID1, &adis_spicfg); /* Set transfer parameters. */
chThdSleepMilliseconds(300);
adis_init();
adis_reset();
/*! Activates the EXT driver 1. */
extStart(&EXTD1, &extcfg);
chThdCreateStatic(waThread_blinker, sizeof(waThread_blinker), NORMALPRIO, Thread_blinker, NULL);
chThdCreateStatic(waThread_adis_dio1, sizeof(waThread_adis_dio1), NORMALPRIO, Thread_adis_dio1, NULL);
chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL);
chThdCreateStatic(waThread_indwatchdog, sizeof(waThread_indwatchdog), NORMALPRIO, Thread_indwatchdog, NULL);
chEvtRegister(&extdetail_wkup_event, &el0, 0);
while (TRUE) {
if (!shelltp && (SDU_PSAS.config->usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500)));
}
}
/*
* Application entry point.
*/
int main(void) {
static thread_t *shelltp = NULL;
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler
};
event_listener_t el0, el1;
/*
* 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();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&SDU1);
sduStart(&SDU1, &serusbcfg);
/*
* 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();
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
while (true) {
if (!shelltp && (SDU1.config->usbp->state == USB_ACTIVE))
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
else if (chThdTerminatedX(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
}
int main(void) {
static const evhandler_t evhndl_main[] = {
extdetail_WKUP_button_handler
};
struct EventListener el0;
/*
* 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();
extdetail_init();
palSetPad(GPIOC, GPIOC_LED);
/*!
* GPIO Pins for generating pulses at data input detect and data output send.
* Used for measuring latency timing of data
*
* \sa board.h
*/
palClearPad( TIMEOUTPUT_PORT, TIMEOUTPUT_PIN);
palSetPadMode(TIMEOUTPUT_PORT, TIMEOUTPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL);
palSetPad( TIMEINPUT_PORT, TIMEINPUT_PIN);
palSetPadMode(TIMEINPUT_PORT, TIMEINPUT_PIN, PAL_MODE_OUTPUT_PUSHPULL );
/*
* I2C2 I/O pins setup
*/
palSetPadMode(si_i2c_connections.i2c_sda_port , si_i2c_connections.i2c_sda_pad,
PAL_MODE_ALTERNATE(4) | PAL_STM32_OTYPE_OPENDRAIN | PAL_STM32_OSPEED_HIGHEST |PAL_STM32_PUDR_FLOATING );
palSetPadMode(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad,
PAL_MODE_ALTERNATE(4) | PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_FLOATING);
palSetPad(si_i2c_connections.i2c_scl_port, si_i2c_connections.i2c_scl_pad );
static const ShellCommand commands[] = {
{"mem", cmd_mem},
{"threads", cmd_threads},
{NULL, NULL}
};
usbSerialShellStart(commands);
mpu9150_start(&I2CD2);
i2cStart(mpu9150_driver.i2c_instance, &si_i2c_config);
mpu9150_init(mpu9150_driver.i2c_instance);
/* Administrative threads */
chThdCreateStatic(waThread_blinker, sizeof(waThread_blinker), NORMALPRIO, Thread_blinker, NULL);
chThdCreateStatic(waThread_indwatchdog, sizeof(waThread_indwatchdog), NORMALPRIO, Thread_indwatchdog, NULL);
/* MAC */
/*!
* Use a locally administered MAC address second LSbit of MSB of MAC should be 1
* Use unicast address LSbit of MSB of MAC should be 0
*/
data_udp_init();
chThdCreateStatic(wa_lwip_thread , sizeof(wa_lwip_thread) , NORMALPRIO + 2, lwip_thread , SENSOR_LWIP);
chThdCreateStatic(wa_data_udp_send_thread , sizeof(wa_data_udp_send_thread) , NORMALPRIO , data_udp_send_thread , NULL);
/* i2c MPU9150 */
chThdCreateStatic(waThread_mpu9150_int, sizeof(waThread_mpu9150_int) , NORMALPRIO , Thread_mpu9150_int, NULL);
/* SPI ADIS */
//chThdCreateStatic(waThread_adis_dio1, sizeof(waThread_adis_dio1), NORMALPRIO, Thread_adis_dio1, NULL);
//chThdCreateStatic(waThread_adis_newdata, sizeof(waThread_adis_newdata), NORMALPRIO, Thread_adis_newdata, NULL);
/*! Activates the EXT driver 1. */
extStart(&EXTD1, &extcfg);
chEvtRegister(&extdetail_wkup_event, &el0, 0);
while (TRUE) {
chEvtDispatch(evhndl_main, chEvtWaitOneTimeout((eventmask_t)1, MS2ST(500)));
}
}
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;
}
int main(void) {
uint32_t i;
FRDMSlave * slave;
FRDMSlave::data_t * data_iter;
FRDMSlave::data_t * buffer_end;
halInit();
chSysInit();
main_thread = chThdSelf();
Platform::early_init();
for(slave = slaves; slave < slaves + num_slaves; slave++){
slave->init();
}
timer_config();
button.set_press_handler((button_t::button_handler)button_cb, nullptr);
term.launch();
while(true){
// Wait for a button press
chEvtGetAndClearEvents(ALL_EVENTS);
chEvtWaitOne(MAIN_SIG_BUTTON);
/*
for(slave = slaves; slave < slaves + num_slaves; slave++){
if(!slave->ping())
break;
} if(slave != slaves + num_slaves){
// One of the slaves didn't respond...
for(i = 0; i < 10; i++){
chThdSleep(MS2ST(100));
leds[0].toggle();
leds[1].toggle();
leds[2].toggle();
leds[3].toggle();
}
}
*/
// Flash lights 3 times
for(i = 0; i < 5; i++){
chThdSleep(MS2ST(500));
leds[0].toggle();
leds[1].toggle();
leds[2].toggle();
leds[3].toggle();
}
// Let the last one go for a whole second
chThdSleep(MS2ST(500));
leds[0].clear();
leds[1].clear();
leds[2].clear();
leds[3].clear();
// Trigger first sample
sync_pin.toggle();
timer_start();
chEvtGetAndClearEvents(MAIN_SIG_BUTTON);
sample_count = 0;
data_iter = acc_buffer1;
buffer_end = acc_buffer1 + sizeof(acc_buffer1)/sizeof(*acc_buffer1);
while(!chEvtWaitOneTimeout(MAIN_SIG_BUTTON, TIME_IMMEDIATE)){
chEvtWaitOne(MAIN_SIG_TICK);
for(slave = slaves; slave < slaves + num_slaves; slave++){
slave->start_read();
}
for(slave = slaves; slave < slaves + num_slaves; slave++){
slave->finish_read(data_iter);
data_iter += FRDMSlave::samples_per_transfer;
sample_count += FRDMSlave::samples_per_transfer;
if(data_iter == acc_buffer1 + (sizeof(acc_buffer1)/sizeof(*acc_buffer1))){
data_iter = acc_buffer2;
} else if(data_iter == acc_buffer2 + (sizeof(acc_buffer2)/sizeof(*acc_buffer2))){
break;
}
} if(slave != slaves + num_slaves){
break;
}
// Trigger next sample
sync_pin.toggle();
#if 0
for(slave = slaves; slave < slaves + num_slaves; slave++){
slave->finish_read(data_iter);
data_iter += FRDMSlave::samples_per_transfer;
if((uint32_t)(buffer_end - data_iter) < FRDMSlave::samples_per_transfer * sizeof(FRDMSlave::data_t)){
if(data_iter < acc_buffer1){
// Buffer is full!
break;
}
data_iter = acc_buffer2;
buffer_end = acc_buffer2 + sizeof(acc_buffer2)/sizeof(*acc_buffer2);
}
} if(slave != slaves + num_slaves){
for(i = 0; i < 10; i++){
chThdSleep(MS2ST(100));
leds[0].toggle();
leds[1].toggle();
leds[2].toggle();
leds[3].toggle();
}
break;
}
#endif
leds[0].toggle();
}
timer_stop();
for(slave = slaves; slave < slaves + num_slaves; slave++){
slave->stop();
}
leds[0].clear();
leds[1].clear();
leds[2].clear();
leds[3].clear();
chThdSleep(MS2ST(250));
}
}
/**
* @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;
}
/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
InsertHandler,
RemoveHandler,
ShellHandler
};
event_listener_t el0, el1, el2;
/*
* 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.
* - lwIP subsystem initialization using the default configuration.
*/
halInit();
chSysInit();
/*
* Initialize RNG
*/
rccEnableAHB2(RCC_AHB2ENR_RNGEN, 0);
RNG->CR |= RNG_CR_IE;
RNG->CR |= RNG_CR_RNGEN;
/* lwip */
lwipInit(NULL);
/*
* Target-dependent setup code.
*/
portab_setup();
/*
* Initializes a serial-over-USB CDC driver.
*/
sduObjectInit(&PORTAB_SDU1);
sduStart(&PORTAB_SDU1, &serusbcfg);
/*
* 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();
/*
* Activates the SDC driver 1 using default configuration.
*/
sdcStart(&SDCD1, NULL);
/*
* Activates the card insertion monitor.
*/
tmr_init(&SDCD1);
/*
* Creates the blinker thread.
*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO, Thread1, NULL);
/*
* Creates the HTTPS thread (it changes priority internally).
*/
chThdCreateStatic(wa_https_server, sizeof(wa_https_server), NORMALPRIO + 1,
https_server, NULL);
/*
* Normal main() thread activity, handling SD card events and shell
* start/exit.
*/
chEvtRegister(&inserted_event, &el0, 0);
chEvtRegister(&removed_event, &el1, 1);
chEvtRegister(&shell_terminated, &el2, 2);
while (true) {
if (!shelltp && (PORTAB_SDU1.config->usbp->state == USB_ACTIVE)) {
shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
"shell", NORMALPRIO + 1,
shellThread, (void *)&shell_cfg1);
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, TIME_MS2I(500)));
}
}
/*
* Application entry point.
*/
int main(void) {
static const evhandler_t evhndl[] = {
WKUP_button_handler
};
struct EventListener el0;
/*
* 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();
/*
* Initialize event structures BEFORE using them
*/
chEvtInit(&wkup_event);
/*
* Shell manager initialization.
*/
usbSerialShellStart(commands);
// Enable Continuous GPT for 1ms Interval
gptStart(&GPTD1, &gpt1cfg);
gptStartContinuous(&GPTD1,10000);
// Configure pins for Feedback ADC's
palSetPadMode(GPIOA, GPIOA_PIN4, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOA, GPIOA_PIN5, PAL_MODE_INPUT_ANALOG);
// Configure pins for Input ADC's
palSetPadMode(GPIOF, GPIOF_PIN6, PAL_MODE_INPUT_ANALOG);
palSetPadMode(GPIOF, GPIOF_PIN7, PAL_MODE_INPUT_ANALOG);
// Configure pins for LED's
// PD3: Vertical Axis LED
palSetPadMode(GPIOD, GPIOD_PIN3, PAL_MODE_OUTPUT_PUSHPULL);
// PD4: Lateral Axis LED
palSetPadMode(GPIOD, GPIOD_PIN4, PAL_MODE_OUTPUT_PUSHPULL);
// Configure pins for switches
// PD8: Drive Enable Switch
palSetPadMode(GPIOD, GPIOD_PIN8, PAL_MODE_INPUT);
// PD9: Mode Select Switch
palSetPadMode(GPIOD, GPIOD_PIN9, PAL_MODE_INPUT);
// Configure pins for long lead GMD Enable/Watchdog
// PD5: Enable out to GMD
palSetPadMode(GPIOD, GPIOD_PIN5, PAL_MODE_OUTPUT_PUSHPULL);
// PD6: Watchdog out to GMD
palSetPadMode(GPIOD, GPIOD_PIN6, PAL_MODE_OUTPUT_PUSHPULL);
// Configure pins for short lead GMD Enable/Watchdog
// PD10: Enable out to GMD
palSetPadMode(GPIOD, GPIOD_PIN10, PAL_MODE_OUTPUT_PUSHPULL);
// PD11: Watchdog out to GMD
palSetPadMode(GPIOD, GPIOD_PIN11, PAL_MODE_OUTPUT_PUSHPULL);
// Configure pins for PWM output (D12-D15: TIM4, channel 1-4)
palSetPadMode(GPIOD, GPIOD_PIN12, PAL_MODE_ALTERNATE(2)); //U-pole, short lead
palSetPadMode(GPIOD, GPIOD_PIN13, PAL_MODE_ALTERNATE(2)); //V-pole, short lead
palSetPadMode(GPIOD, GPIOD_PIN14, PAL_MODE_ALTERNATE(2)); //U-pole, long lead
palSetPadMode(GPIOD, GPIOD_PIN15, PAL_MODE_ALTERNATE(2)); //V-pole, long lead
adcStart(&ADCD1, NULL);
adcStart(&ADCD2, NULL);
adcStart(&ADCD3, NULL);
pwmStart(&PWMD4, &pwmcfg);
// Enable TIM4 PWM channel 1-4 with initial DC=0%
/* @param[in] pwmp pointer to a @p PWMDriver object
* @param[in] channel PWM channel identifier (0...PWM_CHANNELS-1)
* @param[in] width PWM pulse width as clock pulses number
*/
pwmEnableChannel(&PWMD4, 0, 0);
pwmEnableChannel(&PWMD4, 1, 0);
pwmEnableChannel(&PWMD4, 2, 0);
pwmEnableChannel(&PWMD4, 3, 0);
// Set axis control gain and limit values
// Set Vertical Axis Gains
vertAxisStruct.U16PositionPGain = 4;
vertAxisStruct.U16PositionIGain = 1;
vertAxisStruct.U16PositionDGain = 0;
// Set vertical axis limits
vertAxisStruct.U16CommandLimit = VERTICAL_COMMAND_LIMIT;
vertAxisStruct.U16HighPosnLimit = 5100;
vertAxisStruct.U16LowPosnLimit = 2480;
// Set Lateral Axis Gains
latAxisStruct.U16PositionPGain = 2;
latAxisStruct.U16PositionIGain = 0;
latAxisStruct.U16PositionDGain = 0;
// Set lateral axis limits
latAxisStruct.U16CommandLimit = LATERAL_COMMAND_LIMIT;
latAxisStruct.U16HighPosnLimit = 5300;
latAxisStruct.U16LowPosnLimit = 3100;
/*
* Activates the serial driver 6 and SDC driver 1 using default
* configuration.
*/
sdStart(&SD6, NULL);
/*
* Activates the EXT driver 1.
* This is for the external interrupt
*/
extStart(&EXTD1, &extcfg);
/*
* Normal main() thread activity, in this demo it does nothing except
* sleeping in a loop and listen for events.
*/
chEvtRegister(&wkup_event, &el0, 0);
while (TRUE) {
//Cycle motordrive if timer fails
if(U32DelayCount++ > 2500){
motordrive(&GPTD1);
}
chEvtDispatch(evhndl, chEvtWaitOneTimeout(ALL_EVENTS, MS2ST(500)));
}
}
void fatfs_dispatch_event( void )
{
chEvtDispatch( evhndl, chEvtWaitOneTimeout( ALL_EVENTS, MS2ST( 500 ) ) );
}
