void handle_attitude(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (has_att_quat)
return;
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
// NED -> ENU (body-fixed)
tf::Quaternion orientation = tf::createQuaternionFromRPY(
att.roll, -att.pitch, -att.yaw);
fill_imu_msg_attitude(imu_msg, orientation,
att.rollspeed,
-att.pitchspeed,
-att.yawspeed);
uas_store_attitude(orientation,
imu_msg->angular_velocity,
imu_msg->linear_acceleration);
// publish data
imu_msg->header.frame_id = frame_id;
imu_msg->header.stamp = ros::Time::now();
imu_pub.publish(imu_msg);
}
void ml_logger_default_write_attitude(ml_logger_t* mll, mavlink_message_t* msg)
{
/**
uint32_t time_boot_ms; ///< Timestamp (milliseconds since system boot)
float roll; ///< Roll angle (rad, -pi..+pi)
float pitch; ///< Pitch angle (rad, -pi..+pi)
float yaw; ///< Yaw angle (rad, -pi..+pi)
float rollspeed; ///< Roll angular speed (rad/s)
float pitchspeed; ///< Pitch angular speed (rad/s)
float yawspeed; ///< Yaw angular speed (rad/s)
**/
if (msg->msgid == MAVLINK_MSG_ID_ATTITUDE)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
fprintf(mll->files.attitude.fs, "%12" PRIu64 " %10d %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f\n",
utils_us_since_epoch(),
att.time_boot_ms,
att.roll,
att.pitch,
att.yaw,
att.rollspeed,
att.pitchspeed,
att.yawspeed
);
fflush(mll->files.attitude.fs);
}
}
void MavlinkBridge::readFromStream() {
int counter = 0;
while (running && connected && read(bridge_tty_fd, &bridge_c, 1) > 0 && counter++ < 1000) {
if (mavlink_parse_char(MAVLINK_COMM_0, bridge_c, &msg, &status)) {
if (msg.msgid == MAVLINK_MSG_ID_STATUSTEXT) {
mavlink_statustext_t s;
mavlink_msg_statustext_decode(&msg, &s);
if (strstr(s.text, "recieved") == 0){
printf("status %s\n", s.text);
}
}
if (msg.msgid == MAVLINK_MSG_ID_ATTITUDE) {
mavlink_attitude_t at;
mavlink_msg_attitude_decode(&msg, &at);
attitude = at;
}
if (msg.msgid == MAVLINK_MSG_ID_LOCAL_POSITION_NED) {
mavlink_local_position_ned_t localPosition_t;
mavlink_msg_local_position_ned_decode(&msg, &localPosition_t);
localPosition = localPosition_t;
}
if (msg.msgid == MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED) {
mavlink_position_target_local_ned_t localPositionTarget_t;
mavlink_msg_position_target_local_ned_decode(&msg, &localPositionTarget_t);
localPositionTarget = localPositionTarget_t;
}
}
}
}
void handleMessage(mavlink_message_t* msg)
{
//struct Location tell_command = {}; // command for telemetry
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_msg_heartbeat_decode(msg, &heartbeat);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_msg_attitude_decode(msg, &attitude);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
mavlink_msg_global_position_int_decode(msg, &position);
break;
}
case MAVLINK_MSG_ID_AHRS: {
mavlink_msg_ahrs_decode(msg, &ahrs);
break;
}
default:
break;
} // end switch
} // end handle mavlink
void MavlinkReceiver::handle_message_attitude(mavlink_message_t *msg)
{
if(if_hil) return;
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg,&att);
printf("attitude received:roll:%.4f,pitch:%.4f,yaw:%.4f\n",att.roll*180./M_PI,att.pitch*180./M_PI,att.yaw*180./M_PI);
//send to ros
uascode::PlaneAttitude pl_att;
pl_att.roll= att.roll;
pl_att.pitch= att.pitch;
pl_att.yaw= att.yaw;
pub_att.publish(pl_att);
}//ends
void Attitude_Mavlink_Handler::handle (mavlink_message_t &msg)
{
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode( &msg, &attitude);
/* printf("\nAttitude Message: CUSTOM_MODE: %u TYPE: %d AUTOPILOT: %u BASE_MODE: %u SYSTEM_STATUS: %u MAVLINK_VERSION: %u\n", */
/* attitude.time_boot_ms, */
/* attitude.roll, */
/* attitude.pitch, */
/* attitude.yaw, */
/* attitude.rollspeed, */
/* attitude.pitchspeed, */
/* attitude.yawspeed); */
}
void handle_attitude(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(msg, &attitude);
attitude_msg.roll = attitude.roll;
attitude_msg.pitch = attitude.pitch;
attitude_msg.yaw = attitude.yaw;
attitude_msg.rollspeed = attitude.rollspeed;
attitude_msg.pitchspeed = attitude.pitchspeed;
attitude_msg.yawspeed = attitude.yawspeed;
attitude_msg.time_boot_ms = attitude.time_boot_ms;
global_pos_.hdg = (int)(attitude.yaw*360/3.14*100);
//position_pub.publish(position_msg); //already published when received position_int
attitude_pub.publish(attitude_msg);
}
void handle_attitude(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_attitude_t state;
mavlink_msg_attitude_decode(msg, &state);
auto attitude_msg = boost::make_shared<geometry_msgs::Vector3Stamped>();
attitude_msg->header.stamp = ros::Time::now();
attitude_msg->vector.x = state.roll*180/3.14;
attitude_msg->vector.y = state.pitch*180/3.14;
attitude_msg->vector.z = state.yaw*180/3.14;
attitude_pub.publish(attitude_msg);
auto angular_velocity_msg = boost::make_shared<geometry_msgs::Vector3Stamped>();
angular_velocity_msg->header.stamp = ros::Time::now();
angular_velocity_msg->vector.x = state.rollspeed;
angular_velocity_msg->vector.y = state.pitchspeed;
angular_velocity_msg->vector.z = state.yawspeed;
angular_velocity_pub.publish(angular_velocity_msg);
}
/**
* @brief -- parse mavlink_msg after the char is processed
* @param msg -- the msg to be parsed
* @return -- NULL
*/
void UAS_comm::parseApmMessage(mavlink_message_t* pMsg){
switch (pMsg->msgid){
case MAVLINK_MSG_ID_HEARTBEAT:
mavlink_heartbeat_t heartbeat;
mavlink_msg_heartbeat_decode(pMsg, &heartbeat);
bypassMessage(chan_gcs, pMsg);
break;
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(pMsg, &attitude);
updateAttitude(attitude.roll,
attitude.pitch,
attitude.yaw,
attitude.rollspeed,
attitude.pitchspeed,
attitude.yawspeed);
bypassMessage(chan_gcs, pMsg);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
mavlink_global_position_int_t global_position;
mavlink_msg_global_position_int_decode(pMsg, &global_position);
updateGlobalPosition(global_position.lat,
global_position.lon,
global_position.alt,
global_position.relative_alt,
global_position.vx,
global_position.vy,
global_position.vz,
global_position.hdg);
bypassMessage(chan_gcs, pMsg);
break;
default:
if (isGcsOpen()) {
bypassMessage(chan_gcs, pMsg); // pass the message to gcs
}
break;
} // end switch
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// 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:
{
std::cout << "MAVLINK_MSG_ID_HEARTBEAT" << std::endl;
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:
{
//std::cout << "MAVLINK_MSG_ID_SYS_STATUS" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_BATTERY_STATUS" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_RADIO_STATUS" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_LOCAL_POSITION_NED" << std::endl;
mavlink_msg_local_position_ned_decode(&message, &(current_messages.local_position_ned));
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:
{
std::cout << "MAVLINK_MSG_ID_GLOBAL_POSITION_INT" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED" << std::endl;
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:
{
std::cout << "MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT" << std::endl;
mavlink_msg_position_target_global_int_decode(&message, &(current_messages.position_target_global_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:
{
std::cout << "MAVLINK_MSG_ID_HIGHRES_IMU" << std::endl;
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_ATTITUDE" << std::endl;
mavlink_msg_attitude_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;
quaternion_from_euler(&quaternion, current_messages.attitude.roll,
current_messages.attitude.pitch, current_messages.attitude.yaw);
std::cout <<
"\troll: " << current_messages.attitude.roll <<
"\tpitch: " << current_messages.attitude.pitch <<
"\tyaw: " << current_messages.attitude.yaw <<
std::endl;
std::cout <<
"\trollspeed: " << current_messages.attitude.rollspeed <<
"\tpitchspeed: " << current_messages.attitude.pitchspeed <<
"\tyawspeed: " << current_messages.attitude.yawspeed <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
break;
}
case MAVLINK_MSG_ID_DEBUG:
{
std::cout << "MAVLINK_MSG_ID_DEBUG" << std::endl;
/* mavlink_msg_debug_decode(&message, &(current_messages.attitude));
current_messages.time_stamps.attitude = get_time_usec();
this_timestamps.attitude = current_messages.time_stamps.attitude;*/
break;
}
case MAVLINK_MSG_ID_STATUSTEXT:
{
std::cout << "MAVLINK_MSG_ID_STATUSTEXT: ";
mavlink_msg_statustext_decode(&message, &(current_messages.statustext));
current_messages.statustext.text[50] = 0;
printf("%d - '%s'\n", current_messages.statustext.severity, current_messages.statustext.text);
current_messages.time_stamps.statustext = get_time_usec();
this_timestamps.statustext = current_messages.time_stamps.statustext;
break;
}
case MAVLINK_MSG_ID_RAW_IMU:
{
std::cout << "MAVLINK_MSG_ID_RAW_IMU:" << std::endl;
mavlink_msg_raw_imu_decode(&message, &(current_messages.raw_imu));
std::cout << "\tacc :\t" << current_messages.raw_imu.xacc <<
"\t" << current_messages.raw_imu.yacc <<
"\t" << current_messages.raw_imu.zacc <<
std::endl;
std::cout << "\tgyro:\t" << current_messages.raw_imu.xgyro <<
"\t" << current_messages.raw_imu.ygyro <<
"\t" << current_messages.raw_imu.zgyro <<
std::endl;
std::cout << "\tmag:\t" << current_messages.raw_imu.xmag <<
"\t" << current_messages.raw_imu.ymag <<
"\t" << current_messages.raw_imu.zmag <<
std::endl;
current_messages.time_stamps.raw_imu = get_time_usec();
this_timestamps.raw_imu = current_messages.time_stamps.raw_imu;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
std::cout << "MAVLINK_MSG_ID_GPS_RAW_INT:" << std::endl;
mavlink_msg_gps_raw_int_decode(&message, &(current_messages.gps_raw_int));
current_messages.time_stamps.gps_raw_int = get_time_usec();
this_timestamps.gps_raw_int = current_messages.time_stamps.gps_raw_int;
std::cout <<
"\tlat: " << current_messages.gps_raw_int.lat <<
"\tlon: " << current_messages.gps_raw_int.lon <<
"\talt: " << current_messages.gps_raw_int.alt <<
std::endl;
std::cout <<
"\teph: " << current_messages.gps_raw_int.eph <<
"\tepv: " << current_messages.gps_raw_int.epv <<
std::endl;
std::cout <<
"\tvel: " << current_messages.gps_raw_int.vel <<
"\tcog: " << current_messages.gps_raw_int.cog <<
std::endl;
std::cout <<
"\tfix: " << (int)current_messages.gps_raw_int.fix_type <<
"\tsat: " << (int)current_messages.gps_raw_int.satellites_visible <<
std::endl;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
vector4d quaternion;
std::cout << "MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:" << std::endl;
mavlink_msg_nav_controller_output_decode(&message, &(current_messages.nav_controller_output));
current_messages.time_stamps.nav_controller_output = get_time_usec();
this_timestamps.nav_controller_output = current_messages.time_stamps.nav_controller_output;
quaternion_from_euler(&quaternion, current_messages.nav_controller_output.nav_roll,
current_messages.nav_controller_output.nav_pitch, current_messages.nav_controller_output.nav_bearing);
std::cout <<
"\tnav_roll: " << current_messages.nav_controller_output.nav_roll <<
"\tnav_pitch: " << current_messages.nav_controller_output.nav_pitch <<
"\tnav_bearing: " << current_messages.nav_controller_output.nav_bearing <<
std::endl;
std::cout <<
"\tqx: " << quaternion.x <<
"\tqy: " << quaternion.y <<
"\tqz: " << quaternion.z <<
"\tqw: " << quaternion.w <<
std::endl;
std::cout <<
"\ttarget_bearing: " << current_messages.nav_controller_output.target_bearing <<
"\twp_dist: " << current_messages.nav_controller_output.wp_dist <<
std::endl;
std::cout <<
"\talt_error: " << current_messages.nav_controller_output.alt_error <<
"\taspd_error: " << current_messages.nav_controller_output.aspd_error <<
"\txtrack_error: " << current_messages.nav_controller_output.xtrack_error <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE:
{
std::cout << "MAVLINK_MSG_ID_SCALED_PRESSURE:" << std::endl;
mavlink_msg_scaled_pressure_decode(&message, &(current_messages.scaled_pressure));
current_messages.time_stamps.scaled_pressure = get_time_usec();
this_timestamps.scaled_pressure = current_messages.time_stamps.scaled_pressure;
std::cout <<
"\tpress_abs: " << current_messages.scaled_pressure.press_abs <<
"\tpress_diff: " << current_messages.scaled_pressure.press_diff <<
"\ttemperature: " << ((double)current_messages.scaled_pressure.temperature / 100.0) <<
std::endl;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{
std::cout << "MAVLINK_MSG_ID_RC_CHANNELS_RAW:" << std::endl;
mavlink_msg_rc_channels_raw_decode(&message, &(current_messages.rc_channels_raw));
current_messages.time_stamps.rc_channels_raw = get_time_usec();
this_timestamps.rc_channels_raw = current_messages.time_stamps.rc_channels_raw;
std::cout <<
"\tport: " << (int)current_messages.rc_channels_raw.port <<
"\tchan1_raw: " << current_messages.rc_channels_raw.chan1_raw <<
"\tchan2_raw: " << current_messages.rc_channels_raw.chan2_raw <<
"\tchan3_raw: " << current_messages.rc_channels_raw.chan3_raw <<
"\tchan4_raw: " << current_messages.rc_channels_raw.chan4_raw <<
"\tchan5_raw: " << current_messages.rc_channels_raw.chan5_raw <<
"\tchan6_raw: " << current_messages.rc_channels_raw.chan6_raw <<
"\tchan7_raw: " << current_messages.rc_channels_raw.chan7_raw <<
"\tchan8_raw: " << current_messages.rc_channels_raw.chan8_raw <<
"\trssi: " << (int)current_messages.rc_channels_raw.rssi <<
std::endl;
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
{
std::cout << "MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:" << std::endl;
mavlink_msg_servo_output_raw_decode(&message, &(current_messages.servo_output_raw));
current_messages.time_stamps.servo_output_raw = get_time_usec();
this_timestamps.servo_output_raw = current_messages.time_stamps.servo_output_raw;
std::cout <<
"\tport: " << (int)current_messages.servo_output_raw.port <<
"\tservo1_raw: " << current_messages.servo_output_raw.servo1_raw <<
"\tservo2_raw: " << current_messages.servo_output_raw.servo2_raw <<
"\tservo3_raw: " << current_messages.servo_output_raw.servo3_raw <<
"\tservo4_raw: " << current_messages.servo_output_raw.servo4_raw <<
"\tservo5_raw: " << current_messages.servo_output_raw.servo5_raw <<
"\tservo6_raw: " << current_messages.servo_output_raw.servo6_raw <<
"\tservo7_raw: " << current_messages.servo_output_raw.servo7_raw <<
"\tservo8_raw: " << current_messages.servo_output_raw.servo8_raw <<
std::endl;
break;
}
case MAVLINK_MSG_ID_VFR_HUD:
{
std::cout << "MAVLINK_MSG_ID_VFR_HUD:" << std::endl;
mavlink_msg_vfr_hud_decode(&message, &(current_messages.vfr_hud));
current_messages.time_stamps.vfr_hud = get_time_usec();
this_timestamps.vfr_hud = current_messages.time_stamps.vfr_hud;
std::cout <<
"\tairspeed: " << current_messages.vfr_hud.airspeed <<
"\tgroundspeed: " << current_messages.vfr_hud.groundspeed <<
"\theading: " << current_messages.vfr_hud.heading <<
std::endl;
std::cout <<
"\tthrottle: " << current_messages.vfr_hud.throttle <<
"\talt: " << current_messages.vfr_hud.alt <<
"\tclimb: " << current_messages.vfr_hud.climb <<
std::endl;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT:
{
std::cout << "MAVLINK_MSG_ID_MISSION_CURRENT: ";
mavlink_msg_mission_current_decode(&message, &(current_messages.mission_current));
current_messages.time_stamps.mission_current = get_time_usec();
this_timestamps.mission_current = current_messages.time_stamps.mission_current;
std::cout <<
"seq: " << current_messages.mission_current.seq <<
std::endl;
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.sys_status &&
// 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 ;
// 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;
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
int success = 0; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
int err_counter = 0;
struct timespec diff_r;
struct timespec aft_read;
struct timespec bef_read;
struct timespec receive_time;
struct timespec longRelNanoSleep;
longRelNanoSleep.tv_sec = 0;
longRelNanoSleep.tv_nsec = 100000;
struct timespec RelNanoSleep;
RelNanoSleep.tv_sec = 0;
RelNanoSleep.tv_nsec = 10;
// Blocking wait for new data
// pthread_mutex_lock(&mut);
// if (blocked == 0 && writing_status)
// {
// blocked = 1;
// pthread_cond_wait(&cond,&mut);
// blocked = 0;
// }
// else
// {
// pthread_cond_signal(&cond);
// }
// clock_gettime(CLOCK_REALTIME,&bef_read);
// clock_gettime(CLOCK_REALTIME,&aft_read);
// timespec_sub(&diff_r,&aft_read,&bef_read);
// int64_t diff_r_us = diff_r.tv_sec*1e6 + diff_r.tv_nsec/1e3;
// printf("Read Time: %ld \n",diff_r_us);
//
// pthread_mutex_unlock(&mut);
mavlink_message_t message;
while ( (success != 1) && (!time_to_exit) )
{
// ----------------------------------------------------------------------
// READ ONE BYTE AT TIME UNTIL COMPLETE MESSAGE IS RECEIVED
// ----------------------------------------------------------------------
success = serial_port->read_message(message); //Have I obtained a complete message?
if ( success == -1) // Errors...retry...
{
err_counter++;
if (err_counter == 3) // To many void read
{
fprintf(stderr,"ERRORS!\n");
break;
}
}
}
err_counter = 0;
if (success == -1) return;
// ----------------------------------------------------------------------
// 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));
base_mode = current_messages.heartbeat.base_mode;
mav_type = current_messages.heartbeat.type;
system_status = current_messages.heartbeat.system_status;
custom_mode = current_messages.heartbeat.custom_mode;
//printf("base_mode = %u\n",base_mode);
current_messages.time_stamps.heartbeat = get_time_usec();
this_timestamps.heartbeat = current_messages.time_stamps.heartbeat;
heartbeat_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.heartbeat - read_heartbeat_old) > 10000000 )
{
printf("HEARTBEAT frequency : %d \n",heartbeat_count/10);
heartbeat_count = 0;
read_heartbeat_old = 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));
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));
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");
clock_gettime(CLOCK_REALTIME,&receive_time);
mavlink_msg_highres_imu_decode(&message, &(current_messages.highres_imu));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
highres_imu_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.highres_imu - read_highres_imu_old) > 10000000 )
{
printf("HIGHRES_IMU frequency : %d Hz\n",highres_imu_count/10);
highres_imu_count = 0;
read_highres_imu_old = current_messages.time_stamps.highres_imu;
}
//fprintf(logfile_rd,"HIGHRES_IMU %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
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;
}
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//printf("MAVLINK_MSG_ID_HIL_CONTROLS\n");
mavlink_msg_hil_controls_decode(&message, &(current_messages.hil_controls));
current_messages.time_stamps.hil_controls = get_time_usec();
this_timestamps.hil_controls = current_messages.time_stamps.hil_controls;
float data[4] = {current_messages.hil_controls.roll_ailerons,
current_messages.hil_controls.pitch_elevator,
current_messages.hil_controls.yaw_rudder,
current_messages.hil_controls.throttle};
int bsent = udp_mtl->send_bytes((char *)data,sizeof(float[4]));
hil_controls_count++;
//printf("IMU Sensors Timestamp %u\n",current_messages.time_stamps.highres_imu/1000);
if ( (current_messages.time_stamps.hil_controls - read_hil_controls_old) > 10000000 )
{
printf("HIL_CONTROLS frequency : %d Hz\n",hil_controls_count/10);
hil_controls_count = 0;
read_hil_controls_old = current_messages.time_stamps.hil_controls;
printf("%0.2f | ",current_messages.hil_controls.roll_ailerons);
printf("%0.2f | ",current_messages.hil_controls.pitch_elevator);
printf("%0.2f | ",current_messages.hil_controls.yaw_rudder);
printf("%0.2f | \n",current_messages.hil_controls.throttle);
}
//fprintf(logfile_rd,"HIL_CONTROLS %d, %ld \n",receive_time.tv_sec, receive_time.tv_nsec);
break;
}
default:
{
// printf("Warning, did not handle message id %i\n",message.msgid);
break;
}
} // end: switch msgid
return;
}
/*Mainloop*/
void loop() {
/*Auswertung des anstehenden Aufgaben im Scheduler*/
int i; unsigned char new_value = 0; unsigned char new_heading = 0;
for (i = 0; i < MAX_TASKS; i++) {
if (scheduler[i].task == NOTHING || get_current_millis() < scheduler[i].tv_msec) {continue;}
switch (scheduler[i].task) {
case NOTHING: break;
case MEASURE:
/*Fordert den als Parameter uebergebenen Sensor auf eine Messung zu starten und bestimmt den Lese-Zeitpunkt*/
srf[scheduler[i].param - SE0_ADDRESS].measure();
schedule_add((long)srf_speed[scheduler[i].param - SE0_ADDRESS], READ_MEASURE, scheduler[i].param);
scheduler[i].task = NOTHING;
break;
case READ_MEASURE: {
unsigned short index = scheduler[i].param - SE0_ADDRESS;
/*Liest die Messdaten des als Parameter uebergebenen Sensors aus und veranlasst erneute Messung*/
srf[index].read_it(get_current_millis());
schedule_add((long)srf[index].get_delay(), MEASURE, scheduler[i].param);
if(state == HOLD_STILL || state == MEASURE_ENVIRONMENT || state == GET_ANCHOR) {
nvalue[index] = 1;
}
/*if (state == HOLD_STILL || state == HEAD_TO_MIDDLE || state = HTM_DELAY) {
nvalue2[scheduler[i].param - SE0_ADDRESS] = 1;
}*/
new_value = scheduler[i].param;
/*Korrektur der Messwerte bei Schieflage des Copters bei glaubhaften Winkeln*/
if ((index == 0 || index == 1) && (abs(current_roll_rad) < MAX_ROLL_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
else if ((index == 2 || index == 3) && (abs(current_pitch_rad) < MAX_PITCH_ANGLE)) srf[index].set_mean((unsigned short)(srf[index].get_mean()*cos(current_roll_rad)));
/*Korrektur der Messwerte, wenn sich ein Sensor im Nahbereich (< SE_MIN_DISTANCE) befindet*/
static unsigned char rm_state[SE_COUNT];
static short rm_min[SE_COUNT];
if (state != IDLE && state != INIT && state != DELAY) {
if (rm_state[index] == 0 && srf[index].get_mean() < SE_MIN_DISTANCE) {
rm_state[index] = 1;
if (index == 0 || index == 2) rm_min[index] = srf[index+1].get_mean() - SE_MIN_DIFF;
else rm_min[index] = srf[index-1].get_mean() - SE_MIN_DIFF;
srf[index].set_mean(SE_MIN);
} else if (rm_state[index] == 1) {
if ((index == 0 || index == 2) && srf[index+1].get_mean() < rm_min[index]) {
rm_state[index] = 0;
} else if ((index == 1 || index == 3) && srf[index-1].get_mean() < rm_min[index]) {
rm_state[index] = 0;
} else {
srf[index].set_mean(SE_MIN);
}
}
}
if (rm_state[0] == 1 && rm_state[1] == 1) {rm_state[0] = 0; rm_state[1] = 0;}
if (rm_state[2] == 1 && rm_state[3] == 1) {rm_state[2] = 0; rm_state[3] = 0;}
if (state == HOLD_STILL || state == GET_ANCHOR || state == HTM_DELAY) {
if (nvalue2[0] == 1 && nvalue2[1] == 1 && nvalue2[2] == 1 && nvalue2[3] == 1) {
slam_insert_v((srf[0].get_cm_per_s() + srf[1].get_cm_per_s())/2,(srf[2].get_cm_per_s() + srf[3].get_cm_per_s())/2,current_heading,get_current_millis());
nvalue2[0] = 0; nvalue2[1] = 0; nvalue2[2] = 0; nvalue2[3] = 0;
}
}
/*Speichert den gelesenen Messwert in der entsprechenden Log-Datei*/
#if LOG > 0
FILE *fd;
if (scheduler[i].param == SE0_ADDRESS) fd = fd_112;
else if (scheduler[i].param == SE1_ADDRESS) fd = fd_113;
else if (scheduler[i].param == SE2_ADDRESS) fd = fd_114;
else fd = fd_115;
fprintf(fd,"%lu\t%u\t%u\n", srf[scheduler[i].param - SE0_ADDRESS].get_msec(), srf[scheduler[i].param - SE0_ADDRESS].get_data(), srf[scheduler[i].param - SE0_ADDRESS].get_mean());
#endif
scheduler[i].task = NOTHING;
break;
}
case SAVE_LOG:
/*Sichert die bisher geloggten Daten*/
#if LOG > 0
fclose(fd_112);
fclose(fd_113);
fclose(fd_114);
fclose(fd_115);
fclose(fd_data);
char tmp_dir[32]; strcpy(tmp_dir,log_dir);
fd_112 = fopen(strcat(tmp_dir,"/112"), "a"); strcpy(tmp_dir,log_dir);
fd_113 = fopen(strcat(tmp_dir,"/113"), "a"); strcpy(tmp_dir,log_dir);
fd_114 = fopen(strcat(tmp_dir,"/114"), "a"); strcpy(tmp_dir,log_dir);
fd_115 = fopen(strcat(tmp_dir,"/115"), "a"); strcpy(tmp_dir,log_dir);
fd_data= fopen(strcat(tmp_dir,"/data"),"a"); strcpy(tmp_dir,log_dir);
schedule_add(LOG_SPEED,SAVE_LOG,0);
#endif
scheduler[i].task = NOTHING;
break;
case CHECK_STILL:
/*Prueft auf Stillstand des Copters*/
if (still && state == HOLD_STILL) {
if (scheduler[i].param < CHECK_STILL_COUNT) {
schedule_add(CHECK_STILL_SPEED,CHECK_STILL,scheduler[i].param+1);
} else {
state = MEASURE_ENVIRONMENT;
still = 0;
schedule_add(CHECK_STILL_SPEED,CHECK_STILL, 0);
}
} else {
schedule_add(CHECK_STILL_SPEED,CHECK_STILL,0);
}
scheduler[i].task = NOTHING;
break;
case CHANGE_STATE:
/*Ändert den aktuellen Status zum als Parameter übergebenen Status*/
state = (states)scheduler[i].param;
if ((states)scheduler[i].param == HOLD_STILL) hs_state = 0;
scheduler[i].task = NOTHING;
break;
case SHOW_ME: {
/*Schreibt Messwerte und Neigungswinkel auf das Terminal*/
char st[16];
switch (state) {
case INIT: sprintf(st, "INIT"); break;
case MEASURE_ENVIRONMENT: sprintf(st, "ME"); break;
case IDLE: sprintf(st, "IDLE"); break;
case HOLD_STILL: sprintf(st, "HOLD_STILL"); break;
case GET_ALIGNMENT: sprintf(st, "GET_ALIGNMENT"); break;
case GET_ANCHOR: sprintf(st, "GET_ANCHOR"); break;
case DELAY: sprintf(st, "DELAY"); break;
default: sprintf(st, "???"); break;
}
if (roll > 9 || roll < 0) printf("roll: %d\tpitch: %d\tyaw: %d\theading: %d\tdhead: %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
else printf("roll: %d\t\tpitch: %d\tyaw: %d\theading: %d\tdhead. %d\tSE0: %d\tSE1: %d\tSE2: %d\tSE3:%d\tstate: %s\n", roll, pitch, yaw, current_heading, desired_heading, srf[0].get_mean(), srf[1].get_mean(), srf[2].get_mean(), srf[3].get_mean(),st);
schedule_add(100,SHOW_ME,0);
scheduler[i].task = NOTHING;
/*Mavlinkkommunikation mit QGroundcontrol (basierend auf Based on http://qgroundcontrol.org/dev/mavlink_linux_integration_tutorial)*/
mavlink_message_t msg;
uint16_t len;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
static int i; i++;
if (i == 10) {
i = 0;
mavlink_msg_heartbeat_pack(0, 0, &msg, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
}
mavlink_msg_debug_vect_pack(0,0,&msg,"DEBUG",0,roll,srf[0].get_mean(),srf[0].get_data());
len = mavlink_msg_to_send_buffer(buf, &msg);
sendto(s, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
break;
}
default: if(WARNINGS){perror("LOOP: Unbekannte Aufgabe im Scheduler.");} break;
}
}
/*Auswertung der Daten, die an der seriellen Schnittstelle anliegen.*/
struct timeval tv; tv.tv_sec = 0; tv.tv_usec = 0;
/*Prueft, ob Daten an der seriellen Schnittstelle anliegen*/
FD_ZERO(&tty_fdset);
FD_SET(tty_fd, &tty_fdset);
if (select(tty_fd+1, &tty_fdset, NULL, NULL, &tv) == -1) {perror("LOOP: select() fehlgeschlagen"); exit(1);}
/*Liest ein einzelnes Byte von der seriellen Schnittstelle und prueft, ob ein Mavlinkpaket vervollstaendigt wurde*/
if (FD_ISSET(tty_fd, &tty_fdset)) {
static mavlink_message_t msg;
static mavlink_status_t status;
char c[1];
if (read(tty_fd,c,1) == -1) {if(WARNINGS){perror("LOOP: Fehler beim Lesen aus " TTY_DEVICE);}}
#if DEBUG_LEVEL > 2
printf("%#x\n",c[0]);
#endif
if (mavlink_parse_char(MAVLINK_COMM_0,(uint8_t) *c, &msg, &status)) {
#if DEBUG_LEVEL > 1
printf("%u Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
#endif
//printf("%lu Mavlinkpaket empfangen: %d\n", get_current_millis(), msg.msgid);
switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
/*Beim Empfang des ersten Heartbeats wird zunaechst ein eventuell vorhandener Datenstream gekuendigt und ein neuer Datenstream abonnemiert*/
mavlink_heartbeat_t hb;
mavlink_msg_heartbeat_decode(&msg,&hb);
if (!first_heartbeat) {
first_heartbeat = 1;
request_data_stream(MAV_DATA_STREAM_ALL,0,0);
}
//if (state == IDLE) {
if (state == IDLE && (hb.custom_mode == 13 /*EXT_CTRL*/ || desktop_build)) {
std::cout << "State changed from IDLE to INIT.\n";
state = INIT;
schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
request_data_stream(MAV_DATA_STREAM_EXTRA2/*VFR_HUD*/,DATA_STREAM_SPEED,1);
init_state = 1;
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
/*Speichert bei Empfang von HUD-Daten die aktuelle Ausrichtung des Copters*/
mavlink_vfr_hud_t hud_data;
mavlink_msg_vfr_hud_decode(&msg, &hud_data);
old_heading = current_heading;
current_heading = hud_data.heading;
new_heading = 1;
break;
case MAVLINK_MSG_ID_ATTITUDE:
mavlink_attitude_t adata;
mavlink_msg_attitude_decode(&msg,&adata);
old_heading = current_heading;
current_heading = ((short)lround(DEG(adata.yaw))+360)%360;
current_pitch_rad = adata.pitch;
current_roll_rad = adata.roll;
new_heading = 1;
break;
case MAVLINK_MSG_ID_RAW_IMU:
/*Speichert die Beschleunigungen auf der x- und y-Achse*/
mavlink_raw_imu_t raw_imu;
mavlink_msg_raw_imu_decode(&msg,&raw_imu);
xacc = raw_imu.xacc;
yacc = raw_imu.yacc;
//slam_insert_acc(xacc,yacc,current_heading,get_current_millis());
std::cout << "xacc: " << xacc << " yacc: " << yacc << "\n";
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
/*Prüft, ob eine Umstellung auf externe Kontrolle erfolgt ist*/
/*if (state != IDLE) {break;}
mavlink_rc_channels_raw_t rc;
mavlink_msg_rc_channels_raw_decode(&msg,&rc);
if (desktop_build) init_state = 1; rc.chan5_raw = MODE_SWITCH_RANGE_UP-1;
if (init_state && rc.chan5_raw < MODE_SWITCH_RANGE_UP && rc.chan5_raw > MODE_SWITCH_RANGE_DOWN) {
std::cout << "State changed from IDLE to INIT.\n";
schedule_add(0,CHANGE_STATE,(int)INIT);
schedule_add(2000,CHANGE_STATE,(int)HOLD_STILL);
request_data_stream(MAV_DATA_STREAM_RC_CHANNELS,0,0);
request_data_stream(MAV_DATA_STREAM_EXTRA2,DATA_STREAM_SPEED,1);
request_data_stream(MAV_DATA_STREAM_RAW_SENSORS,DATA_STREAM_SPEED,1);
}
if(rc.chan5_raw > MODE_SWITCH_RANGE_UP || rc.chan5_raw < MODE_SWITCH_RANGE_DOWN) {
init_state = 1;
}*/
break;
default: break;
}
}
}
/*Berechnung der vorgeschlagenen Werte fuer roll, pitch und yaw an Hand der neuen Messwerte*/
if (state != INIT && !new_value && !new_heading) {/*Wenn keine neuen Daten vorhanden sind -> Abarbeitung überspringen*/return;}
switch (state) {
case IDLE: /*Wartet auf Aktivierung der externen Kontrolle*/ break;
case INIT:
std::cout << "INIT\n";
/*Initialisiert Abarbeitung, wenn externe Kontrolle zum ersten Mal aktiviert wurde*/
schedule_add(5000,CHECK_STILL,0);
#if LOG > 0
schedule_add(LOG_SPEED,SAVE_LOG,0);
//schedule_add(SLAM_SPEED,SAVE_SLAM,0);
#endif
for (int i = 0; i < SE_COUNT; i++) {
if ((int)pow(2,i) & ENABLED_SENSORS) {
schedule_add(0, MEASURE, SE0_ADDRESS + i);
}
}
slam_init(current_heading,get_current_millis());
state = DELAY;
break;
case HOLD_STILL:
if(1) {
static short set_point[SE_COUNT];
if (hs_state == 0) {
hs_state = 1;
set_point[0] = (srf[0].get_mean() + srf[1].get_mean())/2;
set_point[1] = set_point[0];
set_point[2] = (srf[2].get_mean() + srf[3].get_mean())/2;
set_point[3] = set_point[2];
//for (int i = 0; i < SE_COUNT; i++) {
// set_point[i] = srf[i].get_mean();
// std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
//}
/*for (int i = 0; i < SE_COUNT; i++) {
if (set_point[i] < 100 || set_point[i] > 0) {
if (i == 0 || i == 2) {
set_point[i+1] -= (100 - set_point[i]);
} else {
set_point[i-1] -= (100 - set_point[i]);
}
set_point[i] = 100;
}
}*/
for (int i = 0; i < SE_COUNT; i++) {
std::cout << "Set_Point " << i << ":" << set_point[i] << "\n";
}
}
/*Veranlasst den Copter auf Grundlage der Änderungen der Messwerte still zu stehen*/
if (!new_value) break;
if (nvalue[0] == 1 && nvalue[1] == 1) {
if (srf[0].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(0 - srf[1].get_mean() + set_point[1],srf[1].get_msec_diff()),-max_roll,max_roll);
else if (srf[1].get_mean() == SE_MIN) roll = between<short>(pid_roll.get(srf[0].get_mean() - set_point[0],srf[0].get_msec_diff()),-max_roll,max_roll);
else roll = between<short>(pid_roll.get(((srf[0].get_mean() - set_point[0]) - (srf[1].get_mean() - set_point[1]))/2,(srf[0].get_msec_diff() + srf[1].get_msec_diff())/2),-max_roll,max_roll);
nvalue[0] = 0; nvalue[1] = 0;
}
if (nvalue[2] == 1 && nvalue[3] == 1) {
if (srf[2].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(0 - srf[3].get_mean() + set_point[3],srf[3].get_msec_diff()),-max_pitch,max_pitch);
else if (srf[3].get_mean() == SE_MIN) pitch = between<short>(pid_pitch.get(srf[2].get_mean() - set_point[2],srf[2].get_msec_diff()),-max_pitch,max_pitch);
else pitch = between<short>(pid_pitch.get(((srf[2].get_mean() - set_point[2]) - (srf[3].get_mean() - set_point[3]))/2,(srf[2].get_msec_diff() + srf[3].get_msec_diff())/2),-max_pitch,max_pitch);
nvalue[2] = 0; nvalue[3] = 0;
}
yaw = 0;
send_ext_ctrl();
/*Prüfung auf Stillstand*/
if (abs(roll) <= STILL_FAK_ROLL*max_roll/100 && abs(pitch) <= STILL_FAK_PITCH*max_pitch/100) {
//still = 1;
still = 0; /*FIXME*/
} else {
still = 0;
}
}
break;
case MEASURE_ENVIRONMENT:
/*Vermisst die Umgebung durch eine Drehung um 360/SE_COUNT Grad*/
if (breakpoint == 1) {state = HOLD_STILL; hs_state= 0; break;}
if (abs(xacc) > HOLD_STILL_MAX_XACC || abs(yacc) > HOLD_STILL_MAX_YACC) {/*Wurde der Copter zu stark bewegt: Abbruch*/state = HOLD_STILL; hs_state = 0; std::cout << "State changed to HOLD_STILL\n"; break;}
if (first_heading == -1) {
/*Speichert die anfaengliche Ausrichtung und veranlasst den Copter sich zu drehen*/
std::cout << "State changed to MEASURE_ENVIRONMENT\n";
first_heading = current_heading;
roll = 0; pitch = 0; rotation = 0; yaw = rotation_angle_sign*CONST_YAW;
send_ext_ctrl();
}
//if (new_value) slam_insert_measurement(srf[new_value-SE0_ADDRESS].get_mean(),(current_heading + (360/SE_COUNT)*(new_value-SE0_ADDRESS))%360);
/*Berechnung der aktuellen Drehung*/
if (new_heading) {
if (abs(current_heading - old_heading) > 180/SE_COUNT) rotation += current_heading - old_heading - sign(current_heading - old_heading)*360;
else rotation += current_heading - old_heading;
for(int i = 0; i < SE_COUNT; i++) {
if(nvalue[i] == 1) {
env[(current_heading + srf[i].get_alignment()*(360/SE_COUNT))%360] = srf[i].get_mean();
nvalue[i] = 0;
}
}
//if (abs(rotation) >= 360/SE_COUNT) {
if (abs(rotation) >= rotation_angle) {
/*Copter hat sich ausreichend gedreht, Abbruch der Vermessung*/
//state = HEAD_TO_MIDDLE;
state = GET_ALIGNMENT;
roll = 0; pitch = 0; yaw = 0;
send_ext_ctrl();
rotation_angle_sign = sign(rotation);
}
}
break;
case GET_ALIGNMENT: {
/*Bestimme für jeden Sensor die Minimalentfernung der letzten Drehung*/
/*short min_v[SE_COUNT];
short min_h[SE_COUNT];
for (int i = 0; i < SE_COUNT; i++) {
min_v[i] = 0;
min_h[i] = 0;
}*/
short min_v = 0;
for (int i = 0; i < rotation_angle; i++) {
if (min_v < env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360]) {
min_v = env[(first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360];
desired_heading = (first_heading + rotation_angle_sign*i + 360 + srf[align_se].get_alignment()*(360/SE_COUNT))%360;
}
/*for (int j = 0; j < SE_COUNT; j++) {
if (min_v[j] < env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360]) {
min_v[j] = env[(first_heading + rotation_angle_sign*i + j*360/SE_COUNT + 360)%360];
min_h[j] = (first_heading + rotation_angle_sign*i + 360)%360;
}
}*/
}
//desired_heading = round((float)(min_h[0]+min_h[1]+min_h[2]+min_h[3])/4);
/*Bestimmung der künftigen Drehrichtung*/
for (int k = 0; k < rotation_angle/2; k++) {
/*Wenn Minimum in der ersten Hälfte der Drehung gefunden wurde, Wechsel der Drehrichtung bzw. Abbruch*/
if (desired_heading == (first_heading+rotation_angle_sign*k)%360) {
rotation_angle_sign *= -1;
if (init_state == 2) init_state = 3;
break;
}
}
/*Cleanup*/
first_heading = -1; yaw = 0; pid_roll.reset(); pid_pitch.reset();
for(int i = 0; i < 360; i++) env[i] = 0;
if (init_state == 1) init_state = 2;
if (init_state == 3) {
state = GET_ANCHOR;
pid_yaw.reset();
pid_yaw.set(HTM_YAW_KP,HTM_YAW_TN,HTM_YAW_TV);
pid_yaw.set_target(0);
roll = 0; pitch = 0; yaw = 0;
std::cout << "State changed to GET_ANCHOR\n";
tv_old_heading = get_current_millis();
} else {
state = HOLD_STILL;
hs_state = 0;
}
break;
}
case GET_ANCHOR:
/*Copter versucht sich stabil auf ANCHOR_DISTANCE und desired_heading zu stellen*/
if (new_heading) {
short heading_diff;
/*Berechnung der Differenz zwischen aktueller Ausrichtung und gewünschter Ausrichtung*/
if (abs(desired_heading - current_heading) > 180) {
heading_diff = desired_heading - current_heading - sign(desired_heading - current_heading)*360;
} else {
heading_diff = desired_heading - current_heading;
}
yaw = pid_yaw.get(heading_diff,get_current_millis() - tv_old_heading);
tv_old_heading = get_current_millis();
}
if (new_value) {
roll = 0; pitch = 0;
if (nvalue[align_se] == 1) {
if (align_se == 0) roll = between<short>(pid_roll_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll);
else if (align_se == 1) roll = between<short>(pid_roll_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_roll, max_roll);
else if (align_se == 2) pitch = between<short>(pid_pitch_anc.get(srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch);
else if (align_se == 3) pitch = between<short>(pid_pitch_anc.get((-1)*srf[align_se].get_mean(), srf[align_se].get_msec_diff()), -max_pitch, max_pitch);
nvalue[align_se] = 0;
}
if (srf[0].get_mean() < 30) roll = 1000;
else if (srf[1].get_mean() < 30) roll = -1000;
else if (srf[2].get_mean() < 30) pitch = 1000;
else if (srf[3].get_mean() < 30) pitch = -1000;
if (abs(srf[0].get_mean() - ANCHOR_DISTANCE) < 5) {
for (int i = 0; i < SE_COUNT; i++) nvalue[i] = 0;
/*state = MOVE_BACKWARD*/
/*TODO Neuausrichtung des Copters?*/
}
}
if (breakpoint == 3) {roll = 0; pitch = 0; send_ext_ctrl();}
else if (breakpoint != 2) {send_ext_ctrl();}
break;
case DELAY: break;
case HTM_DELAY: break;
default: break;
}
#if LOG > 0
fprintf(fd_data,"%lu\t%d\t%d\t%d\t%d\t%d\t%f\t%f\n", (unsigned long)get_current_millis(), roll, pitch, yaw, current_heading, state, current_roll_rad, current_pitch_rad);
#endif
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void Autopilot_Interface::read_messages() {
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while (!received_all and !time_to_exit) {
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// 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;
printf("Battery life remaining: %i\n",
current_messages.sys_status.battery_remaining);
break;
}
// this is not being received
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;
printf("Battery life remaining: %i\n",
current_messages.battery_status.battery_remaining);
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));
printf(" pos (GPS): %f %f %f (m)\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;
mavlink_global_position_int_t gpos =
current_messages.global_position_int;
printf(" pos (GPS): %f %f %f (m)\n", gpos.lat, gpos.lon,
gpos.alt);
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));
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));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
mavlink_highres_imu_t imu = current_messages.highres_imu;
printf(" ap time: %llu \n", imu.time_usec);
printf(" acc (NED): % f % f % f (m/s^2)\n", imu.xacc, imu.yacc,
imu.zacc);
printf(" gyro (NED): % f % f % f (rad/s)\n", imu.xgyro, imu.ygyro,
imu.zgyro);
printf(" mag (NED): % f % f % f (Ga)\n", imu.xmag, imu.ymag,
imu.zmag);
printf(" baro: %f (mBar) \n", imu.abs_pressure);
printf(" altitude: %f (m) \n", imu.pressure_alt);
printf(" temperature: %f C \n", imu.temperature);
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;
mavlink_attitude_t att = current_messages.attitude;
printf("### roll: %f pitch: %f yaw: %f\n", att.roll, att.pitch,
att.yaw);
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;
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages(FILE *fd)
{
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;
}
int main(int argc, char* argv[])
{
char help[] = "--help";
// IP 使用的是数组来存储
char target_ip[100];
float position[6] = {};
int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// socket的地址
struct sockaddr_in gcAddr;
struct sockaddr_in locAddr;
//struct sockaddr_in fromAddr;
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
socklen_t fromlen;
int bytes_sent;
mavlink_message_t msg;
uint16_t len;
int i = 0;
//int success = 0;
unsigned int temp = 0;
// channel
int chan1_raw;
int chan2_raw;
int chan3_raw;
int chan4_raw;
int chan5_raw;
int chan6_raw;
int chan7_raw;
int chan8_raw;
int osd_rssi;
// pitch roll yaw
int osd_pitch;
int osd_roll;
int osd_yaw;
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
// 配置socket的地址，这是本地的地址
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14550);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
// 这是远端的地址，也就是模拟飞机的地址
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14551);
for (;;)
{
// 这里有发送的代码模块， 发送的时候，并没有控制发送的速度
/*Send Heartbeat */
// 1,system_id component_id
mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
// /* Send Status */
// mavlink_msg_sys_status_pack(1, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
// len = mavlink_msg_to_send_buffer(buf, &msg);
// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
// Send Local Position
// mavlink_msg_local_position_ned_pack(1, 200, &msg, microsSinceEpoch(),
// position[0], position[1], position[2],
// position[3], position[4], position[5]);
// len = mavlink_msg_to_send_buffer(buf, &msg);
// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
// /* Send attitude */
// mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
// len = mavlink_msg_to_send_buffer(buf, &msg);
// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
// linux socket函数，头文件#include <arpa/inet.h>
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
mavlink_attitude_t attitude;
mavlink_vfr_hud_t hud;
#if SHOW
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
#endif
for (i = 0; i < recsize; ++i)
{
// temp 是一个byteBUF的形式
temp = buf[i];
#if SHOW
printf("%02x ", (unsigned char)temp);
#endif
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
#if SHOW
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
#endif
if(msg.msgid== MAVLINK_MSG_ID_ATTITUDE){
mavlink_msg_attitude_decode(&msg,&attitude);
//printf(" Received attitude packet: roll %f, pitch %f, yaw %f \n",attitude.roll,attitude.pitch,attitude.yaw);
}
if(msg.msgid == MAVLINK_MSG_ID_RC_CHANNELS_RAW){
chan1_raw = mavlink_msg_rc_channels_raw_get_chan1_raw(&msg);
chan2_raw = mavlink_msg_rc_channels_raw_get_chan2_raw(&msg);
chan3_raw = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg);
chan4_raw = mavlink_msg_rc_channels_raw_get_chan4_raw(&msg);
chan5_raw = mavlink_msg_rc_channels_raw_get_chan5_raw(&msg);
chan6_raw = mavlink_msg_rc_channels_raw_get_chan6_raw(&msg);
chan7_raw = mavlink_msg_rc_channels_raw_get_chan7_raw(&msg);
chan8_raw = mavlink_msg_rc_channels_raw_get_chan8_raw(&msg);
osd_rssi = mavlink_msg_rc_channels_raw_get_rssi(&msg);
printf("chan1: %d, chan2: %d,chan3: %d, chan4: %d\n",chan1_raw,chan2_raw,chan3_raw,chan4_raw);
}
if(msg.msgid == MAVLINK_MSG_ID_ATTITUDE){
//osd_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
//osd_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
//osd_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
//printf("pitch: %d,roll: %d,yaw: %d\n",osd_pitch,osd_roll,osd_yaw );
}
if(msg.msgid == MAVLINK_MSG_ID_VFR_HUD){
// 解码
mavlink_msg_vfr_hud_decode(&msg,&hud);
// osd_airspeed = mavlink_msg_vfr_hud_get_airspeed(&msg);
// osd_groundspeed = mavlink_msg_vfr_hud_get_groundspeed(&msg);
// osd_heading = mavlink_msg_vfr_hud_get_heading(&msg); // 0..360 deg, 0=north
// osd_throttle = (uint8_t)mavlink_msg_vfr_hud_get_throttle(&msg);
// osd_alt = mavlink_msg_vfr_hud_get_alt(&msg);
// osd_climb = mavlink_msg_vfr_hud_get_climb(&msg);
// printf("airspeed: %d,osd_throttle %d,osd_alt: %d\n",osd_airspeed,osd_throttle,osd_alt);
printf("airspeed: %f,hrottle %d,alt: %f\n",hud.airspeed,hud.throttle,hud.alt);
}
}
}
#if SHOW
printf("\n");
#endif
}
memset(buf, 0, BUFFER_LENGTH);
usleep(1000); // Sleep one second
}
}
void MAVLink_Message_Handler::handle_message(uint64_t timestamp, mavlink_message_t &msg)
{
// ::fprintf(stderr, "msg.msgid=%u\n", msg.msgid);
switch(msg.msgid) {
case MAVLINK_MSG_ID_AHRS2: {
mavlink_ahrs2_t decoded;
mavlink_msg_ahrs2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t decoded;
mavlink_msg_attitude_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_EKF_STATUS_REPORT: {
mavlink_ekf_status_report_t decoded;
mavlink_msg_ekf_status_report_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: {
mavlink_global_position_int_t decoded;
mavlink_msg_global_position_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t decoded;
mavlink_msg_gps_raw_int_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t decoded;
mavlink_msg_heartbeat_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_MOUNT_STATUS: {
mavlink_mount_status_t decoded;
mavlink_msg_mount_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t decoded;
mavlink_msg_nav_controller_output_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t decoded;
mavlink_msg_param_value_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_REMOTE_LOG_DATA_BLOCK: {
mavlink_remote_log_data_block_t decoded;
mavlink_msg_remote_log_data_block_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t decoded;
mavlink_msg_scaled_pressure_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE2: {
mavlink_scaled_pressure2_t decoded;
mavlink_msg_scaled_pressure2_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
mavlink_servo_output_raw_t decoded;
mavlink_msg_servo_output_raw_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t decoded;
mavlink_msg_statustext_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t decoded;
mavlink_msg_sys_status_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_SYSTEM_TIME: {
mavlink_system_time_t decoded;
mavlink_msg_system_time_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t decoded;
mavlink_msg_vfr_hud_decode(&msg, &decoded);
handle_decoded_message(timestamp, decoded);
break;
}
}
}
void
ArduPilot::readData(){
mavlink_msgs::Mavlink ros_mav_msg;
mavlink_message_t msg;
mavlink_status_t status;
uint8_t byte;
while(my_serial_.available()>0){
my_serial_.read(&byte,1);
if(mavlink_parse_char(MAVLINK_COMM_0,byte,&msg,&status)){
clock_gettime(CLOCK_MONOTONIC, ¤t_time_);
createROSFromMavlink(&msg,&ros_mav_msg);
ros_mav_msg.header.stamp.sec = current_time_.tv_sec;
ros_mav_msg.header.stamp.nsec = current_time_.tv_nsec;
mav_pub_.publish(ros_mav_msg);
switch(msg.msgid){
case MAVLINK_MSG_ID_HEARTBEAT:
{
}
break;
case MAVLINK_MSG_ID_RAW_IMU: // msg ID: 27
{
if(imu_pub_.getNumSubscribers()>0){
mavlink_raw_imu_t imu;
mavlink_msg_raw_imu_decode(&msg,&imu);
ros_imu_msg_.header.stamp.sec = current_time_.tv_sec;
ros_imu_msg_.header.stamp.nsec = current_time_.tv_nsec;
//ros_imu_msg_.header.stamp = ros::Time.now();
ros_imu_msg_.linear_acceleration.x = 9.81*(imu.xacc/1000);
ros_imu_msg_.linear_acceleration.y = 9.81*(imu.yacc/1000);
ros_imu_msg_.linear_acceleration.z = 9.81*(imu.zacc/1000);
ros_imu_msg_.angular_velocity.x = imu.xgyro/1000;
ros_imu_msg_.angular_velocity.y = imu.ygyro/1000;
ros_imu_msg_.angular_velocity.z = imu.zgyro/1000;
imu_pub_.publish(ros_imu_msg_);
}
}
break;
case MAVLINK_MSG_ID_GPS_RAW_INT: //msg ID: 24
{
if(gps_pub_.getNumSubscribers()>0){
mavlink_gps_raw_int_t gps;
mavlink_msg_gps_raw_int_decode(&msg,&gps);
ros_gps_msg_.header.stamp.sec = current_time_.tv_sec;
ros_gps_msg_.header.stamp.nsec = current_time_.tv_nsec;
ros_gps_msg_.latitude = gps.lat/1E7;
ros_gps_msg_.longitude = gps.lon/1E7;
ros_gps_msg_.altitude = gps.alt/1E3;
gps_pub_.publish(ros_gps_msg_);
gps_common::LLtoUTM(gps.lat,gps.lon, north_, east_, utm_zone_);
ros_pose_msg_.position.x = north_;
ros_pose_msg_.position.y = east_;
ros_pose_msg_.position.z = -gps.alt/1E7;
}
}
break;
case MAVLINK_MSG_ID_ATTITUDE: //msg ID: 30
{
if(pose_pub_.getNumSubscribers()>0){
mavlink_attitude_t att;
mavlink_msg_attitude_decode(&msg,&att);
ros_att_msg_.header.stamp.sec = current_time_.tv_sec;
ros_att_msg_.header.stamp.nsec = current_time_.tv_nsec;
ros_att_msg_.orientation.x = att.roll;
ros_att_msg_.orientation.y = att.pitch;
ros_att_msg_.orientation.z = att.yaw;
att_pub_.publish(ros_att_msg_);
ros_pose_msg_.orientation.x = att.roll;
ros_pose_msg_.orientation.y = att.pitch;
ros_pose_msg_.orientation.z = att.yaw;
pose_pub_.publish(ros_pose_msg_);
}
}
break;
case MAVLINK_MSG_ID_AHRS: //msg ID: 163
{
mavlink_ahrs_t ahrs;
}
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: //msg ID: 35
{
if(rc_pub_.getNumSubscribers()>0){
mavlink_rc_channels_raw_t rc;
ros_rc_msg_.time_boot_ms = rc.time_boot_ms;
ros_rc_msg_.chan1_raw = rc.chan1_raw;
ros_rc_msg_.chan2_raw = rc.chan2_raw;
ros_rc_msg_.chan3_raw = rc.chan3_raw;
ros_rc_msg_.chan4_raw = rc.chan4_raw;
ros_rc_msg_.chan5_raw = rc.chan5_raw;
ros_rc_msg_.chan6_raw = rc.chan6_raw;
ros_rc_msg_.chan7_raw = rc.chan7_raw;
ros_rc_msg_.chan8_raw = rc.chan8_raw;
ros_rc_msg_.rssi = rc.rssi;
rc_pub_.publish(ros_rc_msg_);
}
}
break;
/* case MAVLINK_MSG_ID_G: //msg ID: 28
{
mavlink_raw_pressure_t baro;
}
break; */
case MAVLINK_MSG_ID_RAW_PRESSURE: //msg ID: 28
{
mavlink_raw_pressure_t baro;
}
break;
case MAVLINK_MSG_ID_COMMAND_LONG:
// EXECUTE ACTION
break;
default:
//Do nothing
break;
}
}
}
// my_serial.flush();
}
static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user)
{
// Handle param messages
paramClient->handleMAVLinkPacket(msg);
//check for timed-out operations
struct timeval tv;
gettimeofday(&tv, NULL);
uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state);
current_state = PX_WPP_IDLE;
protocol_current_count = 0;
protocol_current_partner_systemid = 0;
protocol_current_partner_compid = 0;
protocol_current_wp_id = -1;
if(waypoints->size() == 0)
{
current_active_wp_id = -1;
}
}
if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size())
{
send_setpoint(current_active_wp_id);
}
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE");
//compare current yaw
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
{
if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
yawReached = true;
}
else if(att.yaw - yaw_tolerance < 0.0f)
{
float lowerBound = 360.0f + att.yaw - yaw_tolerance;
if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
yawReached = true;
}
else
{
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
yawReached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION:
{
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 1)
{
mavlink_local_position_t pos;
mavlink_msg_local_position_decode(msg, &pos);
if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
posReached = false;
// compare current position (given in message) with current waypoint
float orbit = wp->param1;
float dist;
if (wp->param2 == 0)
{
dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z);
}
else
{
dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z);
}
if (dist >= 0.f && dist <= orbit && yawReached)
{
posReached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_CMD: // special action from ground station
{
mavlink_cmd_t action;
mavlink_msg_cmd_decode(msg, &action);
if(action.target == systemid)
{
if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
switch (action.action)
{
// case MAV_ACTION_LAUNCH:
// if (verbose) std::cerr << "Launch received" << std::endl;
// current_active_wp_id = 0;
// if (waypoints->size()>0)
// {
// setActive(waypoints[current_active_wp_id]);
// }
// else
// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// break;
// case MAV_ACTION_CONTINUE:
// if (verbose) std::c
// err << "Continue received" << std::endl;
// idle = false;
// setActive(waypoints[current_active_wp_id]);
// break;
// case MAV_ACTION_HALT:
// if (verbose) std::cerr << "Halt received" << std::endl;
// idle = true;
// break;
// default:
// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// break;
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK:
{
mavlink_waypoint_ack_t wpa;
mavlink_msg_waypoint_ack_decode(msg, &wpa);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid))
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (protocol_current_wp_id == waypoints->size()-1)
{
if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n");
current_state = PX_WPP_IDLE;
protocol_current_wp_id = 0;
}
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
mavlink_waypoint_set_current_t wpc;
mavlink_msg_waypoint_set_current_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE)
{
if (wpc.seq < waypoints->size())
{
if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
current_active_wp_id = wpc.seq;
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (i == current_active_wp_id)
{
waypoints->at(i)->current = true;
}
else
{
waypoints->at(i)->current = false;
}
}
if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
}
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
mavlink_waypoint_request_list_t wprl;
mavlink_msg_waypoint_request_list_decode(msg, &wprl);
if(wprl.target_system == systemid && wprl.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST)
{
if (waypoints->size() > 0)
{
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid);
current_state = PX_WPP_SENDLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
}
else
{
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
}
protocol_current_count = waypoints->size();
send_waypoint_count(msg->sysid,msg->compid, protocol_current_count);
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
mavlink_waypoint_request_t wpr;
mavlink_msg_waypoint_request_decode(msg, &wpr);
if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid)
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size()))
{
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
current_state = PX_WPP_SENDLIST_SENDWPS;
protocol_current_wp_id = wpr.seq;
send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq);
}
else
{
if (verbose)
{
if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; }
else if (current_state == PX_WPP_SENDLIST)
{
if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
}
else if (current_state == PX_WPP_SENDLIST_SENDWPS)
{
if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1);
else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
}
}
}
else
{
//we we're target but already communicating with someone else
if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid))
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
mavlink_waypoint_count_t wpc;
mavlink_msg_waypoint_count_decode(msg, &wpc);
if(wpc.target_system == systemid && wpc.target_component == compid)
{
protocol_timestamp_lastaction = now;
if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0))
{
if (wpc.count > 0)
{
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
current_state = PX_WPP_GETLIST;
protocol_current_wp_id = 0;
protocol_current_partner_systemid = msg->sysid;
protocol_current_partner_compid = msg->compid;
protocol_current_count = wpc.count;
printf("clearing receive buffer and readying for receiving waypoints\n");
while(waypoints_receive_buffer->size() > 0)
{
delete waypoints_receive_buffer->back();
waypoints_receive_buffer->pop_back();
}
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
else if (wpc.count == 0)
{
printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n");
while(waypoints_receive_buffer->size() > 0)
{
delete waypoints->back();
waypoints->pop_back();
}
current_active_wp_id = -1;
yawReached = false;
posReached = false;
break;
}
else
{
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
}
}
else
{
if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state);
else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT:
{
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(msg, &wp);
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid))
{
protocol_timestamp_lastaction = now;
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count))
{
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
current_state = PX_WPP_GETLIST_GETWPS;
protocol_current_wp_id = wp.seq + 1;
mavlink_waypoint_t* newwp = new mavlink_waypoint_t;
memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
waypoints_receive_buffer->push_back(newwp);
if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS)
{
if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count);
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
if (current_active_wp_id > waypoints_receive_buffer->size()-1)
{
current_active_wp_id = waypoints_receive_buffer->size() - 1;
}
// switch the waypoints list
std::vector<mavlink_waypoint_t*>* waypoints_temp = waypoints;
waypoints = waypoints_receive_buffer;
waypoints_receive_buffer = waypoints_temp;
//get the new current waypoint
uint32_t i;
for(i = 0; i < waypoints->size(); i++)
{
if (waypoints->at(i)->current == 1)
{
current_active_wp_id = i;
//if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
yawReached = false;
posReached = false;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
timestamp_firstinside_orbit = 0;
break;
}
}
if (i == waypoints->size())
{
current_active_wp_id = -1;
yawReached = false;
posReached = false;
timestamp_firstinside_orbit = 0;
}
current_state = PX_WPP_IDLE;
}
else
{
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id);
}
}
else
{
if (current_state == PX_WPP_IDLE)
{
//we're done receiving waypoints, answer with ack.
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0);
printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n");
}
if (verbose)
{
if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; }
else if (current_state == PX_WPP_GETLIST)
{
if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else if (current_state == PX_WPP_GETLIST_GETWPS)
{
if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id);
else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
}
}
else
{
//we we're target but already communicating with someone else
if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid);
}
else if(wp.target_system == systemid && wp.target_component == compid)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{
mavlink_waypoint_clear_all_t wpca;
mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE)
{
protocol_timestamp_lastaction = now;
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
while(waypoints->size() > 0)
{
delete waypoints->back();
waypoints->pop_back();
}
current_active_wp_id = -1;
yawReached = false;
posReached = false;
}
else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state);
}
break;
}
default:
{
if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl;
break;
}
}
//check if the current waypoint was reached
if ((posReached && /*yawReached &&*/ !idle))
{
if (current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id);
if (timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
send_waypoint_reached(cur_wp->seq);
timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000)
{
if (cur_wp->autocontinue)
{
cur_wp->current = 0;
if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1)
{
//the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning
current_active_wp_id = 1;
}
else
{
if ((uint16_t)(current_active_wp_id + 1) < waypoints->size())
current_active_wp_id++;
}
// Fly to next waypoint
timestamp_firstinside_orbit = 0;
send_waypoint_current(current_active_wp_id);
send_setpoint(current_active_wp_id);
waypoints->at(current_active_wp_id)->current = true;
posReached = false;
yawReached = false;
if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id);
}
}
}
}
else
{
timestamp_lastoutside_orbit = now;
}
}
void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
uint64_t now = mavlink_missionlib_get_system_timestamp();
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
{
mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
float yaw_tolerance = wpm.accept_range_yaw;
//compare current yaw
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
{
if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
wpm.yaw_reached = true;
}
else if(att.yaw - yaw_tolerance < 0.0f)
{
float lowerBound = 360.0f + att.yaw - yaw_tolerance;
if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
wpm.yaw_reached = true;
}
else
{
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
wpm.yaw_reached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
{
mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
{
mavlink_local_position_ned_t pos;
mavlink_msg_local_position_ned_decode(msg, &pos);
//// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
wpm.pos_reached = false;
// compare current position (given in message) with current waypoint
float orbit = wp->param1;
float dist;
// if (wp->param2 == 0)
// {
// // FIXME segment distance
// //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
// }
// else
// {
dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z);
// }
if (dist >= 0.f && dist <= orbit && wpm.yaw_reached)
{
wpm.pos_reached = true;
}
}
}
break;
}
// case MAVLINK_MSG_ID_CMD: // special action from ground station
// {
// mavlink_cmd_t action;
// mavlink_msg_cmd_decode(msg, &action);
// if(action.target == mavlink_system.sysid)
// {
// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
// switch (action.action)
// {
// // case MAV_ACTION_LAUNCH:
// // // if (verbose) std::cerr << "Launch received" << std::endl;
// // current_active_wp_id = 0;
// // if (wpm.size>0)
// // {
// // setActive(waypoints[current_active_wp_id]);
// // }
// // else
// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// // break;
//
// // case MAV_ACTION_CONTINUE:
// // // if (verbose) std::c
// // err << "Continue received" << std::endl;
// // idle = false;
// // setActive(waypoints[current_active_wp_id]);
// // break;
//
// // case MAV_ACTION_HALT:
// // // if (verbose) std::cerr << "Halt received" << std::endl;
// // idle = true;
// // break;
//
// // default:
// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// // break;
// }
// }
// break;
// }
case MAVLINK_MSG_ID_MISSION_ACK:
{
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/))
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
{
if (wpm.current_wp_id == wpm.size-1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE");
#else
if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
#endif
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
wpm.current_wp_id = 0;
}
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
break;
}
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
{
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
if (wpc.seq < wpm.size)
{
// if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n");
wpm.current_active_wp_id = wpc.seq;
uint32_t i;
for(i = 0; i < wpm.size; i++)
{
if (i == wpm.current_active_wp_id)
{
wpm.waypoints[i].current = true;
}
else
{
wpm.waypoints[i].current = false;
}
}
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("NEW WP SET");
#else
if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id);
#endif
wpm.yaw_reached = false;
wpm.pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.timestamp_firstinside_orbit = 0;
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n");
#endif
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
#endif
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST_LIST:
{
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
if (wpm.size > 0)
{
//if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST;
wpm.current_wp_id = 0;
wpm.current_partner_sysid = msg->sysid;
wpm.current_partner_compid = msg->compid;
}
else
{
// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
}
wpm.current_count = wpm.size;
mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count);
}
else
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
}
}
else
{
// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
}
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST:
{
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size))
{
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
wpm.current_wp_id = wpr.seq;
mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq);
}
else
{
// if (verbose)
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
break;
}
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
if (wpr.seq != 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
#endif
}
}
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
{
if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
#endif
}
else if (wpr.seq >= wpm.size)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
#endif
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n");
#endif
}
}
}
}
else
{
//we we're target but already communicating with someone else
if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
#endif
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT:
{
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0))
{
if (wpc.count > 0)
{
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
#endif
}
wpm.current_state = MAVLINK_WPM_STATE_GETLIST;
wpm.current_wp_id = 0;
wpm.current_partner_sysid = msg->sysid;
wpm.current_partner_compid = msg->compid;
wpm.current_count = wpc.count;
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY");
#else
if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
#endif
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// delete waypoints_receive_buffer->back();
// waypoints_receive_buffer->pop_back();
// }
mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
}
else if (wpc.count == 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("COUNT 0");
#else
if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
#endif
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// delete waypoints->back();
// waypoints->pop_back();
// }
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
break;
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("IGN WP CMD");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
#endif
}
}
else
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
#endif
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n");
#endif
}
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
{
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
mavlink_missionlib_send_gcs_string("GOT WP");
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/))
{
wpm.timestamp_lastaction = now;
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count))
{
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid);
// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid);
//
wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
mavlink_mission_item_t* newwp = &(wpm.rcv_waypoints[wp.seq]);
memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
wpm.current_wp_id = wp.seq + 1;
// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
{
mavlink_missionlib_send_gcs_string("GOT ALL WPS");
// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
if (wpm.current_active_wp_id > wpm.rcv_size-1)
{
wpm.current_active_wp_id = wpm.rcv_size-1;
}
// switch the waypoints list
// FIXME CHECK!!!
for (int i = 0; i < wpm.current_count; ++i)
{
wpm.waypoints[i] = wpm.rcv_waypoints[i];
}
wpm.size = wpm.current_count;
//get the new current waypoint
uint32_t i;
for(i = 0; i < wpm.size; i++)
{
if (wpm.waypoints[i].current == 1)
{
wpm.current_active_wp_id = i;
//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
wpm.yaw_reached = false;
wpm.pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.timestamp_firstinside_orbit = 0;
break;
}
}
if (i == wpm.size)
{
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
wpm.timestamp_firstinside_orbit = 0;
}
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
}
else
{
mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
}
}
else
{
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
//we're done receiving waypoints, answer with ack.
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
// printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
}
// if (verbose)
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state);
break;
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
{
if(!(wp.seq == 0))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq);
}
else
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
{
if (!(wp.seq == wpm.current_wp_id))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
}
else if (!(wp.seq < wpm.current_count))
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq);
}
else
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
}
else
{
// printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq);
}
}
}
}
else
{
//we we're target but already communicating with someone else
if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
}
else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/)
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
}
}
break;
}
case MAVLINK_MSG_ID_MISSION_CLEAR_ALL:
{
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
wpm.timestamp_lastaction = now;
// if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
// Delete all waypoints
wpm.size = 0;
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
}
else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
{
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
}
break;
}
default:
{
// if (debug) // printf("Waypoint: received message of unknown type");
break;
}
}
//check if the current waypoint was reached
if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle)
{
if (wpm.current_active_wp_id < wpm.size)
{
mavlink_mission_item_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if (wpm.timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
// if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq);
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
wpm.timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000)
{
if (cur_wp->autocontinue)
{
cur_wp->current = 0;
if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1)
{
//the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning
wpm.current_active_wp_id = 1;
}
else
{
if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size)
wpm.current_active_wp_id++;
}
// Fly to next waypoint
wpm.timestamp_firstinside_orbit = 0;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.waypoints[wpm.current_active_wp_id].current = true;
wpm.pos_reached = false;
wpm.yaw_reached = false;
// if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
}
}
}
}
else
{
wpm.timestamp_lastoutside_orbit = now;
}
}
//30
void MavSerialPort::attitude_handler(){
// qDebug() << "MAVLINK_MSG_ID_ATTITUDE\n";
mavlink_msg_attitude_decode(&message, &attitude);
emit timeChanged();
emit attitudeChanged();
}
//30
void MavSerialPort::attitude_handler(){
mavlink_msg_attitude_decode(&message, &attitude);
// emit timeChanged();
// emit attitudeChanged();
}
void MavlinkComm::_handleMessage(mavlink_message_t * msg) {
uint32_t timeStamp = micros();
switch (msg->msgid) {
_board->debug->printf_P(PSTR("message received: %d"), msg->msgid);
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
_lastHeartBeat = micros();
break;
}
case MAVLINK_MSG_ID_GPS_RAW: {
// decode
mavlink_gps_raw_t packet;
mavlink_msg_gps_raw_decode(msg, &packet);
_navigator->setTimeStamp(timeStamp);
_navigator->setLat(packet.lat * deg2Rad);
_navigator->setLon(packet.lon * deg2Rad);
_navigator->setAlt(packet.alt);
_navigator->setYaw(packet.hdg * deg2Rad);
_navigator->setGroundSpeed(packet.v);
_navigator->setAirSpeed(packet.v);
//_board->debug->printf_P(PSTR("received hil gps raw packet\n"));
/*
_board->debug->printf_P(PSTR("received lat: %f deg\tlon: %f deg\talt: %f m\n"),
packet.lat,
packet.lon,
packet.alt);
*/
break;
}
case MAVLINK_MSG_ID_HIL_STATE: {
// decode
mavlink_hil_state_t packet;
mavlink_msg_hil_state_decode(msg, &packet);
_navigator->setTimeStamp(timeStamp);
_navigator->setRoll(packet.roll);
_navigator->setPitch(packet.pitch);
_navigator->setYaw(packet.yaw);
_navigator->setRollRate(packet.rollspeed);
_navigator->setPitchRate(packet.pitchspeed);
_navigator->setYawRate(packet.yawspeed);
_navigator->setVN(packet.vx/ 1e2);
_navigator->setVE(packet.vy/ 1e2);
_navigator->setVD(packet.vz/ 1e2);
_navigator->setLat_degInt(packet.lat);
_navigator->setLon_degInt(packet.lon);
_navigator->setAlt(packet.alt / 1e3);
_navigator->setXAccel(packet.xacc/ 1e3);
_navigator->setYAccel(packet.xacc/ 1e3);
_navigator->setZAccel(packet.xacc/ 1e3);
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
// decode
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
// set dcm hil sensor
_navigator->setTimeStamp(timeStamp);
_navigator->setRoll(packet.roll);
_navigator->setPitch(packet.pitch);
_navigator->setYaw(packet.yaw);
_navigator->setRollRate(packet.rollspeed);
_navigator->setPitchRate(packet.pitchspeed);
_navigator->setYawRate(packet.yawspeed);
//_board->debug->printf_P(PSTR("received hil attitude packet\n"));
break;
}
case MAVLINK_MSG_ID_ACTION: {
// decode
mavlink_action_t packet;
mavlink_msg_action_decode(msg, &packet);
if (_checkTarget(packet.target, packet.target_component))
break;
// do action
sendText(SEVERITY_LOW, PSTR("action received"));
switch (packet.action) {
case MAV_ACTION_STORAGE_READ:
AP_Var::load_all();
break;
case MAV_ACTION_STORAGE_WRITE:
AP_Var::save_all();
break;
case MAV_ACTION_MOTORS_START:
_controller->setMode(MAV_MODE_READY);
break;
case MAV_ACTION_CALIBRATE_GYRO:
case MAV_ACTION_CALIBRATE_MAG:
case MAV_ACTION_CALIBRATE_ACC:
case MAV_ACTION_CALIBRATE_PRESSURE:
_controller->setMode(MAV_MODE_LOCKED);
_navigator->calibrate();
break;
case MAV_ACTION_EMCY_KILL:
case MAV_ACTION_CONFIRM_KILL:
case MAV_ACTION_MOTORS_STOP:
case MAV_ACTION_SHUTDOWN:
_controller->setMode(MAV_MODE_LOCKED);
break;
case MAV_ACTION_LAUNCH:
case MAV_ACTION_TAKEOFF:
_guide->setMode(MAV_NAV_LIFTOFF);
break;
case MAV_ACTION_LAND:
_guide->setCurrentIndex(0);
_guide->setMode(MAV_NAV_LANDING);
break;
case MAV_ACTION_EMCY_LAND:
_guide->setMode(MAV_NAV_LANDING);
break;
case MAV_ACTION_LOITER:
case MAV_ACTION_HALT:
_guide->setMode(MAV_NAV_LOITER);
break;
case MAV_ACTION_SET_AUTO:
_controller->setMode(MAV_MODE_AUTO);
break;
case MAV_ACTION_SET_MANUAL:
_controller->setMode(MAV_MODE_MANUAL);
break;
case MAV_ACTION_RETURN:
_guide->setMode(MAV_NAV_RETURNING);
break;
case MAV_ACTION_NAVIGATE:
case MAV_ACTION_CONTINUE:
_guide->setMode(MAV_NAV_WAYPOINT);
break;
case MAV_ACTION_CALIBRATE_RC:
case MAV_ACTION_REBOOT:
case MAV_ACTION_REC_START:
case MAV_ACTION_REC_PAUSE:
case MAV_ACTION_REC_STOP:
sendText(SEVERITY_LOW, PSTR("action not implemented"));
break;
default:
sendText(SEVERITY_LOW, PSTR("unknown action"));
break;
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: {
sendText(SEVERITY_LOW, PSTR("waypoint request list"));
// decode
mavlink_waypoint_request_list_t packet;
mavlink_msg_waypoint_request_list_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// Start sending waypoints
mavlink_msg_waypoint_count_send(_channel, msg->sysid, msg->compid,
_guide->getNumberOfCommands());
_cmdTimeLastSent = millis();
_cmdTimeLastReceived = millis();
_sendingCmds = true;
_receivingCmds = false;
_cmdDestSysId = msg->sysid;
_cmdDestCompId = msg->compid;
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST: {
sendText(SEVERITY_LOW, PSTR("waypoint request"));
// Check if sending waypiont
if (!_sendingCmds)
break;
// decode
mavlink_waypoint_request_t packet;
mavlink_msg_waypoint_request_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
_board->debug->printf_P(PSTR("sequence: %d\n"),packet.seq);
AP_MavlinkCommand cmd(packet.seq);
mavlink_waypoint_t wp = cmd.convert(_guide->getCurrentIndex());
mavlink_msg_waypoint_send(_channel, _cmdDestSysId, _cmdDestCompId,
wp.seq, wp.frame, wp.command, wp.current, wp.autocontinue,
wp.param1, wp.param2, wp.param3, wp.param4, wp.x, wp.y,
wp.z);
// update last waypoint comm stamp
_cmdTimeLastSent = millis();
break;
}
case MAVLINK_MSG_ID_WAYPOINT_ACK: {
sendText(SEVERITY_LOW, PSTR("waypoint ack"));
// decode
mavlink_waypoint_ack_t packet;
mavlink_msg_waypoint_ack_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// check for error
//uint8_t type = packet.type; // ok (0), error(1)
// turn off waypoint send
_sendingCmds = false;
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
sendText(SEVERITY_LOW, PSTR("param request list"));
// decode
mavlink_param_request_list_t packet;
mavlink_msg_param_request_list_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// Start sending parameters - next call to ::update will kick the first one out
_queuedParameter = AP_Var::first();
_queuedParameterIndex = 0;
break;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: {
sendText(SEVERITY_LOW, PSTR("waypoint clear all"));
// decode
mavlink_waypoint_clear_all_t packet;
mavlink_msg_waypoint_clear_all_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// clear all waypoints
uint8_t type = 0; // ok (0), error(1)
_guide->setNumberOfCommands(1);
_guide->setCurrentIndex(0);
// send acknowledgement 3 times to makes sure it is received
for (int i = 0; i < 3; i++)
mavlink_msg_waypoint_ack_send(_channel, msg->sysid,
msg->compid, type);
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: {
sendText(SEVERITY_LOW, PSTR("waypoint set current"));
// decode
mavlink_waypoint_set_current_t packet;
mavlink_msg_waypoint_set_current_decode(msg, &packet);
Serial.print("Packet Sequence:");
Serial.println(packet.seq);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// set current waypoint
Serial.print("Current Index:");
Serial.println(_guide->getCurrentIndex());
Serial.flush();
_guide->setCurrentIndex(packet.seq);
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT: {
sendText(SEVERITY_LOW, PSTR("waypoint count"));
// decode
mavlink_waypoint_count_t packet;
mavlink_msg_waypoint_count_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// start waypoint receiving
if (packet.count > _cmdMax) {
packet.count = _cmdMax;
}
_cmdNumberRequested = packet.count;
_cmdTimeLastReceived = millis();
_receivingCmds = true;
_sendingCmds = false;
_cmdRequestIndex = 0;
break;
}
case MAVLINK_MSG_ID_WAYPOINT: {
sendText(SEVERITY_LOW, PSTR("waypoint"));
// Check if receiving waypiont
if (!_receivingCmds) {
//sendText(SEVERITY_HIGH, PSTR("not receiving commands"));
break;
}
// decode
mavlink_waypoint_t packet;
mavlink_msg_waypoint_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// check if this is the requested waypoint
if (packet.seq != _cmdRequestIndex) {
char warningMsg[50];
sprintf(warningMsg,
"waypoint request out of sequence: (packet) %d / %d (ap)",
packet.seq, _cmdRequestIndex);
sendText(SEVERITY_HIGH, warningMsg);
break;
}
_board->debug->printf_P(PSTR("received waypoint x: %f\ty: %f\tz: %f\n"),
packet.x,
packet.y,
packet.z);
// store waypoint
AP_MavlinkCommand command(packet);
//sendText(SEVERITY_HIGH, PSTR("waypoint stored"));
_cmdRequestIndex++;
if (_cmdRequestIndex == _cmdNumberRequested) {
sendMessage(MAVLINK_MSG_ID_WAYPOINT_ACK);
_receivingCmds = false;
_guide->setNumberOfCommands(_cmdNumberRequested);
// make sure curernt waypoint still exists
if (_cmdNumberRequested > _guide->getCurrentIndex()) {
_guide->setCurrentIndex(0);
mavlink_msg_waypoint_current_send(_channel,
_guide->getCurrentIndex());
}
//sendText(SEVERITY_LOW, PSTR("waypoint ack sent"));
} else if (_cmdRequestIndex > _cmdNumberRequested) {
_receivingCmds = false;
}
_cmdTimeLastReceived = millis();
break;
}
case MAVLINK_MSG_ID_PARAM_SET: {
sendText(SEVERITY_LOW, PSTR("param set"));
AP_Var *vp;
AP_Meta_class::Type_id var_type;
// decode
mavlink_param_set_t packet;
mavlink_msg_param_set_decode(msg, &packet);
if (_checkTarget(packet.target_system, packet.target_component))
break;
// set parameter
char key[_paramNameLengthMax + 1];
strncpy(key, (char *) packet.param_id, _paramNameLengthMax);
key[_paramNameLengthMax] = 0;
// find the requested parameter
vp = AP_Var::find(key);
if ((NULL != vp) && // exists
!isnan(packet.param_value) && // not nan
!isinf(packet.param_value)) { // not inf
// add a small amount before casting parameter values
// from float to integer to avoid truncating to the
// next lower integer value.
const float rounding_addition = 0.01;
// fetch the variable type ID
var_type = vp->meta_type_id();
// handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) {
((AP_Float *) vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_float16) {
((AP_Float16 *) vp)->set_and_save(packet.param_value);
} else if (var_type == AP_Var::k_typeid_int32) {
((AP_Int32 *) vp)->set_and_save(
packet.param_value + rounding_addition);
} else if (var_type == AP_Var::k_typeid_int16) {
((AP_Int16 *) vp)->set_and_save(
packet.param_value + rounding_addition);
} else if (var_type == AP_Var::k_typeid_int8) {
((AP_Int8 *) vp)->set_and_save(
packet.param_value + rounding_addition);
} else {
// we don't support mavlink set on this parameter
break;
}
// Report back the new value if we accepted the change
// we send the value we actually set, which could be
// different from the value sent, in case someone sent
// a fractional value to an integer type
mavlink_msg_param_value_send(_channel, (int8_t *) key,
vp->cast_to_float(), _countParameters(), -1); // XXX we don't actually know what its index is...
}
break;
} // end case
}
}
// ------------------------------------------------------------------------------
// Read Messages
// ------------------------------------------------------------------------------
void
Autopilot_Interface::
read_messages()
{
bool success; // receive success flag
bool received_all = false; // receive only one message
Time_Stamps this_timestamps;
// Blocking wait for new data
while ( not received_all and not time_to_exit )
{
// ----------------------------------------------------------------------
// READ MESSAGE
// ----------------------------------------------------------------------
mavlink_message_t message;
success = serial_port->read_message(message);
// ----------------------------------------------------------------------
// 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));
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));
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));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu = current_messages.time_stamps.highres_imu;
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.sys_status &&
// 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 ;
// 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;
}
void _Mavlink::handleMessages()
{
mavlink_message_t message;
int nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
m_msg.sysid = message.sysid;
m_msg.compid = message.compid;
// Handle Message ID
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
LOG(INFO)<<"-> MAVLINK_MSG_ID_HEARTBEAT";
mavlink_msg_heartbeat_decode(&message, &(m_msg.heartbeat));
m_msg.time_stamps.heartbeat = get_time_usec();
if (m_msg.heartbeat.type != MAV_TYPE_GCS)
{
m_systemID = m_msg.sysid;
m_targetComponentID = m_msg.compid;
LOG_I("-> SYSTEM_ID:"<<m_systemID
<<" COMPONENT_ID:"<<m_componentID
<<" TARGET_COMPONENT_ID:"<<m_targetComponentID);
}
else
{
LOG_I("->HEARTBEAT FROM MAV_TYPE_GCS");
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_SYS_STATUS");
mavlink_msg_sys_status_decode(&message,
&(m_msg.sys_status));
m_msg.time_stamps.sys_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_BATTERY_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_BATTERY_STATUS");
mavlink_msg_battery_status_decode(&message,
&(m_msg.battery_status));
m_msg.time_stamps.battery_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_RADIO_STATUS:
{
LOG_I("-> MAVLINK_MSG_ID_RADIO_STATUS");
mavlink_msg_radio_status_decode(&message,
&(m_msg.radio_status));
m_msg.time_stamps.radio_status = get_time_usec();
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
{
LOG_I("-> MAVLINK_MSG_ID_LOCAL_POSITION_NED");
mavlink_msg_local_position_ned_decode(&message,
&(m_msg.local_position_ned));
m_msg.time_stamps.local_position_ned = get_time_usec();
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
LOG_I("-> MAVLINK_MSG_ID_GLOBAL_POSITION_INT");
mavlink_msg_global_position_int_decode(&message,
&(m_msg.global_position_int));
m_msg.time_stamps.global_position_int = get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED");
mavlink_msg_position_target_local_ned_decode(&message,
&(m_msg.position_target_local_ned));
m_msg.time_stamps.position_target_local_ned =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT:
{
LOG_I("-> MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT");
mavlink_msg_position_target_global_int_decode(&message,
&(m_msg.position_target_global_int));
m_msg.time_stamps.position_target_global_int =
get_time_usec();
break;
}
case MAVLINK_MSG_ID_HIGHRES_IMU:
{
LOG_I("-> MAVLINK_MSG_ID_HIGHRES_IMU");
mavlink_msg_highres_imu_decode(&message,
&(m_msg.highres_imu));
m_msg.time_stamps.highres_imu = get_time_usec();
break;
}
case MAVLINK_MSG_ID_ATTITUDE:
{
LOG_I("-> MAVLINK_MSG_ID_ATTITUDE");
mavlink_msg_attitude_decode(&message, &(m_msg.attitude));
m_msg.time_stamps.attitude = get_time_usec();
break;
}
case MAVLINK_MSG_ID_COMMAND_ACK:
{
mavlink_msg_command_ack_decode(&message,
&(m_msg.command_ack));
m_msg.time_stamps.attitude = get_time_usec();
LOG_I("-> MAVLINK_MSG_ID_COMMAND_ACK:"<<m_msg.command_ack.result);
break;
}
default:
{
LOG_I("-> UNKNOWN MSG_ID:"<<message.msgid);
break;
}
}
if (++nMsgHandled >= NUM_MSG_HANDLE)
return;
}
}
void _MavlinkInterface::handleMessages()
{
Time_Stamps this_timestamps;
mavlink_message_t message;
int nMsgHandled;
nMsgHandled = 0;
//Handle Message while new message is received
while (readMessage(message))
{
// Note this doesn't handle multiple message sources.
current_messages.sysid = message.sysid;
current_messages.compid = message.compid;
system_id = current_messages.sysid;
autopilot_id = current_messages.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));
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));
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));
current_messages.time_stamps.highres_imu = get_time_usec();
this_timestamps.highres_imu =
current_messages.time_stamps.highres_imu;
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
if(++nMsgHandled >= NUM_MSG_HANDLE)return;
}
}
void mavlink_handleMessage(mavlink_message_t* msg) {
mavlink_message_t msg2;
char sysmsg_str[1024];
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
droneType = packet.type;
autoPilot = packet.autopilot;
if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
ModelData.mode = MODEL_MODE_MANUAL;
} else if (packet.base_mode == 128 + 64 + 16) {
ModelData.mode = MODEL_MODE_RTL;
} else if (packet.base_mode == 128 + 16) {
ModelData.mode = MODEL_MODE_POSHOLD;
} else if (packet.base_mode == 128 + 4) {
ModelData.mode = MODEL_MODE_MISSION;
}
if (packet.system_status == MAV_STATE_ACTIVE) {
ModelData.armed = MODEL_ARMED;
} else {
ModelData.armed = MODEL_DISARMED;
}
// SDL_Log("Heartbeat: %i, %i, %i\n", ModelData.armed, ModelData.mode, ModelData.status);
ModelData.heartbeat = 100;
// sprintf(sysmsg_str, "Heartbeat: %i", (int)time(0));
if ((*msg).sysid != 0xff) {
ModelData.sysid = (*msg).sysid;
ModelData.compid = (*msg).compid;
if (mavlink_maxparam == 0) {
mavlink_start_feeds();
}
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
mavlink_rc_channels_scaled_t packet;
mavlink_msg_rc_channels_scaled_decode(msg, &packet);
// SDL_Log("Radio: %i,%i,%i\n", packet.chan1_scaled, packet.chan2_scaled, packet.chan3_scaled);
/* if ((int)packet.chan6_scaled > 1000) {
mode = MODE_MISSION;
} else if ((int)packet.chan6_scaled < -1000) {
mode = MODE_MANUEL;
} else {
mode = MODE_POSHOLD;
}
if ((int)packet.chan7_scaled > 1000) {
mode = MODE_RTL;
} else if ((int)packet.chan7_scaled < -1000) {
mode = MODE_SETHOME;
}
*/
ModelData.radio[0] = (int)packet.chan1_scaled / 100.0;
ModelData.radio[1] = (int)packet.chan2_scaled / 100.0;
ModelData.radio[2] = (int)packet.chan3_scaled / 100.0;
ModelData.radio[3] = (int)packet.chan4_scaled / 100.0;
ModelData.radio[4] = (int)packet.chan5_scaled / 100.0;
ModelData.radio[5] = (int)packet.chan6_scaled / 100.0;
ModelData.radio[6] = (int)packet.chan7_scaled / 100.0;
ModelData.radio[7] = (int)packet.chan8_scaled / 100.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
// SDL_Log("BAR;%i;%0.2f;%0.2f;%0.2f\n", time(0), packet.press_abs, packet.press_diff, packet.temperature / 100.0);
// redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
ModelData.roll = toDeg(packet.roll);
ModelData.pitch = toDeg(packet.pitch);
if (toDeg(packet.yaw) < 0.0) {
ModelData.yaw = 360.0 + toDeg(packet.yaw);
} else {
ModelData.yaw = toDeg(packet.yaw);
}
mavlink_update_yaw = 1;
// SDL_Log("ATT;%i;%0.2f;%0.2f;%0.2f\n", time(0), toDeg(packet.roll), toDeg(packet.pitch), toDeg(packet.yaw));
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_IMU: {
// SDL_Log("SCALED_IMU\n");
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(msg, &packet);
if (packet.lat != 0.0) {
GPS_found = 1;
ModelData.p_lat = (float)packet.lat / 10000000.0;
ModelData.p_long = (float)packet.lon / 10000000.0;
ModelData.p_alt = (float)packet.alt / 1000.0;
ModelData.speed = (float)packet.vel / 100.0;
ModelData.numSat = packet.satellites_visible;
ModelData.gpsfix = packet.fix_type;
redraw_flag = 1;
}
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
// SDL_Log("RC_CHANNELS_RAW\n");
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
// SDL_Log("SERVO_OUTPUT_RAW\n");
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t packet;
mavlink_msg_sys_status_decode(msg, &packet);
// SDL_Log("%0.1f %%, %0.3f V)\n", packet.load / 10.0, packet.voltage_battery / 1000.0);
ModelData.voltage = packet.voltage_battery / 1000.0;
ModelData.load = packet.load / 10.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t packet;
mavlink_msg_statustext_decode(msg, &packet);
SDL_Log("mavlink: ## %s ##\n", packet.text);
sys_message((char *)packet.text);
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
char var[101];
uint16_t n1 = 0;
uint16_t n2 = 0;
for (n1 = 0; n1 < strlen(packet.param_id); n1++) {
if (packet.param_id[n1] != 9 && packet.param_id[n1] != ' ' && packet.param_id[n1] != '\t') {
var[n2++] = packet.param_id[n1];
}
}
var[n2++] = 0;
// MAV_VAR_FLOAT=0, /* 32 bit float | */
// MAV_VAR_UINT8=1, /* 8 bit unsigned integer | */
// MAV_VAR_INT8=2, /* 8 bit signed integer | */
// MAV_VAR_UINT16=3, /* 16 bit unsigned integer | */
// MAV_VAR_INT16=4, /* 16 bit signed integer | */
// MAV_VAR_UINT32=5, /* 32 bit unsigned integer | */
// MAV_VAR_INT32=6, /* 32 bit signed integer | */
sprintf(sysmsg_str, "PARAM_VALUE (%i/%i): #%s# = %f (Type: %i)", packet.param_index + 1, packet.param_count, var, packet.param_value, packet.param_type);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
mavlink_maxparam = packet.param_count;
mavlink_timeout = 0;
mavlink_set_value(var, packet.param_value, packet.param_type, packet.param_index);
if (packet.param_index + 1 == packet.param_count || packet.param_index % 10 == 0) {
mavlink_param_xml_meta_load();
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t packet;
mavlink_msg_mission_count_decode(msg, &packet);
sprintf(sysmsg_str, "MISSION_COUNT: %i\n", packet.count);
sys_message(sysmsg_str);
mission_max = packet.count;
if (mission_max > 0) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 0);
mavlink_send_message(&msg2);
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ACK: {
SDL_Log("mavlink: Mission-Transfer ACK\n");
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
uint16_t id = packet.seq;
uint16_t id2 = packet.seq;
uint16_t type = 0;
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (id2 > 0) {
id2 = id2 - 1;
} else {
SDL_Log("mavlink: WORKAROUND: first WP == HOME ?\n");
}
}
sprintf(sysmsg_str, "sending Waypoint (%i): %s\n", id, WayPoints[1 + id2].name);
sys_message(sysmsg_str);
if (strcmp(WayPoints[1 + id2].command, "WAYPOINT") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_WAYPOINT\n");
type = MAV_CMD_NAV_WAYPOINT;
} else if (strcmp(WayPoints[1 + id2].command, "RTL") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
type = MAV_CMD_NAV_RETURN_TO_LAUNCH;
} else if (strcmp(WayPoints[1 + id2].command, "LAND") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_LAND\n");
type = MAV_CMD_NAV_LAND;
} else if (strcmp(WayPoints[1 + id2].command, "TAKEOFF") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_TAKEOFF\n");
type = MAV_CMD_NAV_TAKEOFF;
} else {
SDL_Log("mavlink: Type: UNKNOWN\n");
type = MAV_CMD_NAV_WAYPOINT;
}
sprintf(sysmsg_str, "SENDING MISSION_ITEM: %i: %f, %f, %f\n", id, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
SDL_Log("mavlink: %s\n", sysmsg_str);
mavlink_msg_mission_item_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, id, 0, type, 0.0, 0.0, WayPoints[1 + id2].radius, WayPoints[1 + id2].wait, WayPoints[1 + id2].orbit, WayPoints[1 + id2].yaw, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
mavlink_send_message(&msg2);
/*
mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t packet;
mavlink_msg_mission_item_decode(msg, &packet);
sprintf(sysmsg_str, "RECEIVED MISSION_ITEM: %i/%i: %f, %f, %f (%i)\n", packet.seq, mission_max, packet.x, packet.y, packet.z, packet.frame);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
if (packet.seq < mission_max - 1) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, packet.seq + 1);
mavlink_send_message(&msg2);
} else {
mavlink_msg_mission_ack_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 15);
mavlink_send_message(&msg2);
}
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (packet.seq > 0) {
packet.seq = packet.seq - 1;
} else {
SDL_Log("mavlink: WORKAROUND: ignore first WP\n");
break;
}
}
SDL_Log("mavlink: getting WP(%i): %f, %f\n", packet.seq, packet.x, packet.y);
switch (packet.command) {
case MAV_CMD_NAV_WAYPOINT: {
strcpy(WayPoints[1 + packet.seq].command, "WAYPOINT");
break;
}
case MAV_CMD_NAV_LOITER_UNLIM: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_UNLIM");
break;
}
case MAV_CMD_NAV_LOITER_TURNS: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TURNS");
break;
}
case MAV_CMD_NAV_LOITER_TIME: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TIME");
break;
}
case MAV_CMD_NAV_RETURN_TO_LAUNCH: {
strcpy(WayPoints[1 + packet.seq].command, "RTL");
break;
}
case MAV_CMD_NAV_LAND: {
strcpy(WayPoints[1 + packet.seq].command, "LAND");
break;
}
case MAV_CMD_NAV_TAKEOFF: {
strcpy(WayPoints[1 + packet.seq].command, "TAKEOFF");
break;
}
default: {
sprintf(WayPoints[1 + packet.seq].command, "CMD:%i", packet.command);
break;
}
}
if (packet.x == 0.0) {
packet.x = 0.00001;
}
if (packet.y == 0.0) {
packet.y = 0.00001;
}
if (packet.z == 0.0) {
packet.z = 0.00001;
}
WayPoints[1 + packet.seq].p_lat = packet.x;
WayPoints[1 + packet.seq].p_long = packet.y;
WayPoints[1 + packet.seq].p_alt = packet.z;
WayPoints[1 + packet.seq].yaw = packet.param4;
sprintf(WayPoints[1 + packet.seq].name, "WP%i", packet.seq + 1);
WayPoints[1 + packet.seq + 1].p_lat = 0.0;
WayPoints[1 + packet.seq + 1].p_long = 0.0;
WayPoints[1 + packet.seq + 1].p_alt = 0.0;
WayPoints[1 + packet.seq + 1].yaw = 0.0;
WayPoints[1 + packet.seq + 1].name[0] = 0;
WayPoints[1 + packet.seq + 1].command[0] = 0;
/*
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="0" name="MAV_FRAME_GLOBAL">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="1" name="MAV_FRAME_LOCAL_NED">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="2" name="MAV_FRAME_MISSION">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="3" name="MAV_FRAME_GLOBAL_RELATIVE_ALT">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="4" name="MAV_FRAME_LOCAL_ENU">
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT: {
mavlink_mission_current_t packet;
mavlink_msg_mission_current_decode(msg, &packet);
// SDL_Log("mavlink: ## Active_WP %f ##\n", packet.seq);
uav_active_waypoint = (uint8_t)packet.seq;
break;
}
case MAVLINK_MSG_ID_RAW_IMU: {
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
/*
SDL_Log("## IMU_RAW_ACC_X %i ##\n", packet.xacc);
SDL_Log("## IMU_RAW_ACC_Y %i ##\n", packet.yacc);
SDL_Log("## IMU_RAW_ACC_Z %i ##\n", packet.zacc);
SDL_Log("## IMU_RAW_GYRO_X %i ##\n", packet.xgyro);
SDL_Log("## IMU_RAW_GYRO_Y %i ##\n", packet.ygyro);
SDL_Log("## IMU_RAW_GYRO_Z %i ##\n", packet.zgyro);
SDL_Log("## IMU_RAW_MAG_X %i ##\n", packet.xmag);
SDL_Log("## IMU_RAW_MAG_Y %i ##\n", packet.ymag);
SDL_Log("## IMU_RAW_MAG_Z %i ##\n", packet.zmag);
*/
ModelData.acc_x = (float)packet.xacc / 1000.0;
ModelData.acc_y = (float)packet.yacc / 1000.0;
ModelData.acc_z = (float)packet.zacc / 1000.0;
ModelData.gyro_x = (float)packet.zgyro;
ModelData.gyro_y = (float)packet.zgyro;
ModelData.gyro_z = (float)packet.zgyro;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t packet;
mavlink_msg_nav_controller_output_decode(msg, &packet);
/*
nav_roll
nav_pitch
alt_error
aspd_error
xtrack_error
nav_bearing
target_bearing
wp_dist
*/
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t packet;
mavlink_msg_vfr_hud_decode(msg, &packet);
// SDL_Log("## pa %f ##\n", packet.airspeed);
// SDL_Log("## pg %f ##\n", packet.groundspeed);
// SDL_Log("## palt %f ##\n", packet.alt);
if (GPS_found == 0) {
ModelData.p_alt = packet.alt;
}
// SDL_Log("## pc %f ##\n", packet.climb);
// SDL_Log("## ph %i ##\n", packet.heading);
// SDL_Log("## pt %i ##\n", packet.throttle);
break;
}
case MAVLINK_MSG_ID_RADIO: {
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
SDL_Log("mavlink: ## rxerrors %i ##\n", packet.rxerrors);
SDL_Log("mavlink: ## fixed %i ##\n", packet.fixed);
SDL_Log("mavlink: ## rssi %i ##\n", packet.rssi);
SDL_Log("mavlink: ## remrssi %i ##\n", packet.remrssi);
SDL_Log("mavlink: ## txbuf %i ##\n", packet.txbuf);
SDL_Log("mavlink: ## noise %i ##\n", packet.noise);
SDL_Log("mavlink: ## remnoise %i ##\n", packet.remnoise);
break;
}
default: {
// SDL_Log(" ## MSG_ID == %i ##\n", msg->msgid);
break;
}
}
}
