int getByte(void)
{
return sdGetTimeout(_interface, DEFAULT_TIMEOUT);
//uint8_t cmd = 0;
//sdRead(_interface,&cmd,1);
//return cmd;
}
void cpu_io_process( void )
{
static uint8_t slash = 0;
static int out_index = 0;
// Try reading serial.
/*
msg_t msg;
msg = sdGetTimeout( &SERIAL, TIME_INFINITE );
sdPut( &SERIAL, msg );
return;
*/
msg_t msg;
msg = sdGetTimeout( &SERIAL, TIME_INFINITE );
uint8_t noData = ( ( msg == Q_TIMEOUT ) || ( msg == Q_RESET ) ) ? 1 : 0;
if ( !noData )
{
uint8_t v = (uint8_t)msg;
//shift = serial_decode_byte( msg, &(buffer[out_index]), &eom );
if ( !slash )
{
if ( v == '\0' )
{
// Execute command
process_command( buffer, out_index );
out_index = 0;
}
else if ( v != '\\' )
buffer[ out_index++ ] = v;
else
slash = 1;
}
else
{
slash = 0;
buffer[ out_index++ ] = v;
}
// Just in case of crazy command
out_index = ( out_index < BUFFER_SZ ) ? out_index : BUFFER_SZ;
}
}
void
vexLcdCheckReceiveMessage( LcdData *lcd )
{
int16_t i;
// any characters
if( sdGetWouldBlock(lcd->sdp) )
return;
// read up to 16 bytes from serial port
for(i=0;i<16;i++)
{
int16_t c;
c = sdGetTimeout( lcd->sdp, TIME_IMMEDIATE);
if( c != Q_TIMEOUT )
lcd->rxbuf[i] = c;
else
break;
}
// 6 chars ?
if( i == 6 )
{
// lcd message ?
if( (lcd->rxbuf[0] == 0xAA) && (lcd->rxbuf[1] == 0x55) && (lcd->rxbuf[2] == 0x16))
// verify checksum
if( !((lcd->rxbuf[4] + lcd->rxbuf[5]) & 0xFF) )
lcd->buttons = lcd->rxbuf[4];
}
// flush anything left
if( !sdGetWouldBlock(lcd->sdp) )
{
chSysLock();
chIQResetI( &(lcd->sdp)->iqueue );
chSysUnlock();
}
}
static msg_t ShellThread(void *arg){
chRegSetThreadName("Shell");
chThdSleepMilliseconds(1000);
/* init static pointer for serial driver with received pointer */
shell_sdp = (SerialUSBDriver *)arg;
// create and init microrl object
microrl_t microrl_shell;
// cli_print("@@*** Super cool device, version 1.2.3, for help type help... ***@@\r\n");
microrl_init(µrl_shell, cli_print);
// set callback for execute
microrl_set_execute_callback(µrl_shell, execute);
// set callback for completion (optionally)
microrl_set_complete_callback(µrl_shell, complete);
// set callback for ctrl+c handling (optionally)
microrl_set_sigint_callback(µrl_shell, sigint);
while (TRUE){
// put received char from stdin to microrl lib
msg_t c = sdGetTimeout(shell_sdp, MS2ST(50));
if (c != Q_TIMEOUT)
microrl_insert_char(µrl_shell, (char)c);
/* умираем по всем правилам, не забываем убить потомков */
if (chThdShouldTerminate()){
if ((current_cmd_tp != NULL) && (current_cmd_tp->p_state != THD_STATE_FINAL)){
chThdTerminate(current_cmd_tp);
chThdWait(current_cmd_tp);
}
chThdExit(0);
}
}
return 0;
}
void CcUnpackCycle(SerialDriver *sdp){
mavlink_message_t msg;
mavlink_status_t status;
msg_t c = 0;
msg_t prev_c = 0;
while (!chThdShouldTerminate()) {
if (!cc_port_ready()){
chThdSleepMilliseconds(50);
continue;
}
else{
/* Try to get an escaped with DLE symbols message */
c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50));
if (c == Q_TIMEOUT)
continue;
prev_c = c;
if (prev_c == DLE){
prev_c = 0; /* set it to any just not DLE nor ETX */
c = sdGetTimeout((SerialDriver *)sdp, MS2ST(50));
if (c == Q_TIMEOUT)
continue;
}
}
if (mavlink_parse_char(MAVLINK_COMM_0, (uint8_t)c, &msg, &status)) {
switch(msg.msgid){
case MAVLINK_MSG_ID_COMMAND_LONG:
mavlink_msg_command_long_decode(&msg, &mavlink_command_long_struct);
if (mavlink_command_long_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_command_long, EVMSK_MAVLINK_COMMAND_LONG);
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
mavlink_msg_param_request_read_decode(&msg, &mavlink_param_request_read_struct);
if (mavlink_param_request_read_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_param_request_read, EVMSK_MAVLINK_PARAM_REQUEST_READ);
break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
mavlink_msg_param_request_list_decode(&msg, &mavlink_param_request_list_struct);
if (mavlink_param_request_list_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_param_request_list, EVMSK_MAVLINK_PARAM_REQUEST_LIST);
break;
case MAVLINK_MSG_ID_PARAM_SET:
mavlink_msg_param_set_decode(&msg, &mavlink_param_set_struct);
if (mavlink_param_set_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_param_set, EVMSK_MAVLINK_PARAM_SET);
break;
case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_CC:
mavlink_msg_oblique_storage_request_cc_decode(&msg, &mavlink_oblique_storage_request_cc_struct);
if (mavlink_oblique_storage_request_cc_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_CC);
break;
case MAVLINK_MSG_ID_OBLIQUE_STORAGE_REQUEST_COUNT_CC:
mavlink_msg_oblique_storage_request_count_cc_decode(&msg, &mavlink_oblique_storage_request_count_cc_struct);
if (mavlink_oblique_storage_request_count_cc_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_oblique_storage_request_count_cc, EVMSK_MAVLINK_OBLIQUE_STORAGE_REQUEST_COUNT_CC);
break;
case MAVLINK_MSG_ID_HEARTBEAT_CC:
mavlink_msg_heartbeat_cc_decode(&msg, &mavlink_heartbeat_cc_struct);
// if (mavlink_heartbeat_cc_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_heartbeat_cc, EVMSK_MAVLINK_HEARTBEAT_CC);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
mavlink_msg_statustext_decode(&msg, &mavlink_statustext_struct);
// if (mavlink_statustext_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_statustext, EVMSK_MAVLINK_STATUSTEXT);
break;
case MAVLINK_MSG_ID_OBLIQUE_AGPS:
mavlink_msg_oblique_agps_decode(&msg, &mavlink_oblique_agps_struct);
if (mavlink_oblique_agps_struct.target_system == mavlink_system_struct.sysid)
chEvtBroadcastFlags(&event_mavlink_oblique_agps, EVMSK_MAVLINK_OBLIQUE_AGPS);
break;
default:
break;
}
}
}
}
int main(void)
{
bool_t isUserRun = FALSE;
bool_t displayCountdown = TRUE;
uint16_t bootByte;
_ee_getReserved(_AI_EE_RES_ADDR_BOOT, &bootByte);
if (button_get(0) && button_get(1))
{
// Both buttons are pressed so the user
// wants to run the bootloader.
isUserRun = TRUE;
}
else if (bootByte != _AI_EE_RES_VAL_BOOT_RUN)
{
startProgram();
}
_ee_putReserved(_AI_EE_RES_ADDR_BOOT, _AI_EE_RES_VAL_DEFAULT);
led_on(0);
led_on(1);
int i, j;
for (i = 0; i < BOOT_TIMEOUT; i++)
{
if (isUserRun)
{
// flash LED1
if (i % 100 == 0)
led_toggle(1);
// update the countdown
if (i % 1000 == 0)
{
lcd_clear();
lcd_printf("Aithon Board\n%d", (BOOT_TIMEOUT-i)/1000);
displayCountdown = TRUE;
}
// show the bootloader build date if button 0 pressed
if (button_get(0) && displayCountdown) {
if (i > (.1 * BOOT_TIMEOUT)) {
lcd_clear();
lcd_printf(DATE);
displayCountdown = FALSE;
}
}
}
// check all the interfaces for a SYNC
for (j = 0; j < NUM_INTERFACES; j++)
{
if (sdGetTimeout(_interfaces[j], TIME_IMMEDIATE) == SYNC)
{
_interface = _interfaces[j];
updateProgram();
// We should never get here...
while(1);
}
}
chThdSleepMilliseconds(1);
}
startProgram();
return 0;
}
