/**
* @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
}
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);
}