mavlink_msg_sys_status.h 27.7 KB
Newer Older
James Goppert's avatar
James Goppert committed
1 2
// MESSAGE SYS_STATUS PACKING

3 4 5 6 7
#define MAVLINK_MSG_ID_SYS_STATUS 1
#define MAVLINK_MSG_ID_SYS_STATUS_LEN 23
#define MAVLINK_MSG_1_LEN 23
#define MAVLINK_MSG_ID_SYS_STATUS_KEY 0xDA
#define MAVLINK_MSG_1_KEY 0xDA
James Goppert's avatar
James Goppert committed
8 9 10

typedef struct __mavlink_sys_status_t 
{
11 12 13
	uint16_t onboard_control_sensors_present;	///< Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	uint16_t onboard_control_sensors_enabled;	///< Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	uint16_t 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: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
lm's avatar
lm committed
14
	uint16_t load;	///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
15 16 17 18 19 20 21 22
	uint16_t voltage_battery;	///< Battery voltage, in millivolts (1 = 1 millivolt)
	uint16_t current_battery;	///< Battery current, in milliamperes (1 = 1 milliampere)
	uint16_t watt;	///< Watts consumed from this battery since startup
	uint16_t errors_uart;	///< Dropped packets on all links (packets that were corrupted on reception on the MAV)
	uint16_t errors_i2c;	///< Dropped packets on all links (packets that were corrupted on reception)
	uint16_t errors_spi;	///< Dropped packets on all links (packets that were corrupted on reception)
	uint16_t errors_can;	///< Dropped packets on all links (packets that were corrupted on reception)
	uint8_t battery_percent;	///< Remaining battery energy: (0%: 0, 100%: 255)
James Goppert's avatar
James Goppert committed
23 24 25 26 27 28 29 30 31

} mavlink_sys_status_t;

/**
 * @brief Pack a sys_status message
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 *
32 33 34
 * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 * @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: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
James Goppert's avatar
James Goppert committed
35
 * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
36 37 38 39 40 41 42 43
 * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 * @param current_battery Battery current, in milliamperes (1 = 1 milliampere)
 * @param watt Watts consumed from this battery since startup
 * @param battery_percent Remaining battery energy: (0%: 0, 100%: 255)
 * @param errors_uart Dropped packets on all links (packets that were corrupted on reception on the MAV)
 * @param errors_i2c Dropped packets on all links (packets that were corrupted on reception)
 * @param errors_spi Dropped packets on all links (packets that were corrupted on reception)
 * @param errors_can Dropped packets on all links (packets that were corrupted on reception)
James Goppert's avatar
James Goppert committed
44 45
 * @return length of the message in bytes (excluding serial stream start sign)
 */
46
static inline uint16_t mavlink_msg_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can)
James Goppert's avatar
James Goppert committed
47
{
lm's avatar
lm committed
48
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
49 50
	msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;

51 52 53
	p->onboard_control_sensors_present = onboard_control_sensors_present;	// uint16_t:Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	p->onboard_control_sensors_enabled = onboard_control_sensors_enabled;	// uint16_t:Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	p->onboard_control_sensors_health = onboard_control_sensors_health;	// uint16_t:Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
lm's avatar
lm committed
54
	p->load = load;	// uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
55 56 57 58 59 60 61 62
	p->voltage_battery = voltage_battery;	// uint16_t:Battery voltage, in millivolts (1 = 1 millivolt)
	p->current_battery = current_battery;	// uint16_t:Battery current, in milliamperes (1 = 1 milliampere)
	p->watt = watt;	// uint16_t:Watts consumed from this battery since startup
	p->battery_percent = battery_percent;	// uint8_t:Remaining battery energy: (0%: 0, 100%: 255)
	p->errors_uart = errors_uart;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception on the MAV)
	p->errors_i2c = errors_i2c;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
	p->errors_spi = errors_spi;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
	p->errors_can = errors_can;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
James Goppert's avatar
James Goppert committed
63

lm's avatar
lm committed
64
	return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SYS_STATUS_LEN);
James Goppert's avatar
James Goppert committed
65 66 67 68 69 70 71 72
}

/**
 * @brief Pack a sys_status message
 * @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 was sent over
 * @param msg The MAVLink message to compress the data into
73 74 75
 * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 * @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: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
James Goppert's avatar
James Goppert committed
76
 * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
77 78 79 80 81 82 83 84
 * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 * @param current_battery Battery current, in milliamperes (1 = 1 milliampere)
 * @param watt Watts consumed from this battery since startup
 * @param battery_percent Remaining battery energy: (0%: 0, 100%: 255)
 * @param errors_uart Dropped packets on all links (packets that were corrupted on reception on the MAV)
 * @param errors_i2c Dropped packets on all links (packets that were corrupted on reception)
 * @param errors_spi Dropped packets on all links (packets that were corrupted on reception)
 * @param errors_can Dropped packets on all links (packets that were corrupted on reception)
James Goppert's avatar
James Goppert committed
85 86
 * @return length of the message in bytes (excluding serial stream start sign)
 */
87
static inline uint16_t mavlink_msg_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can)
James Goppert's avatar
James Goppert committed
88
{
lm's avatar
lm committed
89
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
James Goppert's avatar
James Goppert committed
90 91
	msg->msgid = MAVLINK_MSG_ID_SYS_STATUS;

92 93 94
	p->onboard_control_sensors_present = onboard_control_sensors_present;	// uint16_t:Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	p->onboard_control_sensors_enabled = onboard_control_sensors_enabled;	// uint16_t:Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	p->onboard_control_sensors_health = onboard_control_sensors_health;	// uint16_t:Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
lm's avatar
lm committed
95
	p->load = load;	// uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
96 97 98 99 100 101 102 103
	p->voltage_battery = voltage_battery;	// uint16_t:Battery voltage, in millivolts (1 = 1 millivolt)
	p->current_battery = current_battery;	// uint16_t:Battery current, in milliamperes (1 = 1 milliampere)
	p->watt = watt;	// uint16_t:Watts consumed from this battery since startup
	p->battery_percent = battery_percent;	// uint8_t:Remaining battery energy: (0%: 0, 100%: 255)
	p->errors_uart = errors_uart;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception on the MAV)
	p->errors_i2c = errors_i2c;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
	p->errors_spi = errors_spi;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
	p->errors_can = errors_can;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
James Goppert's avatar
James Goppert committed
104

lm's avatar
lm committed
105
	return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SYS_STATUS_LEN);
James Goppert's avatar
James Goppert committed
106 107 108 109 110 111 112 113 114 115 116 117
}

/**
 * @brief Encode a sys_status struct into a message
 *
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param msg The MAVLink message to compress the data into
 * @param sys_status C-struct to read the message contents from
 */
static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_status_t* sys_status)
{
118
	return mavlink_msg_sys_status_pack(system_id, component_id, msg, sys_status->onboard_control_sensors_present, sys_status->onboard_control_sensors_enabled, sys_status->onboard_control_sensors_health, sys_status->load, sys_status->voltage_battery, sys_status->current_battery, sys_status->watt, sys_status->battery_percent, sys_status->errors_uart, sys_status->errors_i2c, sys_status->errors_spi, sys_status->errors_can);
James Goppert's avatar
James Goppert committed
119 120
}

lm's avatar
lm committed
121 122

#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
James Goppert's avatar
James Goppert committed
123 124 125 126
/**
 * @brief Send a sys_status message
 * @param chan MAVLink channel to send the message
 *
127 128 129
 * @param onboard_control_sensors_present Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 * @param onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
 * @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: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
James Goppert's avatar
James Goppert committed
130
 * @param load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
131 132 133 134 135 136 137 138
 * @param voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
 * @param current_battery Battery current, in milliamperes (1 = 1 milliampere)
 * @param watt Watts consumed from this battery since startup
 * @param battery_percent Remaining battery energy: (0%: 0, 100%: 255)
 * @param errors_uart Dropped packets on all links (packets that were corrupted on reception on the MAV)
 * @param errors_i2c Dropped packets on all links (packets that were corrupted on reception)
 * @param errors_spi Dropped packets on all links (packets that were corrupted on reception)
 * @param errors_can Dropped packets on all links (packets that were corrupted on reception)
James Goppert's avatar
James Goppert committed
139
 */
140
static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint16_t onboard_control_sensors_present, uint16_t onboard_control_sensors_enabled, uint16_t onboard_control_sensors_health, uint16_t load, uint16_t voltage_battery, uint16_t current_battery, uint16_t watt, uint8_t battery_percent, uint16_t errors_uart, uint16_t errors_i2c, uint16_t errors_spi, uint16_t errors_can)
lm's avatar
lm committed
141 142 143 144
{
	mavlink_header_t hdr;
	mavlink_sys_status_t payload;

lm's avatar
lm committed
145
	MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SYS_STATUS_LEN )
146 147 148
	payload.onboard_control_sensors_present = onboard_control_sensors_present;	// uint16_t:Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	payload.onboard_control_sensors_enabled = onboard_control_sensors_enabled;	// uint16_t:Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
	payload.onboard_control_sensors_health = onboard_control_sensors_health;	// uint16_t:Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
lm's avatar
lm committed
149
	payload.load = load;	// uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
150 151 152 153 154 155 156 157
	payload.voltage_battery = voltage_battery;	// uint16_t:Battery voltage, in millivolts (1 = 1 millivolt)
	payload.current_battery = current_battery;	// uint16_t:Battery current, in milliamperes (1 = 1 milliampere)
	payload.watt = watt;	// uint16_t:Watts consumed from this battery since startup
	payload.battery_percent = battery_percent;	// uint8_t:Remaining battery energy: (0%: 0, 100%: 255)
	payload.errors_uart = errors_uart;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception on the MAV)
	payload.errors_i2c = errors_i2c;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
	payload.errors_spi = errors_spi;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
	payload.errors_can = errors_can;	// uint16_t:Dropped packets on all links (packets that were corrupted on reception)
lm's avatar
lm committed
158 159 160 161 162 163 164 165 166

	hdr.STX = MAVLINK_STX;
	hdr.len = MAVLINK_MSG_ID_SYS_STATUS_LEN;
	hdr.msgid = MAVLINK_MSG_ID_SYS_STATUS;
	hdr.sysid = mavlink_system.sysid;
	hdr.compid = mavlink_system.compid;
	hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;
	mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;
	mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );
167
	mavlink_send_mem(chan, (uint8_t *)&payload, sizeof(payload) );
lm's avatar
lm committed
168

lm's avatar
lm committed
169 170 171
	crc_init(&hdr.ck);
	crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN);
	crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len );
172
	crc_accumulate( 0xDA, &hdr.ck); /// include key in X25 checksum
lm's avatar
lm committed
173 174
	mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES);
	MAVLINK_BUFFER_CHECK_END
James Goppert's avatar
James Goppert committed
175 176 177 178 179 180
}

#endif
// MESSAGE SYS_STATUS UNPACKING

/**
181
 * @brief Get field onboard_control_sensors_present from sys_status message
James Goppert's avatar
James Goppert committed
182
 *
183
 * @return Bitmask showing which onboard controllers and sensors are present. Value of 0: not present. Value of 1: present. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
James Goppert's avatar
James Goppert committed
184
 */
185
static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_present(const mavlink_message_t* msg)
James Goppert's avatar
James Goppert committed
186
{
lm's avatar
lm committed
187
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
188
	return (uint16_t)(p->onboard_control_sensors_present);
James Goppert's avatar
James Goppert committed
189 190 191
}

/**
192
 * @brief Get field onboard_control_sensors_enabled from sys_status message
James Goppert's avatar
James Goppert committed
193
 *
194
 * @return Bitmask showing which onboard controllers and sensors are enabled:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
James Goppert's avatar
James Goppert committed
195
 */
196
static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_enabled(const mavlink_message_t* msg)
James Goppert's avatar
James Goppert committed
197
{
lm's avatar
lm committed
198
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
199
	return (uint16_t)(p->onboard_control_sensors_enabled);
James Goppert's avatar
James Goppert committed
200 201 202
}

/**
203
 * @brief Get field onboard_control_sensors_health from sys_status message
James Goppert's avatar
James Goppert committed
204
 *
205
 * @return Bitmask showing which onboard controllers and sensors are operational or have an error:  Value of 0: not enabled. Value of 1: enabled. Indices: 0: 3D gyro, 1: 3D acc, 2: 3D mag, 3: absolute pressure, 4: differential pressure, 5: GPS, 6: optical flow, 7: computer vision position, 8: laser based position, 9: external ground-truth (Vicon or Leica). Controllers: 10: 3D angular rate control 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control, 15: motor outputs / control
James Goppert's avatar
James Goppert committed
206
 */
207
static inline uint16_t mavlink_msg_sys_status_get_onboard_control_sensors_health(const mavlink_message_t* msg)
James Goppert's avatar
James Goppert committed
208
{
lm's avatar
lm committed
209
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
210
	return (uint16_t)(p->onboard_control_sensors_health);
James Goppert's avatar
James Goppert committed
211 212 213 214 215 216 217 218 219
}

/**
 * @brief Get field load from sys_status message
 *
 * @return Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
 */
static inline uint16_t mavlink_msg_sys_status_get_load(const mavlink_message_t* msg)
{
lm's avatar
lm committed
220 221
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
	return (uint16_t)(p->load);
James Goppert's avatar
James Goppert committed
222 223 224
}

/**
225
 * @brief Get field voltage_battery from sys_status message
James Goppert's avatar
James Goppert committed
226 227 228
 *
 * @return Battery voltage, in millivolts (1 = 1 millivolt)
 */
229
static inline uint16_t mavlink_msg_sys_status_get_voltage_battery(const mavlink_message_t* msg)
James Goppert's avatar
James Goppert committed
230
{
lm's avatar
lm committed
231
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
232
	return (uint16_t)(p->voltage_battery);
James Goppert's avatar
James Goppert committed
233 234 235
}

/**
236
 * @brief Get field current_battery from sys_status message
James Goppert's avatar
James Goppert committed
237
 *
238
 * @return Battery current, in milliamperes (1 = 1 milliampere)
James Goppert's avatar
James Goppert committed
239
 */
240
static inline uint16_t mavlink_msg_sys_status_get_current_battery(const mavlink_message_t* msg)
James Goppert's avatar
James Goppert committed
241
{
lm's avatar
lm committed
242
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
243
	return (uint16_t)(p->current_battery);
James Goppert's avatar
James Goppert committed
244 245 246
}

/**
247
 * @brief Get field watt from sys_status message
James Goppert's avatar
James Goppert committed
248
 *
249
 * @return Watts consumed from this battery since startup
James Goppert's avatar
James Goppert committed
250
 */
251
static inline uint16_t mavlink_msg_sys_status_get_watt(const mavlink_message_t* msg)
James Goppert's avatar
James Goppert committed
252
{
lm's avatar
lm committed
253
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309
	return (uint16_t)(p->watt);
}

/**
 * @brief Get field battery_percent from sys_status message
 *
 * @return Remaining battery energy: (0%: 0, 100%: 255)
 */
static inline uint8_t mavlink_msg_sys_status_get_battery_percent(const mavlink_message_t* msg)
{
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
	return (uint8_t)(p->battery_percent);
}

/**
 * @brief Get field errors_uart from sys_status message
 *
 * @return Dropped packets on all links (packets that were corrupted on reception on the MAV)
 */
static inline uint16_t mavlink_msg_sys_status_get_errors_uart(const mavlink_message_t* msg)
{
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
	return (uint16_t)(p->errors_uart);
}

/**
 * @brief Get field errors_i2c from sys_status message
 *
 * @return Dropped packets on all links (packets that were corrupted on reception)
 */
static inline uint16_t mavlink_msg_sys_status_get_errors_i2c(const mavlink_message_t* msg)
{
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
	return (uint16_t)(p->errors_i2c);
}

/**
 * @brief Get field errors_spi from sys_status message
 *
 * @return Dropped packets on all links (packets that were corrupted on reception)
 */
static inline uint16_t mavlink_msg_sys_status_get_errors_spi(const mavlink_message_t* msg)
{
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
	return (uint16_t)(p->errors_spi);
}

/**
 * @brief Get field errors_can from sys_status message
 *
 * @return Dropped packets on all links (packets that were corrupted on reception)
 */
static inline uint16_t mavlink_msg_sys_status_get_errors_can(const mavlink_message_t* msg)
{
	mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg->payload[0];
	return (uint16_t)(p->errors_can);
James Goppert's avatar
James Goppert committed
310 311 312 313 314 315 316 317 318 319
}

/**
 * @brief Decode a sys_status message into a struct
 *
 * @param msg The message to decode
 * @param sys_status C-struct to decode the message contents into
 */
static inline void mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status)
{
lm's avatar
lm committed
320
	memcpy( sys_status, msg->payload, sizeof(mavlink_sys_status_t));
James Goppert's avatar
James Goppert committed
321
}