void _MavlinkInterface::requestDataStream(uint8_t stream_id, int rate)
{
mavlink_message_t message;
mavlink_request_data_stream_t ds;
ds.target_system = system_id;
ds.target_component = autopilot_id;
ds.req_stream_id = stream_id;
ds.req_message_rate = rate;
ds.start_stop = 1;
mavlink_msg_request_data_stream_encode(system_id, companion_id, &message,&ds);
writeMessage(message);
return;
}
void _Mavlink::requestDataStream(uint8_t stream_id, int rate)
{
mavlink_message_t message;
mavlink_request_data_stream_t ds;
ds.target_system = m_systemID;
ds.target_component = m_targetComponentID;
ds.req_stream_id = stream_id;
ds.req_message_rate = rate;
ds.start_stop = 1;
mavlink_msg_request_data_stream_encode(m_systemID, m_componentID, &message,
&ds);
writeMessage(message);
LOG_I("<- REQUEST_DATA_STREAM");
return;
}
void ofxMavlink::requestDataStreams() {
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_request_data_stream_t packet;
mavlink_message_t message;
packet.target_component = compId;
packet.target_system = targetId;
packet.req_message_rate = 20;
packet.req_stream_id = MAV_DATA_STREAM_ALL;
packet.start_stop = 1;
mavlink_msg_request_data_stream_encode(sysId, compId, &message, &packet);
unsigned len = mavlink_msg_to_send_buffer(buf, &message);
/* write packet via serial link */
int returnval = write(fd, buf, len);
if(returnval < 0) ofLogError("ofxMavlink") << "Error writing to serial port";
/* wait until all data has been written */
tcdrain(fd);
}
// ------------------------------------------------------------------------------
// STARTUP
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
start()
{
int result;
// --------------------------------------------------------------------------
// CHECK SERIAL PORT
// --------------------------------------------------------------------------
if ( not serial_port->status == 1 ) // SERIAL_PORT_OPEN
{
fprintf(stderr,"ERROR: serial port not open\n");
throw 1;
}
// --------------------------------------------------------------------------
// READ THREAD
// --------------------------------------------------------------------------
printf("START READ THREAD \n");
result = pthread_create( &read_tid, NULL, &start_autopilot_interface_read_thread, this );
if ( result ) throw result;
// now we're reading messages
printf("\n");
// --------------------------------------------------------------------------
// CHECK FOR MESSAGES
// --------------------------------------------------------------------------
printf("CHECK FOR MESSAGES\n");
while ( not current_messages.sysid )
{
if ( time_to_exit )
return;
usleep(500000); // check at 2Hz
}
printf("Found\n");
// now we know autopilot is sending messages
printf("\n");
// --------------------------------------------------------------------------
// GET SYSTEM and COMPONENT IDs
// --------------------------------------------------------------------------
// This comes from the heartbeat, which in theory should only come from
// the autopilot we're directly connected to it. If there is more than one
// vehicle then we can't expect to discover id's like this.
// In which case set the id's manually.
// System ID
if ( not system_id )
{
system_id = current_messages.sysid;
printf("GOT VEHICLE SYSTEM ID: %i\n", system_id );
}
// Component ID
if ( not autopilot_id )
{
autopilot_id = current_messages.compid;
printf("GOT AUTOPILOT COMPONENT ID: %i\n", autopilot_id);
printf("\n");
}
////////////////////////////////////////////////////////////////////////////////
mavlink_message_t message;
mavlink_request_data_stream_t req;
req.req_message_rate = 1;
req.req_stream_id = MAV_DATA_STREAM_ALL;
//req.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS;
req.start_stop = 1;
req.target_system = current_messages.sysid;
req.target_component = current_messages.compid;
mavlink_msg_request_data_stream_encode(system_id, companion_id, &message, &req);
// Send the message
int len = serial_port->write_message(message);
////////////////////////////////////////////////////////////////////////////////
// --------------------------------------------------------------------------
// GET INITIAL POSITION
// --------------------------------------------------------------------------
// Wait for initial position ned
// while ( not ( current_messages.time_stamps.local_position_ned &&
while ( not ( current_messages.time_stamps.raw_imu &&
current_messages.time_stamps.attitude ) )
{
if ( time_to_exit )
return;
usleep(500000);
}
// copy initial position ned
Mavlink_Messages local_data = current_messages;
initial_position.x = local_data.local_position_ned.x;
initial_position.y = local_data.local_position_ned.y;
initial_position.z = local_data.local_position_ned.z;
initial_position.vx = local_data.local_position_ned.vx;
initial_position.vy = local_data.local_position_ned.vy;
initial_position.vz = local_data.local_position_ned.vz;
initial_position.yaw = local_data.attitude.yaw;
initial_position.yaw_rate = local_data.attitude.yawspeed;
printf("INITIAL POSITION XYZ = [ %.4f , %.4f , %.4f ] \n", initial_position.x, initial_position.y, initial_position.z);
printf("INITIAL POSITION YAW = %.4f \n", initial_position.yaw);
printf("\n");
// we need this before starting the write thread
// --------------------------------------------------------------------------
// WRITE THREAD
// --------------------------------------------------------------------------
printf("START WRITE THREAD \n");
result = pthread_create( &write_tid, NULL, &start_autopilot_interface_write_thread, this );
if ( result ) throw result;
// wait for it to be started
// while ( not writing_status )
while ( true )
usleep(100000); // 10Hz
// now we're streaming setpoint commands
printf("\n");
// Done!
return;
}
