bool MAVLink::getSystemStatus(uint8_t &mode, uint8_t &nav_mode, uint8_t &status, uint16_t &load, uint16_t &vbat, uint16_t &battery_remaining, uint16_t &packet_drop) { MAVLinkMessage * mm = findMessage(MAVLINK_MSG_ID_SYS_STATUS); if (mm == NULL) { return false; } mode = mavlink_msg_sys_status_get_mode(&mm->msg); nav_mode = mavlink_msg_sys_status_get_nav_mode(&mm->msg); status = mavlink_msg_sys_status_get_status(&mm->msg); load = mavlink_msg_sys_status_get_load(&mm->msg); vbat = mavlink_msg_sys_status_get_vbat(&mm->msg); battery_remaining = mavlink_msg_sys_status_get_battery_remaining(&mm->msg); packet_drop = mavlink_msg_sys_status_get_packet_drop(&mm->msg); return checkTimeout(mm,MAVLINK_STATUS_TIMEOUT); }
int main(int argc, char** argv) { int portNum = -1; char portArgBuf[6]; char fName[0xff]; /// Connect VICON initialize_VICON(); srand ( time(NULL) ); sprintf_s(fName,"data-%d.txt",rand()); fp = fopen(fName,"w"); if(fp == NULL) { printf("Fopen failed!\n"); return -1; } /// Timer Thread function. HANDLE hThread = ::CreateThread(NULL, NULL, ThreadProc, NULL, NULL, NULL); CloseHandle(hThread); if(argc < 2) { printf("Specify port (e.g., COM1)\n"); return -1; } sscanf_s(argv[1],"COM%d",&portNum); if(portNum < 0) { printf("Specify valid port %d\n",portNum); return -1; } else { printf("Port COM%d opening... \n",portNum); sprintf_s(portArgBuf,"COM%d",portNum); } hSerial = CreateFile(portArgBuf, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); if(hSerial == INVALID_HANDLE_VALUE) { if(GetLastError() == ERROR_FILE_NOT_FOUND) { printf("Invalid port.\n"); return -1; } else { printf("ErrorNo %d\n",GetLastError()); return -1; } } dcbSerialParams.DCBlength = sizeof(dcbSerialParams); if(!GetCommState(hSerial,&dcbSerialParams)) { printf("Error getting state\n"); return -1; } //dcbSerialParams.BaudRate = CBR_115200; dcbSerialParams.BaudRate = CBR_57600; dcbSerialParams.ByteSize = 8; dcbSerialParams.StopBits = ONESTOPBIT; dcbSerialParams.Parity = NOPARITY; if(!SetCommState(hSerial,&dcbSerialParams)) { printf("Error setting serial port state\n"); return -1; } timeouts.ReadIntervalTimeout = 50000; timeouts.ReadTotalTimeoutConstant = 50000; timeouts.ReadTotalTimeoutMultiplier = 10000; timeouts.WriteTotalTimeoutConstant = 50000; timeouts.WriteTotalTimeoutMultiplier = 10000; if(!SetCommTimeouts(hSerial,&timeouts)) { printf("Error setting timeout\n"); return -1; } uint8_t szBuff = 0; DWORD dwBytesRead = 0; DWORD dwBytesWritten = 0; /// Mavlink mavlink_status_t lastStatus; lastStatus.packet_rx_drop_count = 0; /// System information mavlink_system_t mavlink_system; mavlink_system.sysid = 255; ///< ID 255 mavlink_system.compid = 0; ///< The component sending the message is the IMU, it could be also a Linux process /// Sending buffer mavlink_message_t sendMsg; uint8_t sendBuf[MAVLINK_MAX_PACKET_LEN]; uint16_t len = 0; printf("Open success.\n"); while(!_kbhit()) { mavlink_message_t message; mavlink_status_t status; uint8_t msgReceived = false; if(!ReadFile(hSerial,&szBuff,1,&dwBytesRead,NULL)) { printf("Read error %d\n",GetLastError()); dwBytesRead = 0; } else { msgReceived = mavlink_parse_char(MAVLINK_COMM_0,szBuff,&message,&status); //printf("%d ",szBuff); //printf("%d bytes read\n",dwBytesRead); //if(mavlink_parse_char(0,szBuff,&message,&status)) //{ //printf("Received message with ID %d, sequence: %d from component %d of system %d\n", message.msgid, message.seq, message.compid, message.sysid); //} if (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) { printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count); } lastStatus = status; } if(msgReceived) { //printf("Received message with ID %d, sequence: %d from component %d of system %d\n", message.msgid, message.seq, message.compid, message.sysid); switch(message.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: if(isLiveChecked == false) { // Request data stream //mavlink_msg_request_data_stream_pack(mavlink_system.sysid, mavlink_system.compid,&sendMsg,message.sysid,message.compid,MAV_DATA_STREAM_RAW_SENSORS,10,1); mavlink_msg_request_data_stream_pack(mavlink_system.sysid, mavlink_system.compid,&sendMsg,message.sysid,message.compid,MAV_DATA_STREAM_EXTRA3,50,1); len = mavlink_msg_to_send_buffer(sendBuf,&sendMsg); if(!WriteFile(hSerial,sendBuf,len,&dwBytesWritten,NULL)) { printf("Send error : %d\n",GetLastError()); } else { printf("Ask success %d/%d\n",len,dwBytesWritten); //isLiveChecked = true; } } break; case MAVLINK_MSG_ID_OPTICAL_FLOW: isLiveChecked = true; mavlink_msg_optical_flow_decode(&message,&optical_flow); break; case MAVLINK_MSG_ID_STATE_CORRECTION: isLiveChecked = true; mavlink_msg_state_correction_decode(&message,&state_error); break; case MAVLINK_MSG_ID_SYS_STATUS: printf("[STATUS] mode:%d\n",mavlink_msg_sys_status_get_status(&message)); break; default: printf("Unknown message %d\n",message.msgid); break; } msgReceived = false; } } CloseHandle(hSerial); fclose(fp); printf("File closed\n"); printf("Terminated"); return 0; }