void MavlinkProcessor::tryToConnectToAPM() {
uint16_t len = 0;
if (millis() - heartbeat_timer > 1500UL) {
heartbeat_timer = millis();
if (!is_connected) { // Start requesting data streams from MavLink
digitalWrite(alive_led_pin,HIGH);
mavlink_msg_request_data_stream_pack(0xFF, 0xBE, &msg, 1, 1, MAV_DATA_STREAM_EXTENDED_STATUS,
MSG_RATE, START);
len = mavlink_msg_to_send_buffer(buf, &msg);
MavLinkSerial.write(buf, len);
delay(10);
mavlink_msg_request_data_stream_pack(0xFF, 0xBE, &msg, 1, 1, MAV_DATA_STREAM_EXTRA2,
MSG_RATE, START);
len = mavlink_msg_to_send_buffer(buf, &msg);
MavLinkSerial.write(buf, len);
delay(10);
mavlink_msg_request_data_stream_pack(0xFF, 0xBE, &msg, 1, 1, MAV_DATA_STREAM_RAW_SENSORS,
MSG_RATE, START);
len = mavlink_msg_to_send_buffer(buf, &msg);
MavLinkSerial.write(buf,len);
digitalWrite(alive_led_pin, LOW);
}
}
if((millis() - connection_timer) > 1500) { // if no HEARTBEAT from APM in 1.5s then we are not connected
is_connected = false;
heartbeat_count = 0;
}
}
void Pixhawk_Interface::
read_RC_raw(mavlink_message_t msg)
{
mavlink_msg_request_data_stream_pack(id.sysid, id.compid, &msg, 1, 1, MAV_DATA_STREAM_RC_CHANNELS, 10, 1);
serial_port->write_message(msg);
}
void request_data_stream(short stream_id, short mav_speed, unsigned char mav_mode) {
mavlink_message_t smsg;
uint8_t sbuf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_request_data_stream_pack(0, MAV_COMP_ID_IMU, &smsg, 1, 0, stream_id, mav_speed, mav_mode);
uint16_t len = mavlink_msg_to_send_buffer(sbuf, &smsg);
if (write(tty_fd, sbuf, len) != len) {perror("LOOP: Es konnte kein Datenstream abbonniert werden."); exit(1);}
}
void MavLinkInterpreter::ConfigureConnection()
{
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tSetting up Mavlink streams\r\n", millis());
debugSerial.printf("%d\tSerial available for write? %d bytes avail\r\n", millis(), _MavLinkSerial.availableForWrite());
#endif
digitalWrite(LED_BOARD_P, HIGH);
// mavlink_msg_request_data_stream_pack(0xFF,0xBE,&_msg,1,1,MAV_DATA_STREAM_EXTENDED_STATUS, MSG_RATE, START);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_EXTENDED_STATUS, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_EXTENDED_STATUS request\r\n", millis());
#endif
delay(10);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_EXTRA2, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_EXTRA2 request\r\n", millis());
#endif
delay(10);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_RAW_SENSORS, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_RAW_SENSORS request\r\n", millis());
#endif
#ifdef USE_RC_CHANNELS
delay(10);
mavlink_msg_request_data_stream_pack(_mavlink_system.sysid, _mavlink_system.compid, &_msg, AP_SYSID, AP_CMPID, MAV_DATA_STREAM_RC_CHANNELS, MSG_RATE, START);
_len = mavlink_msg_to_send_buffer(_buf, &_msg);
_MavLinkSerial.write(_buf, _len);
#ifdef DEBUG_APM_CONNECTION
debugSerial.printf("%d\tWrote MAV_DATA_STREAM_RC_CHANNELS request\r\n", millis());
#endif
#endif
digitalWrite(LED_BOARD_P, LOW);
_send_mavlink_connection_config++;
}
void Quadcopter::setHeadingStreamSpeed(int i){
mavlink_msg_request_data_stream_pack(
SYSTEMID,
COMPONENTID,
&message,
TARGET_SYSTEMID,
TARGET_COMPONENTID,
MAV_DATA_STREAM_RAW_CONTROLLER,
i,
true);
}
void mavlink_start_feeds (void) {
mavlink_message_t msg;
mavlink_timeout = 0;
SDL_Log("mavlink: starting feeds!\n");
mavlink_msg_param_request_list_pack(127, 0, &msg, ModelData.sysid, ModelData.compid);
mavlink_send_message(&msg);
SDL_Delay(10);
mavlink_msg_request_data_stream_pack(127, 0, &msg, ModelData.sysid, ModelData.compid, MAV_DATA_STREAM_RAW_SENSORS, MAV_DATA_STREAM_RAW_SENSORS_RATE, MAV_DATA_STREAM_RAW_SENSORS_ACTIVE);
mavlink_send_message(&msg);
SDL_Delay(10);
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT || ModelData.teletype == TELETYPE_HARAKIRIML) {
mavlink_msg_request_data_stream_pack(127, 0, &msg, ModelData.sysid, ModelData.compid, MAV_DATA_STREAM_EXTENDED_STATUS, MAV_DATA_STREAM_EXTENDED_STATUS_RATE, MAV_DATA_STREAM_EXTENDED_STATUS_ACTIVE);
mavlink_send_message(&msg);
SDL_Delay(10);
mavlink_msg_request_data_stream_pack(127, 0, &msg, ModelData.sysid, ModelData.compid, MAV_DATA_STREAM_RAW_CONTROLLER, MAV_DATA_STREAM_RAW_CONTROLLER_RATE, MAV_DATA_STREAM_RAW_CONTROLLER_ACTIVE);
mavlink_send_message(&msg);
SDL_Delay(10);
mavlink_msg_request_data_stream_pack(127, 0, &msg, ModelData.sysid, ModelData.compid, MAV_DATA_STREAM_POSITION, MAV_DATA_STREAM_POSITION_RATE, MAV_DATA_STREAM_POSITION_ACTIVE);
mavlink_send_message(&msg);
SDL_Delay(10);
mavlink_msg_request_data_stream_pack(127, 0, &msg, ModelData.sysid, ModelData.compid, MAV_DATA_STREAM_EXTRA1, MAV_DATA_STREAM_EXTRA1_RATE, MAV_DATA_STREAM_EXTRA1_ACTIVE);
mavlink_send_message(&msg);
SDL_Delay(10);
mavlink_msg_request_data_stream_pack(127, 0, &msg, ModelData.sysid, ModelData.compid, MAV_DATA_STREAM_EXTRA2, MAV_DATA_STREAM_EXTRA2_RATE, MAV_DATA_STREAM_EXTRA2_ACTIVE);
mavlink_send_message(&msg);
SDL_Delay(10);
}
}
static bool
setup_data_stream(struct sol_mavlink *mavlink)
{
mavlink_message_t msg = { 0 };
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len;
SOL_NULL_CHECK(mavlink, false);
mavlink_msg_request_data_stream_pack
(0, 0, &msg, mavlink->sysid, mavlink->compid,
MAV_DATA_STREAM_ALL, 1, 1);
len = mavlink_msg_to_send_buffer(buf, &msg);
return write(mavlink->fd, buf, len) == len;
}
void MavlinkAppLayer::request_data_stream(const unsigned int target_system,
const unsigned int target_component,
const unsigned int stream_id,
const unsigned int rate) const {
Lock tx_lock(tx_mav_mutex);
mavlink_msg_request_data_stream_pack(system_id(),
component_id,
&tx_mav_msg,
target_system,
target_component,
stream_id,
rate,
rate > 0 ? 1 : 0);
send(tx_mav_msg);
}
static void mavlink_request_data_stream(unsigned char id, unsigned char rate)
{
mavlink_message_t msg;
unsigned char i, start_stop = (rate == 0) ? 0 : 1;
if (id >= sizeof(mavlink_stream_map))
return;
if (id == 0) {
for (i = 0; i < sizeof(mavlink_stream_map)-1; i++)
config.mav.streams[i] = rate;
} else {
config.mav.streams[id - 1] = rate;
}
mavlink_msg_request_data_stream_pack(config.mav.osd_sysid, MAV_COMP_ID_PERIPHERAL, &msg,
config.mav.uav_sysid, MAV_COMP_ID_ALL,
mavlink_stream_map[id], rate, start_stop);
mavlink_send_msg(&msg);
}
bool MAVLink::sendRequestStream(uint8_t streamId, uint16_t steamRate, uint8_t startStop) {
mavlink_message_t msg;
mavlink_msg_request_data_stream_pack(mySystemId, myComponentId, &msg, targetSystemId, targetComponentId, streamId, steamRate, startStop);
return sendMessage(msg);
}
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;
}
void mavlink_stop_feeds (void) {
SDL_Log("mavlink: stopping feeds!\n");
mavlink_message_t msg1;
mavlink_msg_request_data_stream_pack(127, 0, &msg1, ModelData.sysid, ModelData.compid, MAV_DATA_STREAM_ALL, 0, 0);
mavlink_send_message(&msg1);
}
