/**
 * @brief Pack a mission_request message on a channel
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param chan The MAVLink channel this message will be sent over
 * @param msg The MAVLink message to compress the data into
 * @param target_system System ID
 * @param target_component Component ID
 * @param seq Sequence
 * @return length of the message in bytes (excluding serial stream start sign)
 */
uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
        mavlink_message_t* msg,
        uint8_t target_system,uint8_t target_component,uint16_t seq)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
    char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN];
    _mav_put_uint16_t(buf, 0, seq);
    _mav_put_uint8_t(buf, 2, target_system);
    _mav_put_uint8_t(buf, 3, target_component);

    memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
#else
    mavlink_mission_request_t packet;
    packet.seq = seq;
    packet.target_system = target_system;
    packet.target_component = target_component;

    memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
#endif

    msg->msgid = MAVLINK_MSG_ID_MISSION_REQUEST;
#if MAVLINK_CRC_EXTRA
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC);
#else
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MISSION_REQUEST_LEN);
#endif
}
/**
 * @brief Pack a vfr_hud message on a channel
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param chan The MAVLink channel this message will be sent over
 * @param msg The MAVLink message to compress the data into
 * @param airspeed Current airspeed in m/s
 * @param groundspeed Current ground speed in m/s
 * @param heading Current heading in degrees, in compass units (0..360, 0=north)
 * @param throttle Current throttle setting in integer percent, 0 to 100
 * @param alt Current altitude (MSL), in meters
 * @param climb Current climb rate in meters/second
 * @return length of the message in bytes (excluding serial stream start sign)
 */
uint16_t _pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
							   mavlink_message_t* msg,
						           float airspeed,float groundspeed,int16_t heading,uint16_t throttle,float alt,float climb)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[MAVLINK_MSG_ID_VFR_HUD_LEN];
	_mav_put_float(buf, 0, airspeed);
	_mav_put_float(buf, 4, groundspeed);
	_mav_put_float(buf, 8, alt);
	_mav_put_float(buf, 12, climb);
	_mav_put_int16_t(buf, 16, heading);
	_mav_put_uint16_t(buf, 18, throttle);

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VFR_HUD_LEN);
#else
	mavlink_vfr_hud_t packet;
	packet.airspeed = airspeed;
	packet.groundspeed = groundspeed;
	packet.alt = alt;
	packet.climb = climb;
	packet.heading = heading;
	packet.throttle = throttle;

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VFR_HUD_LEN);
#endif

	msg->msgid = MAVLINK_MSG_ID_VFR_HUD;
#if MAVLINK_CRC_EXTRA
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN, MAVLINK_MSG_ID_VFR_HUD_CRC);
#else
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VFR_HUD_LEN);
#endif
}
/**
 * @brief Pack a sys_status message on a channel
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param chan The MAVLink channel this message will be sent over
 * @param msg The MAVLink message to compress the data into
 * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
 * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
 * @param onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices defined by ENUM MAV_SYS_STATUS_SENSOR
 * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
 * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 * @param current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
 * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
 * @param drop_rate_comm Communication drops in percent, (0%: 0, 100%: 10'000), (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 * @param errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
 * @param errors_count1 Autopilot-specific errors
 * @param errors_count2 Autopilot-specific errors
 * @param errors_count3 Autopilot-specific errors
 * @param errors_count4 Autopilot-specific errors
 * @return length of the message in bytes (excluding serial stream start sign)
 */
uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan,
							   mavlink_message_t* msg,
						           uint32_t onboard_control_sensors_present,uint32_t onboard_control_sensors_enabled,uint32_t onboard_control_sensors_health,uint16_t load,uint16_t voltage_battery,int16_t current_battery,int8_t battery_remaining,uint16_t drop_rate_comm,uint16_t errors_comm,uint16_t errors_count1,uint16_t errors_count2,uint16_t errors_count3,uint16_t errors_count4)
{
#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
	char buf[MAVLINK_MSG_ID_SYS_STATUS_LEN];
	_mav_put_uint32_t(buf, 0, onboard_control_sensors_present);
	_mav_put_uint32_t(buf, 4, onboard_control_sensors_enabled);
	_mav_put_uint32_t(buf, 8, onboard_control_sensors_health);
	_mav_put_uint16_t(buf, 12, load);
	_mav_put_uint16_t(buf, 14, voltage_battery);
	_mav_put_int16_t(buf, 16, current_battery);
	_mav_put_uint16_t(buf, 18, drop_rate_comm);
	_mav_put_uint16_t(buf, 20, errors_comm);
	_mav_put_uint16_t(buf, 22, errors_count1);
	_mav_put_uint16_t(buf, 24, errors_count2);
	_mav_put_uint16_t(buf, 26, errors_count3);
	_mav_put_uint16_t(buf, 28, errors_count4);
	_mav_put_int8_t(buf, 30, battery_remaining);

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SYS_STATUS_LEN);
#else
	mavlink_sys_status_t packet;
	packet.onboard_control_sensors_present = onboard_control_sensors_present;
	packet.onboard_control_sensors_enabled = onboard_control_sensors_enabled;
	packet.onboard_control_sensors_health = onboard_control_sensors_health;
	packet.load = load;
	packet.voltage_battery = voltage_battery;
	packet.current_battery = current_battery;
	packet.drop_rate_comm = drop_rate_comm;
	packet.errors_comm = errors_comm;
	packet.errors_count1 = errors_count1;
	packet.errors_count2 = errors_count2;
	packet.errors_count3 = errors_count3;
	packet.errors_count4 = errors_count4;
	packet.battery_remaining = battery_remaining;

        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SYS_STATUS_LEN);
#endif

	msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;
#if MAVLINK_CRC_EXTRA
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN, MAVLINK_MSG_ID_SYS_STATUS_CRC);
#else
    return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN);
#endif
}
示例#4
0
void
ArduPilot::receiverCallBack(const mavlink_msgs::Mavlink &mav_msg){

    mavlink_message_t msg;
    msg.msgid = mav_msg.msgid;

    static uint8_t mavlink_crcs[] = MAVLINK_MESSAGE_CRCS;

    //Copy payload from mavlink_msg (from ROS) to the new "real" mavlink message
    copy(mav_msg.payload.begin(), mav_msg.payload.end(), msg.payload64);

    mavlink_finalize_message_chan(&msg, mav_msg.sysid, mav_msg.compid, MAVLINK_COMM_0,
                                  mav_msg.len, mavlink_crcs[msg.msgid]);

    static uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    int messageLength = mavlink_msg_to_send_buffer(buffer, &msg);
    int written = my_serial_.write(buffer,messageLength);
    my_serial_.flushOutput();

 //   if (messageLength != written)
 //     fprintf(stderr, "ERROR: Wrote %d bytes but should have written %d\n", written, messageLength);

}