void message_rx_cb(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(msg, &ack);
lock_guard lock(mutex);
for (auto it = ack_waiting_list.cbegin();
it != ack_waiting_list.cend(); it++)
if ((*it)->expected_command == ack.command) {
(*it)->result = ack.result;
(*it)->ack.notify_all();
return;
}
ROS_WARN_THROTTLE_NAMED(10, "cmd", "Unexpected command %u, result %u",
ack.command, ack.result);
}
void ParamManager::handle_command_ack_msg(const mavlink_message_t &msg)
{
if (write_request_in_progress_)
{
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&msg, &ack);
if (ack.command == MAV_CMD_PREFLIGHT_STORAGE && ack.result == MAV_RESULT_ACCEPTED)
{
write_request_in_progress_ = false;
unsaved_changes_ = false;
for (int i = 0; i < listeners_.size(); i++)
listeners_[i]->on_params_saved_change(unsaved_changes_);
}
}
}
void handle_arm_ack(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_command_ack_t cmd_ack;
mavlink_msg_command_ack_decode(msg, &cmd_ack);
auto arm_ack_msg = boost::make_shared<mms::Ack_arm>();
if (cmd_ack.command == 400){ //ARM_DISARM command
ROS_INFO("Received arm_disarm");
if (cmd_ack.result == MAV_RESULT_ACCEPTED){
arm_ack_msg->mav_result = true;
ROS_INFO("Arm-disarm: succsessful");
} else {
arm_ack_msg->mav_result = false;
ROS_INFO("Arm-disarm: fail");
}
arm_ack_pub.publish(arm_ack_msg);
}
}
void Pixhawk_Interface::
read_msg()
{
bool success;
//printf("come first\n");
mavlink_message_t msg;
success = serial_port->read_message(msg);
//printf("come here\n");
if (success)
{
switch (msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
if (msg.sysid == 1)
{
static int count = 0;
//send back an heartbeat
//mavlink_message_t msg_heartbeat;
//send_heartbeat(msg_heartbeat);
//print out
printf("<3 beat %d, sys_ID: %d\n", count, msg.sysid);
count++;
}
break;
}
case MAVLINK_MSG_ID_SYS_STATUS:
{
mavlink_sys_status_t sys_status;
mavlink_msg_sys_status_decode(&msg, &sys_status);
//print
break;
}
case COMMAND_ACK: //COMMAND_ACK #77
{
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&msg, &ack);
//print out
printf("Received command: %d, result: %d\n", ack.command, ack.result);
break;
}
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
{
//dont need to do anything
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
{
mavlink_rc_channels_raw_t rc_raw;
mavlink_msg_rc_channels_raw_decode(&msg, &rc_raw);
printf("rc1 %d, rc2 %d, rc3 %d, rc4 %d\n", rc_raw.chan1_raw,rc_raw.chan2_raw,rc_raw.chan3_raw,rc_raw.chan4_raw);
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: //#36 from data stream RC_CHANNELS
{
break;
}
default:
{
/*
* 125: POWER_STATUS
* 253: STATUTEXT
*/
//print
//printf("Other messages - msg ID: %d\n", msg.msgid);
break;
}
}
}
}// end read_msg
void ofxMavlink::threadedFunction() {
while( isThreadRunning() != 0 ) {
// static unsigned int imu_receive_counter =
uint8_t cp;
mavlink_message_t message;
mavlink_status_t status;
uint8_t msgReceived = false;
if (read(fd, &cp, 1) > 0)
{
// Check if a message could be decoded, return the message in case yes
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
if (status.packet_rx_drop_count > 0)
{
if(bDebug) ofLogWarning("ofxMavlink") << "ERROR: DROPPED " << status.packet_rx_drop_count <<" PACKETS:" << " " << cp;
}
}
else
{
if(bDebug) ofLogError("ofxMavlink") << "ERROR: Could not read from fd " << fd;
}
// If a message could be decoded, handle it
if(msgReceived)
{
msgcount++;
if (bDebug)
{
fprintf(stderr,"Received serial data: ");
unsigned int i;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message);
if (messageLength > MAVLINK_MAX_PACKET_LEN)
{
fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
}
else
{
for (i=0; i<messageLength; i++)
{
unsigned char v=buffer[i];
fprintf(stderr,"%02x ", v);
}
fprintf(stderr,"\n");
}
}
//if(bDebug) ofLogNotice("ofxMavlink") << "Received message from serial with ID #" << message.msgid << " (sys:" << message.sysid << "|comp:" << message.compid << "):\n";
switch (message.msgid)
{
case MAVLINK_MSG_ID_COMMAND_ACK:
{
mavlink_command_ack_t ack;
mavlink_msg_command_ack_decode(&message, &ack);
switch (ack.result)
{
case MAV_RESULT_ACCEPTED:
{
ofLogNotice("ofxMavlink") << "SUCCESS: Executed Command: " << ack.command;
}
break;
case MAV_RESULT_TEMPORARILY_REJECTED:
{
ofLogError("ofxMavlink") << "FAILURE: Temporarily rejected Command: " << ack.command;
}
break;
case MAV_RESULT_DENIED:
{
ofLogError("ofxMavlink") << "FAILURE: Denied Command: " << ack.command;
}
break;
case MAV_RESULT_UNSUPPORTED:
{
ofLogError("ofxMavlink") << "FAILURE: Unsupported Command: " << ack.command;
}
break;
case MAV_RESULT_FAILED:
{
ofLogError("ofxMavlink") << "FAILURE: Failed Command: " << ack.command;
}
break;
}
break;
}
// case MAVLINK_MSG_ID_COMMAND_ACK:
// {
// mavlink_command_ack_t ack;
// mavlink_msg_command_ack_decode(&message,&ack);
// cout << "received CMD_ACK command=" << ack.command << " result=" << ack.result << endl;
//
// if(ack.command == MAV_CMD_COMPONENT_ARM_DISARM) {
// cout << "Acknowledge arm/disarm command";
//
// }
// if(ack.result == MAV_CMD_ACK_OK) {
// cout << "received CMD_ACK_OK" << endl;
// }
// break;
// }
case MAVLINK_MSG_ID_RAW_IMU:
{
mavlink_raw_imu_t imu;
mavlink_msg_raw_imu_decode(&message, &imu);
lock();
time_usec = imu.time_usec;
unlock();
ofLogVerbose("ofxMavlink") << "-- RAW_IMU message received --" << endl;
ofLogVerbose("ofxMavlink") << "\t time: (us) " << imu.time_usec << endl;
ofLogVerbose("ofxMavlink") << "\t acc:" << imu.xacc << " " << imu.yacc << " " << imu.zacc << endl;
ofLogVerbose("ofxMavlink") << "\t gyro: (rad/s)" << imu.xgyro << " " << imu.ygyro << " " << imu.zgyro << endl;
ofLogVerbose("ofxMavlink") << "\t mag: (Ga)" << imu.xmag << " " << imu.ymag << " " << imu.zmag << endl;
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
ofLogNotice("ofxMavlink") << "-- GPS RAW INT message received --" << endl;
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(&message, &packet);
if (packet.fix_type > 2)
{
lock();
latitude = packet.lat/(double)1E7;
longitude = packet.lon/(double)1E7;
altitude = packet.alt/1000.0;
unlock();
ofLogNotice("ofxMavlink") << "Altitude:" << packet.alt/1000.0 << "latitude:" << (float)packet.lat / 10000000.0 << " longitude:" << (float)packet.lon / 10000000.0 << " ";
ofLogNotice("ofxMavlink") << "Satellites visible:" << packet.satellites_visible << " GPS fix:" << packet.fix_type << endl;
// if (packet.lat != 0.0) {
//
// latitude = (float)packet.lat;
// longitude = (float)packet.lon;
// altitude = (float)packet.alt;
// //ModelData.speed = (float)packet.vel / 100.0;
// //ModelData.numSat = packet.satellites_visible;
// gpsfix = packet.fix_type;
}
break;
}
case MAVLINK_MSG_ID_HEARTBEAT:
{
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(&message, &packet);
int droneType = packet.type;
int autoPilot = packet.autopilot;
// cout << "base mode:" << packet.base_mode << " custom mode:" << packet.custom_mode << endl;
// if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
// //ModelData.mode = MODEL_MODE_MANUAL;
// cout << "Manual Mode" << endl;
// } else if (packet.base_mode == 128 + 64 + 16) {
// //ModelData.mode = MODEL_MODE_RTL;
// cout << "RTL Mode" << endl;
// } else if (packet.base_mode == 128 + 16) {
// //ModelData.mode = MODEL_MODE_POSHOLD;
// cout << "Poshold Mode" << endl;
// } else if (packet.base_mode == 128 + 4) {
// //ModelData.mode = MODEL_MODE_MISSION;
// cout << "Mission Mode" << endl;
// }
// if(packet.base_mode == MAV_MODE_STABILIZE_DISARMED)
// {
// cout << "Stablilize disarmed mode" << endl;
// }
ofLogNotice("ofxMavlink") << "-- Heartbeat -- sysId:" << message.sysid << " compId:" << message.compid << " drone type:" << droneType << " autoPilot:" << autoPilot;
// if(droneType == MAV_TYPE_QUADROTOR) cout << " Quadrotor"; else cout << droneType;
// cout << " Autopilot:";
// if(autoPilot == MAV_AUTOPILOT_ARDUPILOTMEGA) cout << " ArduPilotMega"; else cout << autoPilot;
// cout << endl;
if (message.sysid != 0xff) {
lock();
targetId = message.sysid;
compId = message.compid;
//cout << "compid = " << message.compid << " sysid =" << message.sysid;
unlock();
}
break;
}
default:
break;
}
}
}
}
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;
}
}
