Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

Merge branch 'master' into gps_task_time

Browse source
This commit is contained in:
Michael Keller 2021-05-17 22:44:18 +12:00 • committed by GitHub
commit e9b77e9f30
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
104 changed files with 1879 additions and 583 deletions
Download patch file Download diff file Expand all files Collapse all files

33
src/main/io/gps.c
View file

@ -968,11 +968,7 @@ static bool gpsNewFrameNMEA(char c)
gps_Msg.longitude *= -1;
break;
case 6:
if (string[0] > '0') {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
gpsSetFixState(string[0] > '0');
break;
case 7:
gps_Msg.numSat = grab_fields(string, 0);
@ -1295,11 +1291,7 @@ static bool UBLOX_parse_gps(void)
gpsSol.llh.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm
if (next_fix) {
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
gpsSetFixState(next_fix);
_new_position = true;
break;
case MSG_STATUS:
@ -1517,7 +1509,7 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir);
if (gpsConfig()->gps_use_3d_speed) {
dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f));
}
@ -1527,8 +1519,8 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
}
lastCoord[LON] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat;
lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
lastAlt = gpsSol.llh.altCm;
lastMillis = currentMillis;
}
@ -1537,8 +1529,8 @@ void GPS_reset_home_position(void)
{
if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon;
GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
// Set ground altitude
ENABLE_STATE(GPS_FIX_HOME);
@ -1568,7 +1560,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
} else {
@ -1603,4 +1595,13 @@ void onGpsNewData(void)
#endif
}
void gpsSetFixState(bool state)
{
if (state) {
ENABLE_STATE(GPS_FIX);
ENABLE_STATE(GPS_FIX_EVER);
} else {
DISABLE_STATE(GPS_FIX);
}
}
#endif