Exemple #1
0
	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);
	}
Exemple #2
0
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
Exemple #5
0
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;
            }
        }

    }
}
Exemple #6
0
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;
	}

}