diff --git a/flix/mavlink.ino b/flix/mavlink.ino index 5054070..b3f508a 100644 --- a/flix/mavlink.ino +++ b/flix/mavlink.ino @@ -13,6 +13,7 @@ int mavlinkSysId = 1; Rate telemetrySlow(2); Rate telemetryAttitude(20); +Rate telemetryAttitudeTarget(0); Rate telemetryRC(10); Rate telemetryMotors(10); Rate telemetryIMU(15); @@ -65,6 +66,15 @@ void sendMavlink() { sendMessage(&msg); } + if (telemetryAttitudeTarget) { + const float att[] = {attitudeTarget.w, attitudeTarget.x, -attitudeTarget.y, -attitudeTarget.z}; + Vector rt = attitudeTarget.invalid() ? ratesTarget : ratesExtra; + mavlink_msg_attitude_target_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, time, + attitudeTarget.invalid() ? ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE : 0, + att, rt.x, -rt.y, -rt.z, thrustTarget); + sendMessage(&msg); + } + if (telemetryRC && channels[0]) { // 0 means no RC input mavlink_msg_rc_channels_raw_pack(mavlinkSysId, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0, channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX); diff --git a/flix/parameters.ino b/flix/parameters.ino index 1bcbf2e..f84637a 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -125,6 +125,7 @@ Parameter parameters[] = { {"MAV_SYS_ID", &mavlinkSysId}, {"MAV_RATE_SLOW", &telemetrySlow.rate}, {"MAV_RATE_ATT", &telemetryAttitude.rate}, + {"MAV_RATE_ATT_TG", &telemetryAttitudeTarget.rate}, {"MAV_RATE_RC", &telemetryRC.rate}, {"MAV_RATE_MOT", &telemetryMotors.rate}, {"MAV_RATE_IMU", &telemetryIMU.rate},