void mavlinkSendRCChannelsAndRSSI(void)
{
uint16_t msgLength;
mavlink_msg_rc_channels_raw_pack(0, 200, &mavMsg,
// time_boot_ms Timestamp (milliseconds since system boot)
millis(),
// port Servo output port (set of 8 outputs = 1 port). Most MAVs will just use one, but this allows to encode more than 8 servos.
0,
// chan1_raw RC channel 1 value, in microseconds
(rxRuntimeConfig.channelCount >= 1) ? rcData[0] : 0,
// chan2_raw RC channel 2 value, in microseconds
(rxRuntimeConfig.channelCount >= 2) ? rcData[1] : 0,
// chan3_raw RC channel 3 value, in microseconds
(rxRuntimeConfig.channelCount >= 3) ? rcData[2] : 0,
// chan4_raw RC channel 4 value, in microseconds
(rxRuntimeConfig.channelCount >= 4) ? rcData[3] : 0,
// chan5_raw RC channel 5 value, in microseconds
(rxRuntimeConfig.channelCount >= 5) ? rcData[4] : 0,
// chan6_raw RC channel 6 value, in microseconds
(rxRuntimeConfig.channelCount >= 6) ? rcData[5] : 0,
// chan7_raw RC channel 7 value, in microseconds
(rxRuntimeConfig.channelCount >= 7) ? rcData[6] : 0,
// chan8_raw RC channel 8 value, in microseconds
(rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0,
// rssi Receive signal strength indicator, 0: 0%, 255: 100%
scaleRange(rssi, 0, 1023, 0, 255));
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
void mavlinkSendSystemStatus(void)
{
uint16_t msgLength;
uint32_t onboardControlAndSensors = 35843;
/*
onboard_control_sensors_present Bitmask
fedcba9876543210
1000110000000011 For all = 35843
0001000000000100 With Mag = 4100
0010000000001000 With Baro = 8200
0100000000100000 With GPS = 16416
0000001111111111
*/
if (sensors(SENSOR_MAG)) onboardControlAndSensors |= 4100;
if (sensors(SENSOR_BARO)) onboardControlAndSensors |= 8200;
if (sensors(SENSOR_GPS)) onboardControlAndSensors |= 16416;
mavlink_msg_sys_status_pack(0, 200, &mavMsg,
// 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
onboardControlAndSensors,
// onboard_control_sensors_enabled Bitmask showing which onboard controllers and sensors are enabled
onboardControlAndSensors,
// onboard_control_sensors_health Bitmask showing which onboard controllers and sensors are operational or have an error.
onboardControlAndSensors & 1023,
// load Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
0,
// voltage_battery Battery voltage, in millivolts (1 = 1 millivolt)
feature(FEATURE_VBAT) ? vbat * 100 : 0,
// current_battery Battery current, in 10*milliamperes (1 = 10 milliampere), -1: autopilot does not measure the current
feature(FEATURE_VBAT) ? amperage : -1,
// battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot estimate the remaining battery
feature(FEATURE_VBAT) ? calculateBatteryPercentage() : 100,
// 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)
0,
// errors_comm Communication errors (UART, I2C, SPI, CAN), dropped packets on all links (packets that were corrupted on reception on the MAV)
0,
// errors_count1 Autopilot-specific errors
0,
// errors_count2 Autopilot-specific errors
0,
// errors_count3 Autopilot-specific errors
0,
// errors_count4 Autopilot-specific errors
0);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
void mavlinkSendAttitude(void)
{
uint16_t msgLength;
mavlink_msg_attitude_pack(0, 200, &mavMsg,
// time_boot_ms Timestamp (milliseconds since system boot)
millis(),
// roll Roll angle (rad)
DECIDEGREES_TO_RADIANS(attitude.values.roll),
// pitch Pitch angle (rad)
DECIDEGREES_TO_RADIANS(-attitude.values.pitch),
// yaw Yaw angle (rad)
DECIDEGREES_TO_RADIANS(attitude.values.yaw),
// rollspeed Roll angular speed (rad/s)
0,
// pitchspeed Pitch angular speed (rad/s)
0,
// yawspeed Yaw angular speed (rad/s)
0);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
void mavlinkSendHUDAndHeartbeat(void)
{
uint16_t msgLength;
float mavAltitude = 0;
float mavGroundSpeed = 0;
float mavAirSpeed = 0;
float mavClimbRate = 0;
#if defined(GPS)
// use ground speed if source available
if (sensors(SENSOR_GPS)) {
mavGroundSpeed = GPS_speed / 100.0f;
}
#endif
// select best source for altitude
#if defined(BARO) || defined(SONAR)
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
// Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
mavAltitude = altitudeHoldGetEstimatedAltitude() / 100.0;
}
#if defined(GPS)
else if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#endif
#elif defined(GPS)
if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude;
}
#endif
mavlink_msg_vfr_hud_pack(0, 200, &mavMsg,
// airspeed Current airspeed in m/s
mavAirSpeed,
// groundspeed Current ground speed in m/s
mavGroundSpeed,
// heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw),
// throttle Current throttle setting in integer percent, 0 to 100
scaleRange(constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX), PWM_RANGE_MIN, PWM_RANGE_MAX, 0, 100),
// alt Current altitude (MSL), in meters, if we have sonar or baro use them, otherwise use GPS (less accurate)
mavAltitude,
// climb Current climb rate in meters/second
mavClimbRate);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
uint8_t mavModes = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
if (ARMING_FLAG(ARMED))
mavModes |= MAV_MODE_FLAG_SAFETY_ARMED;
uint8_t mavSystemType;
switch(mixerConfig()->mixerMode)
{
case MIXER_TRI:
mavSystemType = MAV_TYPE_TRICOPTER;
break;
case MIXER_QUADP:
case MIXER_QUADX:
case MIXER_Y4:
case MIXER_VTAIL4:
mavSystemType = MAV_TYPE_QUADROTOR;
break;
case MIXER_Y6:
case MIXER_HEX6:
case MIXER_HEX6X:
mavSystemType = MAV_TYPE_HEXAROTOR;
break;
case MIXER_OCTOX8:
case MIXER_OCTOFLATP:
case MIXER_OCTOFLATX:
mavSystemType = MAV_TYPE_OCTOROTOR;
break;
case MIXER_FLYING_WING:
case MIXER_AIRPLANE:
case MIXER_CUSTOM_AIRPLANE:
mavSystemType = MAV_TYPE_FIXED_WING;
break;
case MIXER_HELI_120_CCPM:
case MIXER_HELI_90_DEG:
mavSystemType = MAV_TYPE_HELICOPTER;
break;
default:
mavSystemType = MAV_TYPE_GENERIC;
break;
}
// Custom mode for compatibility with APM OSDs
uint8_t mavCustomMode = 1; // Acro by default
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
mavCustomMode = 0; //Stabilize
mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
}
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE))
mavCustomMode = 2; //Alt Hold
if (FLIGHT_MODE(GPS_HOME_MODE))
mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(GPS_HOLD_MODE))
mavCustomMode = 16; //Position Hold (Earlier called Hybrid)
uint8_t mavSystemState = 0;
if (ARMING_FLAG(ARMED)) {
if (failsafeIsActive()) {
mavSystemState = MAV_STATE_CRITICAL;
}
else {
mavSystemState = MAV_STATE_ACTIVE;
}
}
else {
mavSystemState = MAV_STATE_STANDBY;
}
mavlink_msg_heartbeat_pack(0, 200, &mavMsg,
// type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
mavSystemType,
// autopilot Autopilot type / class. defined in MAV_AUTOPILOT ENUM
MAV_AUTOPILOT_GENERIC,
// base_mode System mode bitfield, see MAV_MODE_FLAGS ENUM in mavlink/include/mavlink_types.h
mavModes,
// custom_mode A bitfield for use for autopilot-specific flags.
mavCustomMode,
// system_status System status flag, see MAV_STATE ENUM
mavSystemState);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
void mavlinkSendPosition(void)
{
uint16_t msgLength;
uint8_t gpsFixType = 0;
if (!sensors(SENSOR_GPS))
return;
if (!STATE(GPS_FIX)) {
gpsFixType = 1;
}
else {
if (GPS_numSat < 5) {
gpsFixType = 2;
}
else {
gpsFixType = 3;
}
}
mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
micros(),
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gpsFixType,
// lat Latitude in 1E7 degrees
GPS_coord[LAT],
// lon Longitude in 1E7 degrees
GPS_coord[LON],
// alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 1000,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
GPS_speed,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
GPS_ground_course * 10,
// satellites_visible Number of satellites visible. If unknown, set to 255
GPS_numSat);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
// Global position
mavlink_msg_global_position_int_pack(0, 200, &mavMsg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
micros(),
// lat Latitude in 1E7 degrees
GPS_coord[LAT],
// lon Longitude in 1E7 degrees
GPS_coord[LON],
// alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 1000,
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(BARO) || defined(SONAR)
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? altitudeHoldGetEstimatedAltitude() * 10 : GPS_altitude * 1000,
#else
GPS_altitude * 1000,
#endif
// Ground X Speed (Latitude), expressed as m/s * 100
0,
// Ground Y Speed (Longitude), expressed as m/s * 100
0,
// Ground Z Speed (Altitude), expressed as m/s * 100
0,
// heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw)
);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg,
// latitude Latitude (WGS84), expressed as * 1E7
GPS_home[LAT],
// longitude Longitude (WGS84), expressed as * 1E7
GPS_home[LON],
// altitude Altitude(WGS84), expressed as * 1000
0);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
void mavlinkSendPosition(timeUs_t currentTimeUs)
{
uint16_t msgLength;
uint8_t gpsFixType = 0;
if (!sensors(SENSOR_GPS))
return;
if (gpsSol.fixType == GPS_NO_FIX)
gpsFixType = 1;
else if (gpsSol.fixType == GPS_FIX_2D)
gpsFixType = 2;
else if (gpsSol.fixType == GPS_FIX_3D)
gpsFixType = 3;
mavlink_msg_gps_raw_int_pack(0, 200, &mavMsg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
currentTimeUs,
// fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
gpsFixType,
// lat Latitude in 1E7 degrees
gpsSol.llh.lat,
// lon Longitude in 1E7 degrees
gpsSol.llh.lon,
// alt Altitude in 1E3 meters (millimeters) above MSL
gpsSol.llh.alt * 10,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535
gpsSol.groundSpeed,
// cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
gpsSol.groundCourse * 10,
// satellites_visible Number of satellites visible. If unknown, set to 255
gpsSol.numSat);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
// Global position
mavlink_msg_global_position_int_pack(0, 200, &mavMsg,
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
currentTimeUs,
// lat Latitude in 1E7 degrees
gpsSol.llh.lat,
// lon Longitude in 1E7 degrees
gpsSol.llh.lon,
// alt Altitude in 1E3 meters (millimeters) above MSL
gpsSol.llh.alt * 10,
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(NAV)
getEstimatedActualPosition(Z) * 10,
#else
gpsSol.llh.alt * 10,
#endif
// Ground X Speed (Latitude), expressed as m/s * 100
0,
// Ground Y Speed (Longitude), expressed as m/s * 100
0,
// Ground Z Speed (Altitude), expressed as m/s * 100
0,
// heading Current heading in degrees, in compass units (0..360, 0=north)
DECIDEGREES_TO_DEGREES(attitude.values.yaw)
);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg,
// latitude Latitude (WGS84), expressed as * 1E7
GPS_home.lat,
// longitude Longitude (WGS84), expressed as * 1E7
GPS_home.lon,
// altitude Altitude(WGS84), expressed as * 1000
GPS_home.alt * 10); // FIXME
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength);
}
