static inline void hottEAMUpdateAltitudeAndClimbrate(HOTT_EAM_MSG_t *hottEAMMessage)
{
const int32_t alt = MAX(0, (int32_t)(getEstimatedActualPosition(Z) / 100.0f + HOTT_GPS_ALTITUDE_OFFSET)); // Value of 500 = 0m
hottEAMMessage->altitude_L = alt & 0xFF;
hottEAMMessage->altitude_H = alt >> 8;
const int32_t climbrate = MAX(0, (int32_t)(getEstimatedActualVelocity(Z) + 30000));
hottEAMMessage->climbrate_L = climbrate & 0xFF;
hottEAMMessage->climbrate_H = climbrate >> 8;
const int32_t climbrate3s = MAX(0, (int32_t)(3.0f * getEstimatedActualVelocity(Z) / 100 + 120));
hottEAMMessage->climbrate3s = climbrate3s & 0xFF;
}
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{
hottGPSMessage->gps_satelites = gpsSol.numSat;
// Report climb rate regardless of GPS fix
const int32_t climbrate = MAX(0, getEstimatedActualVelocity(Z) + 30000);
hottGPSMessage->climbrate_L = climbrate & 0xFF;
hottGPSMessage->climbrate_H = climbrate >> 8;
const int32_t climbrate3s = MAX(0, 3.0f * getEstimatedActualVelocity(Z) / 100 + 120);
hottGPSMessage->climbrate3s = climbrate3s & 0xFF;
if (!STATE(GPS_FIX)) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return;
}
if (gpsSol.fixType == GPS_FIX_3D) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
} else {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
}
addGPSCoordinates(hottGPSMessage, gpsSol.llh.lat, gpsSol.llh.lon);
// GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement)
const uint16_t speed = (gpsSol.groundSpeed * 36) / 1000;
hottGPSMessage->gps_speed_L = speed & 0x00FF;
hottGPSMessage->gps_speed_H = speed >> 8;
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
const uint16_t hottGpsAltitude = (gpsSol.llh.alt / 100) + HOTT_GPS_ALTITUDE_OFFSET; // meters
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8;
hottGPSMessage->home_direction = GPS_directionToHome;
}
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 = gpsSol.groundSpeed / 100.0f;
}
#endif
// select best source for altitude
#if defined(NAV)
mavAltitude = getEstimatedActualPosition(Z) / 100.0f;
mavClimbRate = getEstimatedActualVelocity(Z) / 100.0f;
#elif defined(GPS)
if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS
mavAltitude = gpsSol.llh.alt;
}
#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(NAV_ALTHOLD_MODE))
mavCustomMode = 2; //Alt Hold
if (FLIGHT_MODE(NAV_RTH_MODE))
mavCustomMode = 6; //Return to Launch
if (FLIGHT_MODE(NAV_POSHOLD_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 if (isCalibrating()) {
mavSystemState = MAV_STATE_CALIBRATING;
}
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 handleSmartPortTelemetry(void)
{
uint32_t smartPortLastServiceTime = millis();
if (!smartPortTelemetryEnabled) {
return;
}
if (!canSendSmartPortTelemetry()) {
return;
}
while (serialRxBytesWaiting(smartPortSerialPort) > 0) {
uint8_t c = serialRead(smartPortSerialPort);
smartPortDataReceive(c);
}
uint32_t now = millis();
// if timed out, reconfigure the UART back to normal so the GUI or CLI works
if ((now - smartPortLastRequestTime) > SMARTPORT_NOT_CONNECTED_TIMEOUT_MS) {
smartPortState = SPSTATE_TIMEDOUT;
return;
}
while (smartPortHasRequest) {
// Ensure we won't get stuck in the loop if there happens to be nothing available to send in a timely manner - dump the slot if we loop in there for too long.
if ((millis() - smartPortLastServiceTime) > SMARTPORT_SERVICE_TIMEOUT_MS) {
smartPortHasRequest = 0;
return;
}
// we can send back any data we want, our table keeps track of the order and frequency of each data type we send
uint16_t id = frSkyDataIdTable[smartPortIdCnt];
if (id == 0) { // end of table reached, loop back
smartPortIdCnt = 0;
id = frSkyDataIdTable[smartPortIdCnt];
}
smartPortIdCnt++;
int32_t tmpi;
switch(id) {
#ifdef GPS
case FSSP_DATAID_SPEED :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
//convert to knots: 1cm/s = 0.0194384449 knots
//Speed should be sent in knots/1000 (GPS speed is in cm/s)
uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0;
}
break;
#endif
case FSSP_DATAID_VFAS :
if (feature(FEATURE_VBAT)) {
smartPortSendPackage(id, vbat * 10); // given in 0.1V, convert to volts
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_CURRENT :
if (feature(FEATURE_CURRENT_METER)) {
smartPortSendPackage(id, amperage / 10); // given in 10mA steps, unknown requested unit
smartPortHasRequest = 0;
}
break;
//case FSSP_DATAID_RPM :
case FSSP_DATAID_ALTITUDE :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, BaroAlt); // unknown given unit, requested 100 = 1 meter
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_FUEL :
if (feature(FEATURE_CURRENT_METER)) {
smartPortSendPackage(id, mAhDrawn); // given in mAh, unknown requested unit
smartPortHasRequest = 0;
}
break;
//case FSSP_DATAID_ADC1 :
//case FSSP_DATAID_ADC2 :
#ifdef GPS
case FSSP_DATAID_LATLONG :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
uint32_t tmpui = 0;
// the same ID is sent twice, one for longitude, one for latitude
// the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track
if (smartPortIdCnt & 1) {
tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
}
else {
tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare
tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
}
smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0;
}
break;
#endif
//case FSSP_DATAID_CAP_USED :
case FSSP_DATAID_VARIO :
if (sensors(SENSOR_BARO)) {
smartPortSendPackage(id, getEstimatedActualVelocity(Z)); // unknown given unit but requested in 100 = 1m/s
smartPortHasRequest = 0;
}
break;
case FSSP_DATAID_HEADING :
smartPortSendPackage(id, attitude.values.yaw * 10); // given in 10*deg, requested in 10000 = 100 deg
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCX :
smartPortSendPackage(id, accADC[X] / 44);
// unknown input and unknown output unit
// we can only show 00.00 format, another digit won't display right on Taranis
// dividing by roughly 44 will give acceleration in G units
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCY :
smartPortSendPackage(id, accADC[Y] / 44);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_ACCZ :
smartPortSendPackage(id, accADC[Z] / 44);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_T1 :
// we send all the flags as decimal digits for easy reading
tmpi = 10000; // start off with at least one digit so the most significant 0 won't be cut off
// the Taranis seems to be able to fit 5 digits on the screen
// the Taranis seems to consider this number a signed 16 bit integer
if (ARMING_FLAG(OK_TO_ARM))
tmpi += 1;
if (ARMING_FLAG(PREVENT_ARMING))
tmpi += 2;
if (ARMING_FLAG(ARMED))
tmpi += 4;
if (FLIGHT_MODE(ANGLE_MODE))
tmpi += 10;
if (FLIGHT_MODE(HORIZON_MODE))
tmpi += 20;
if (FLIGHT_MODE(UNUSED_MODE))
tmpi += 40;
if (FLIGHT_MODE(PASSTHRU_MODE))
tmpi += 40;
if (FLIGHT_MODE(MAG_MODE))
tmpi += 100;
if (FLIGHT_MODE(NAV_ALTHOLD_MODE))
tmpi += 200;
if (FLIGHT_MODE(NAV_POSHOLD_MODE))
tmpi += 400;
if (FLIGHT_MODE(NAV_RTH_MODE))
tmpi += 1000;
if (FLIGHT_MODE(NAV_WP_MODE))
tmpi += 2000;
if (FLIGHT_MODE(HEADFREE_MODE))
tmpi += 4000;
smartPortSendPackage(id, (uint32_t)tmpi);
smartPortHasRequest = 0;
break;
case FSSP_DATAID_T2 :
if (sensors(SENSOR_GPS)) {
#ifdef GPS
// provide GPS lock status
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
smartPortHasRequest = 0;
#endif
}
else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0);
smartPortHasRequest = 0;
}
break;
#ifdef GPS
case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, gpsSol.llh.alt); // cm
smartPortHasRequest = 0;
}
break;
#endif
default:
break;
// if nothing is sent, smartPortHasRequest isn't cleared, we already incremented the counter, just loop back to the start
}
}
}
