{

static struct timeval _time_stamp;

gettimeofday(&_time_stamp, NULL);

return _time_stamp.tv_sec*1000000 + _time_stamp.tv_usec;

{

tcgetattr(0,&initial_settings);

new_settings = initial_settings;

new_settings.c_lflag |= ICANON;

new_settings.c_lflag |= ECHO;

new_settings.c_lflag |= ISIG;

new_settings.c_cc[VMIN] = 1;

new_settings.c_cc[VTIME] = 0;

tcsetattr(0, TCSANOW, &new_settings);

{

unsigned char ch;

int nread;

if (peek_character != -1) return 1;

new_settings.c_cc[VMIN]=0;

tcsetattr(0, TCSANOW, &new_settings);

nread = read(0,&ch,1);

new_settings.c_cc[VMIN]=1;

tcsetattr(0, TCSANOW, &new_settings);

if(nread == 1)

{

peek_character = ch;

return 1;

}

return 0;

{

char ch;

if(peek_character != -1)

{

ch = peek_character;

peek_character = -1;

return ch;

}

read(0,&ch,1);

return ch;

{

sp.type_mask =

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION;

sp.coordinate_frame = MAV_FRAME_LOCAL_NED;

sp.x = x;

sp.y = y;

sp.z = z;

printf("POSITION SETPOINT XYZ = [ %.4f , %.4f , %.4f ] \n", sp.x, sp.y, sp.z);

{

sp.type_mask =

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY ;

sp.coordinate_frame = MAV_FRAME_LOCAL_NED;

sp.vx = vx;

sp.vy = vy;

sp.vz = vz;

//printf("VELOCITY SETPOINT UVW = [ %.4f , %.4f , %.4f ] \n", sp.vx, sp.vy, sp.vz);

{

// NOT IMPLEMENTED

fprintf(stderr,"set_acceleration doesn't work yet \n");

throw 1;

sp.type_mask =

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_ACCELERATION &

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY ;

sp.coordinate_frame = MAV_FRAME_LOCAL_NED;

sp.afx = ax;

sp.afy = ay;

sp.afz = az;

{

sp.type_mask &=

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_ANGLE ;

sp.yaw = yaw;

printf("POSITION SETPOINT YAW = %.4f \n", sp.yaw);

{

sp.type_mask &=

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE ;

sp.yaw_rate = yaw_rate;

{

// initialize attributes

write_count = 0;

reading_status = 0; // whether the read thread is running

writing_status = 0; // whether the write thread is running

listening_status = 0;

receive_flag = 0;

control_status = 0; // whether the autopilot is in offboard control mode

time_to_exit = false; // flag to signal thread exit

read_tid = 0; // read thread id

write_tid = 0; // write thread id

system_id = 0; // system id

autopilot_id = 0; // autopilot component id

companion_id = 0; // companion computer component id

current_messages.sysid = system_id;

current_messages.compid = autopilot_id;

serial_port = serial_port_; // serial port management object

{

bool success; // receive success flag

bool received_all = false; // receive only one message

Time_Stamps this_timestamps;

// fprintf(fd," xacc yacc zacc xgyro ygyro zgyro xmag ymag zmag altitude temperature\n");

// Blocking wait for new data

while ( !received_all and !time_to_exit )

{

// ----------------------------------------------------------------------

// READ MESSAGE

// ----------------------------------------------------------------------

mavlink_message_t message;

success = serial_port->read_message(message);

// printf("messageid = %d\n", message.msgid);

printf("flag = %d ch = %d \n",receive_flag,receive_ch);

// ----------------------------------------------------------------------

// HANDLE MESSAGE

// ----------------------------------------------------------------------

if( success )

{

// Store message sysid and compid.

// Note this doesn't handle multiple message sources.

current_messages.sysid = message.sysid;

current_messages.compid = message.compid;

// Handle Message ID

switch (message.msgid)

{

case MAVLINK_MSG_ID_HEARTBEAT:

{

//printf("MAVLINK_MSG_ID_HEARTBEAT\n");

mavlink_msg_heartbeat_decode(&message, &(current_messages.heartbeat));

current_messages.time_stamps.heartbeat = get_time_usec();

this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;

break;

}

case MAVLINK_MSG_ID_SYS_STATUS:

{

//printf("MAVLINK_MSG_ID_SYS_STATUS\n");

mavlink_msg_sys_status_decode(&message, &(current_messages.sys_status));

current_messages.time_stamps.sys_status = get_time_usec();

this_timestamps.sys_status = current_messages.time_stamps.sys_status;

break;

}

case MAVLINK_MSG_ID_BATTERY_STATUS:

{

//printf("MAVLINK_MSG_ID_BATTERY_STATUS\n");

mavlink_msg_battery_status_decode(&message, &(current_messages.battery_status));

current_messages.time_stamps.battery_status = get_time_usec();

this_timestamps.battery_status = current_messages.time_stamps.battery_status;

break;

}

case MAVLINK_MSG_ID_RADIO_STATUS:

{

//printf("MAVLINK_MSG_ID_RADIO_STATUS\n");

mavlink_msg_radio_status_decode(&message, &(current_messages.radio_status));

current_messages.time_stamps.radio_status = get_time_usec();

this_timestamps.radio_status = current_messages.time_stamps.radio_status;

break;

}

case MAVLINK_MSG_ID_LOCAL_POSITION_NED:

{

//printf("MAVLINK_MSG_ID_LOCAL_POSITION_NED\n");

mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));

//fprintf(fd,"x = %f y = %f z = %f \n",current_messages.local_position_ned.x,current_messages.local_position_ned.y,current_messages.local_position_ned.z);

current_messages.time_stamps.local_position_ned = get_time_usec();

this_timestamps.local_position_ned = current_messages.time_stamps.local_position_ned;

break;

}

case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:

{

//printf("MAVLINK_MSG_ID_GLOBAL_POSITION_INT\n");

mavlink_msg_global_position_int_decode(&message, &(current_messages.global_position_int));

current_messages.time_stamps.global_position_int = get_time_usec();

this_timestamps.global_position_int = current_messages.time_stamps.global_position_int;

break;

}

case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:

{

//printf("MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED\n");

mavlink_msg_position_target_local_ned_decode(&message, &(current_messages.position_target_local_ned));

current_messages.time_stamps.position_target_local_ned = get_time_usec();

this_timestamps.position_target_local_ned = current_messages.time_stamps.position_target_local_ned;

break;

}

case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:

{

//printf("MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT\n");

mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_int));

fprintf(fd, "lat_int = %d\n", current_messages.position_target_global_int.lat_int);

current_messages.time_stamps.position_target_global_int = get_time_usec();

this_timestamps.position_target_global_int = current_messages.time_stamps.position_target_global_int;

break;

}

case MAVLINK_MSG_ID_HIGHRES_IMU:

{

//printf("MAVLINK_MSG_ID_HIGHRES_IMU\n");

mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));

// fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);

// fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);

// fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);

// fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);

if (receive_flag && receive_ch == 10) {

fprintf(fd,"acc: %f %f %f ",current_messages.highres_imu.xacc,current_messages.highres_imu.yacc,current_messages.highres_imu.zacc);

fprintf(fd,"gyro: %f %f %f ",current_messages.highres_imu.xgyro,current_messages.highres_imu.ygyro,current_messages.highres_imu.zgyro);

fprintf(fd,"mag: %f %f %f ",current_messages.highres_imu.xmag,current_messages.highres_imu.ymag,current_messages.highres_imu.zmag);

fprintf(fd,"alt: %f %f\n",current_messages.highres_imu.pressure_alt,current_messages.highres_imu.temperature);

receive_flag = false;

}

current_messages.time_stamps.highres_imu = get_time_usec();

this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;

break;

}

case MAVLINK_MSG_ID_GPS_RAW_INT:

{

mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));

fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);

fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);

if (receive_flag && receive_ch == 10) {

fprintf(fd, "lat = %d lon = %d alt = %d ", current_messages.gps_raw_int.lat,current_messages.gps_raw_int.lon,current_messages.gps_raw_int.alt);

fprintf(fd, "vel = %d cog = %d satellites_visible = %d\n", current_messages.gps_raw_int.vel,current_messages.gps_raw_int.cog,current_messages.gps_raw_int.satellites_visible);

receive_flag = false;

}

break;

}

case MAVLINK_MSG_ID_ATTITUDE:

{

//printf("MAVLINK_MSG_ID_ATTITUDE\n");

mavlink_msg_attitude_decode(&message, &(current_messages.attitude));

current_messages.time_stamps.attitude = get_time_usec();

this_timestamps.attitude = current_messages.time_stamps.attitude;

break;

}

default:

{

// printf("Warning, did not handle message id %i\n",message.msgid);

break;

}

} // end: switch msgid

} // end: if read message

// Check for receipt of all items

received_all =

this_timestamps.heartbeat &&

// this_timestamps.battery_status &&

// this_timestamps.radio_status &&

// this_timestamps.local_position_ned &&

// this_timestamps.global_position_int &&

// this_timestamps.position_target_local_ned &&

// this_timestamps.position_target_global_int &&

// this_timestamps.highres_imu &&

// this_timestamps.attitude &&

this_timestamps.sys_status

;

// give the write thread time to use the port

if ( writing_status > false ) {

usleep(100); // look for components of batches at 10kHz

}

} // end: while not received all

return;

{

// do the write

int len = serial_port->write_message(message);

// book keep

write_count++;

// Done!

return len;

{

// --------------------------------------------------------------------------

// PACK PAYLOAD

// --------------------------------------------------------------------------

// pull from position target

mavlink_set_position_target_local_ned_t sp = current_setpoint;

// double check some system parameters

if ( not sp.time_boot_ms )

sp.time_boot_ms = (uint32_t) (get_time_usec()/1000);

sp.target_system = system_id;

sp.target_component = autopilot_id;

// --------------------------------------------------------------------------

// ENCODE

// --------------------------------------------------------------------------

mavlink_message_t message;

mavlink_msg_set_position_target_local_ned_encode(system_id, companion_id, &message, &sp);

// --------------------------------------------------------------------------

// WRITE

// --------------------------------------------------------------------------

// do the write

int len = write_message(message);

// check the write

if ( len <= 0 )

fprintf(stderr,"WARNING: could not send POSITION_TARGET_LOCAL_NED \n");

// else

// printf("%lu POSITION_TARGET = [ %f , %f , %f ] \n", write_count, position_target.x, position_target.y, position_target.z);

return;

{

// --------------------------------------------------------------------------

// PACK PAYLOAD

// --------------------------------------------------------------------------

// pull from position target

mavlink_set_position_target_global_int_t sp = current_setpoint_global;

// double check some system parameters

if ( not sp.time_boot_ms )

sp.time_boot_ms = (uint32_t) (get_time_usec()/1000);

sp.target_system = system_id;

sp.target_component = autopilot_id;

// --------------------------------------------------------------------------

// ENCODE

// --------------------------------------------------------------------------

mavlink_message_t message;

mavlink_msg_set_position_target_global_int_encode(system_id, companion_id, &message, &sp);

// --------------------------------------------------------------------------

// WRITE

// --------------------------------------------------------------------------

// do the write

int len = write_message(message);

// check the write

if ( len <= 0 )

fprintf(stderr,"WARNING: could not send POSITION_TARGET_GLOBLE_INT \n");

// else

// printf("%lu POSITION_TARGET = [ %f , %f , %f ] \n", write_count, position_target.x, position_target.y, position_target.z);

return;

{

// Should only send this command once

if ( control_status == false )

{

printf("ENABLE OFFBOARD MODE\n");

// ----------------------------------------------------------------------

// TOGGLE OFF-BOARD MODE

// ----------------------------------------------------------------------

// Sends the command to go off-board

int success = toggle_offboard_control( true );

// Check the command was written

if ( success )

control_status = true;

else

{

fprintf(stderr,"Error: off-board mode not set, could not write message\n");

//throw EXIT_FAILURE;

}

printf("\n");

} // end: if not offboard_status

{

// Should only send this command once

if ( control_status == true )

{

printf("DISABLE OFFBOARD MODE\n");

// ----------------------------------------------------------------------

// TOGGLE OFF-BOARD MODE

// ----------------------------------------------------------------------

// Sends the command to stop off-board

int success = toggle_offboard_control( false );

// Check the command was written

if ( success )

control_status = false;

else

{

fprintf(stderr,"Error: off-board mode not set, could not write message\n");

//throw EXIT_FAILURE;

}

printf("\n");

} // end: if offboard_status

{

// Prepare command for off-board mode

mavlink_command_long_t com = { 0 };

com.target_system = system_id;

com.target_component = autopilot_id;

com.command = MAV_CMD_NAV_GUIDED_ENABLE;

com.confirmation = true;

com.param1 = (float) flag; // flag >0.5 => start, <0.5 => stop

// Encode

mavlink_message_t message;

mavlink_msg_command_long_encode(system_id, companion_id, &message, &com);

// Send the message

int len = serial_port->write_message(message);

// Done!

return len;

{

int result;

// --------------------------------------------------------------------------

// CHECK SERIAL PORT

// --------------------------------------------------------------------------

if ( serial_port->status != 1 ) // SERIAL_PORT_OPEN

{

fprintf(stderr,"ERROR: serial port not open\n");

throw 1;

}

//--------------------

// LISTEN_KEYBOARD

//--------------------

printf("START LISTEN KEYBOARD THREAD \n");

result = pthread_create(&listen_tid,NULL,&start_autopilot_interface_listen_thread, this);

if (result) throw result;

printf("\n");

// --------------------------------------------------------------------------

// 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");

}

// --------------------------------------------------------------------------

// GET INITIAL POSITION

// --------------------------------------------------------------------------

// Wait for initial position ned

while ( not ( current_messages.time_stamps.local_position_ned &&

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 )

usleep(100000); // 10Hz

// now we're streaming setpoint commands

printf("\n");

// Done!

return;

{

// --------------------------------------------------------------------------

// CLOSE THREADS

// --------------------------------------------------------------------------

printf("CLOSE THREADS\n");

// signal exit

time_to_exit = true;

// wait for exit

pthread_join(read_tid ,NULL);

pthread_join(write_tid,NULL);

// now the read and write threads are closed

printf("\n");

// still need to close the serial_port separately

{

if ( reading_status != 0 )

{

fprintf(stderr,"read thread already running\n");

return;

}

else

{

read_thread();

return;

}

{

if ( not writing_status == false )

{

fprintf(stderr,"write thread already running\n");

return;

}

else

{

write_thread();

// write_thread_global();

return;

}

{

if ( not listening_status == false )

{

fprintf(stderr,"listen thread already running\n");

return;

}

else

{

// write_thread();

listen_thread();

return;

}

{

disable_offboard_control();

try {

stop();

}

catch (int error) {

fprintf(stderr,"Warning, could not stop autopilot interface\n");

}

{

reading_status = true;

FILE *fd;

fd = fopen("/Users/mac/Desktop/test.txt","w+");

if(fd == NULL){

printf("open file failed!\n");

}

while ( ! time_to_exit )

{

read_messages(fd);

usleep(100000); // Read batches at 10Hz

}

printf("end####################\n");

reading_status = false;

fclose(fd);

return;

{

// signal startup

writing_status = 2;

// prepare an initial setpoint, just stay put

mavlink_set_position_target_local_ned_t sp;

sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY &

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE;

sp.coordinate_frame = MAV_FRAME_LOCAL_NED;

sp.vx = 0.0;

sp.vy = 0.0;

sp.vz = 0.0;

sp.yaw_rate = 0.0;

// set position target

current_setpoint = sp;

// write a message and signal writing

write_setpoint();

writing_status = true;

// Pixhawk needs to see off-board commands at minimum 2Hz,

// otherwise it will go into fail safe

while ( !time_to_exit )

{

usleep(250000); // Stream at 4Hz

write_setpoint();

}

// signal end

writing_status = false;

return;

{

// signal startup

writing_status = 2;

// prepare an initial setpoint, just stay put

mavlink_set_position_target_global_int_t sp;

sp.type_mask = MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_VELOCITY &

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_YAW_RATE &

MAVLINK_MSG_SET_POSITION_TARGET_LOCAL_NED_POSITION;

sp.coordinate_frame = MAV_FRAME_GLOBAL_INT;

sp.lat_int = 39*pow(10,7);

sp.lon_int = 116*pow(10,7);