Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 14:55:18 +03:00
Code Issues Wiki Activity

Merge pull request #3207 from iNavFlight/agh_sport_gps_sensor_when_configured

Browse source 
Send GPS related sensors via S.Port even without a fix
This commit is contained in:
Konstantin Sharlaimov 2018-05-13 15:31:00 +10:00 • committed by GitHub
commit 40a95d8e3a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 15 additions and 10 deletions
Download patch file Download diff file Expand all files Collapse all files

25
src/main/telemetry/smartport.c
View file

@ -327,6 +327,15 @@ static void smartPortSendMspResponse(uint8_t *data) {
} }
#endif #endif
static bool smartPortShouldSendGPSData(void)
{
// We send GPS data if the GPS is configured and we have a fix
// or the craft has never been armed yet. This way if GPS stops working
// while in flight, the user will easily notice because the sensor will stop
// updating.
return feature(FEATURE_GPS) && (STATE(GPS_FIX) || !ARMING_FLAG(WAS_EVER_ARMED));
}
void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout) void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clearToSend, const uint32_t *requestTimeout)
{ {
if (payload) { if (payload) {
@ -485,10 +494,9 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
break; break;
} }
case FSSP_DATAID_T2 : case FSSP_DATAID_T2 :
if (sensors(SENSOR_GPS)) { if (smartPortShouldSendGPSData()) {
#ifdef USE_GPS
uint32_t tmpi = 0; uint32_t tmpi = 0;
#ifdef USE_GPS
// ones and tens columns (# of satellites 0 - 99) // ones and tens columns (# of satellites 0 - 99)
tmpi += constrain(gpsSol.numSat, 0, 99); tmpi += constrain(gpsSol.numSat, 0, 99);
@ -506,14 +514,11 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
smartPortSendPackage(id, tmpi); smartPortSendPackage(id, tmpi);
*clearToSend = false; *clearToSend = false;
#endif #endif
} else if (feature(FEATURE_GPS)) {
smartPortSendPackage(id, 0);
*clearToSend = false;
} }
break; break;
#ifdef USE_GPS #ifdef USE_GPS
case FSSP_DATAID_SPEED : case FSSP_DATAID_SPEED :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (smartPortShouldSendGPSData()) {
//convert to knots: 1cm/s = 0.0194384449 knots //convert to knots: 1cm/s = 0.0194384449 knots
//Speed should be sent in knots/1000 (GPS speed is in cm/s) //Speed should be sent in knots/1000 (GPS speed is in cm/s)
uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100; uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
@ -522,7 +527,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
} }
break; break;
case FSSP_DATAID_LATLONG : case FSSP_DATAID_LATLONG :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (smartPortShouldSendGPSData()) {
uint32_t tmpui = 0; uint32_t tmpui = 0;
// the same ID is sent twice, one for longitude, one for latitude // the same ID is sent twice, one for longitude, one for latitude
// the MSB of the sent uint32_t helps FrSky keep track // the MSB of the sent uint32_t helps FrSky keep track
@ -542,13 +547,13 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
} }
break; break;
case FSSP_DATAID_HOME_DIST : case FSSP_DATAID_HOME_DIST :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (smartPortShouldSendGPSData()) {
smartPortSendPackage(id, GPS_distanceToHome); smartPortSendPackage(id, GPS_distanceToHome);
*clearToSend = false; *clearToSend = false;
} }
break; break;
case FSSP_DATAID_GPS_ALT : case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (smartPortShouldSendGPSData()) {
smartPortSendPackage(id, gpsSol.llh.alt); // cm smartPortSendPackage(id, gpsSol.llh.alt); // cm
*clearToSend = false; *clearToSend = false;
} }