rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Move some GPS variables into struct. Some alignment with iNav

This commit is contained in:
Martin Budden 2017-06-25 06:55:17 +01:00
parent 582af3d515
commit 564e0c94b8
22 changed files with 192 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
src/main/blackbox/blackbox.c
View File

@ -992,16 +992,16 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time); blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
} }
blackboxWriteUnsignedVB(GPS_numSat); blackboxWriteUnsignedVB(gpsSol.numSat);
blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]);
blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]);
blackboxWriteUnsignedVB(GPS_altitude); blackboxWriteUnsignedVB(gpsSol.llh.alt);
blackboxWriteUnsignedVB(GPS_speed); blackboxWriteUnsignedVB(gpsSol.groundSpeed);
blackboxWriteUnsignedVB(GPS_ground_course); blackboxWriteUnsignedVB(gpsSol.groundCourse);
gpsHistory.GPS_numSat = GPS_numSat; gpsHistory.GPS_numSat = gpsSol.numSat;
gpsHistory.GPS_coord[0] = GPS_coord[0]; gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat;
gpsHistory.GPS_coord[1] = GPS_coord[1]; gpsHistory.GPS_coord[LON] = gpsSol.llh.lon;
} }
#endif #endif
@ -1474,8 +1474,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
if (blackboxShouldLogGpsHomeFrame()) { if (blackboxShouldLogGpsHomeFrame()) {
writeGPSHomeFrame(); writeGPSHomeFrame();
writeGPSFrame(currentTimeUs); writeGPSFrame(currentTimeUs);
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0] } else if (gpsSol.numSat != gpsHistory.GPS_numSat
|| GPS_coord[1] != gpsHistory.GPS_coord[1]) { || gpsSol.llh.lat != gpsHistory.GPS_coord[LAT]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) {
//We could check for velocity changes as well but I doubt it changes independent of position //We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame(currentTimeUs); writeGPSFrame(currentTimeUs);
} }

2
src/main/fc/fc_core.c
View File

@ -231,7 +231,7 @@ void mwArm(void)
//beep to indicate arming //beep to indicate arming
#ifdef GPS #ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5) if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX); beeper(BEEPER_ARMING_GPS_FIX);
else else
beeper(BEEPER_ARMING); beeper(BEEPER_ARMING);

34
src/main/fc/fc_msp.c
View File

@ -1113,12 +1113,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_RAW_GPS: case MSP_RAW_GPS:
sbufWriteU8(dst, STATE(GPS_FIX)); sbufWriteU8(dst, STATE(GPS_FIX));
sbufWriteU8(dst, GPS_numSat); sbufWriteU8(dst, gpsSol.numSat);
sbufWriteU32(dst, GPS_coord[LAT]); sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, GPS_coord[LON]); sbufWriteU32(dst, gpsSol.llh.lon);
sbufWriteU16(dst, GPS_altitude); sbufWriteU16(dst, gpsSol.llh.alt);
sbufWriteU16(dst, GPS_speed); sbufWriteU16(dst, gpsSol.groundSpeed);
sbufWriteU16(dst, GPS_ground_course); sbufWriteU16(dst, gpsSol.groundCourse);
break; break;
case MSP_COMP_GPS: case MSP_COMP_GPS:
@ -1129,12 +1129,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
sbufWriteU8(dst, GPS_numCh); sbufWriteU8(dst, GPS_numCh);
for (int i = 0; i < GPS_numCh; i++) { for (int i = 0; i < GPS_numCh; i++) {
sbufWriteU8(dst, GPS_svinfo_chn[i]); sbufWriteU8(dst, GPS_svinfo_chn[i]);
sbufWriteU8(dst, GPS_svinfo_svid[i]); sbufWriteU8(dst, GPS_svinfo_svid[i]);
sbufWriteU8(dst, GPS_svinfo_quality[i]); sbufWriteU8(dst, GPS_svinfo_quality[i]);
sbufWriteU8(dst, GPS_svinfo_cno[i]); sbufWriteU8(dst, GPS_svinfo_cno[i]);
} }
break; break;
#endif #endif
@ -1838,11 +1838,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
} else { } else {
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
} }
GPS_numSat = sbufReadU8(src); gpsSol.numSat = sbufReadU8(src);
GPS_coord[LAT] = sbufReadU32(src); gpsSol.llh.lat = sbufReadU32(src);
GPS_coord[LON] = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src);
GPS_altitude = sbufReadU16(src); gpsSol.llh.alt = sbufReadU16(src);
GPS_speed = sbufReadU16(src); gpsSol.groundSpeed = sbufReadU16(src);
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
break; break;

4
src/main/flight/imu.c
View File

@ -419,9 +419,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
useMag = true; useMag = true;
} }
#if defined(GPS) #if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) { else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course); rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
useYaw = true; useYaw = true;
} }
#endif #endif

47
src/main/flight/navigation.c
View File

@ -254,7 +254,7 @@ void GPS_calculateDistanceAndDirectionToHome(void)
if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist; uint32_t dist;
int32_t dir; int32_t dir;
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir); GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir);
GPS_distanceToHome = dist / 100; GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100; GPS_directionToHome = dir / 100;
} else { } else {
@ -270,7 +270,7 @@ void onGpsNewData(void)
uint16_t speed; uint16_t speed;
if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) { if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
return; return;
} }
@ -284,7 +284,7 @@ void onGpsNewData(void)
#if defined(GPS_FILTERING) #if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
// How close we are to a degree line ? its the first three digits from the fractions of degree // How close we are to a degree line ? its the first three digits from the fractions of degree
@ -296,8 +296,13 @@ void onGpsNewData(void)
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000);
if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode... if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode...
if (fraction3[axis] > 1 && fraction3[axis] < 999) if (fraction3[axis] > 1 && fraction3[axis] < 999) {
GPS_coord[axis] = GPS_filtered[axis]; if (axis == LAT) {
gpsSol.llh.lat = GPS_filtered[LAT];
} else {
gpsSol.llh.lon = GPS_filtered[LON];
}
}
} }
} }
#endif #endif
@ -320,8 +325,8 @@ void onGpsNewData(void)
// we are navigating // we are navigating
// gps nav calculations, these are common for nav and poshold // gps nav calculations, these are common for nav and poshold
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
switch (nav_mode) { switch (nav_mode) {
case NAV_MODE_POSHOLD: case NAV_MODE_POSHOLD:
@ -359,10 +364,10 @@ void onGpsNewData(void)
void GPS_reset_home_position(void) void GPS_reset_home_position(void)
{ {
if (STATE(GPS_FIX) && GPS_numSat >= 5) { if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT]; GPS_home[LAT] = gpsSol.llh.lat;
GPS_home[LON] = GPS_coord[LON]; GPS_home[LON] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading
// Set ground altitude // Set ground altitude
ENABLE_STATE(GPS_FIX_HOME); ENABLE_STATE(GPS_FIX_HOME);
@ -429,10 +434,10 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon)
GPS_WP[LON] = *lon; GPS_WP[LON] = *lon;
GPS_calc_longitude_scaling(*lat); GPS_calc_longitude_scaling(*lat);
GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing);
nav_bearing = target_bearing; nav_bearing = target_bearing;
GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon);
original_target_bearing = target_bearing; original_target_bearing = target_bearing;
waypoint_speed_gov = navigationConfig()->nav_speed_min; waypoint_speed_gov = navigationConfig()->nav_speed_min;
} }
@ -484,13 +489,11 @@ static void GPS_calc_velocity(void)
static int16_t speed_old[2] = { 0, 0 }; static int16_t speed_old[2] = { 0, 0 };
static int32_t last_coord[2] = { 0, 0 }; static int32_t last_coord[2] = { 0, 0 };
static uint8_t init = 0; static uint8_t init = 0;
// y_GPS_speed positive = Up
// x_GPS_speed positive = Right
if (init) { if (init) {
float tmp = 1.0f / dTnav; float tmp = 1.0f / dTnav;
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp; actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp;
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * tmp; actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp;
actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2; actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2;
actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2; actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
@ -500,8 +503,8 @@ static void GPS_calc_velocity(void)
} }
init = 1; init = 1;
last_coord[LON] = GPS_coord[LON]; last_coord[LON] = gpsSol.llh.lon;
last_coord[LAT] = GPS_coord[LAT]; last_coord[LAT] = gpsSol.llh.lat;
} }
//////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////
@ -666,7 +669,7 @@ void updateGpsWaypointsAndMode(void)
bool resetNavNow = false; bool resetNavNow = false;
static bool gpsReadyBeepDone = false; static bool gpsReadyBeepDone = false;
if (STATE(GPS_FIX) && GPS_numSat >= 5) { if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
// //
// process HOME mode // process HOME mode
@ -701,8 +704,8 @@ void updateGpsWaypointsAndMode(void)
// Transition to HOLD mode // Transition to HOLD mode
ENABLE_FLIGHT_MODE(GPS_HOLD_MODE); ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
GPS_hold[LAT] = GPS_coord[LAT]; GPS_hold[LAT] = gpsSol.llh.lat;
GPS_hold[LON] = GPS_coord[LON]; GPS_hold[LON] = gpsSol.llh.lon;
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
nav_mode = NAV_MODE_POSHOLD; nav_mode = NAV_MODE_POSHOLD;
resetNavNow = true; resetNavNow = true;

4
src/main/io/beeper.c
View File

@ -295,12 +295,12 @@ void beeperConfirmationBeeps(uint8_t beepCount)
void beeperGpsStatus(void) void beeperGpsStatus(void)
{ {
// if GPS fix then beep out number of satellites // if GPS fix then beep out number of satellites
if (STATE(GPS_FIX) && GPS_numSat >= 5) { if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
uint8_t i = 0; uint8_t i = 0;
do { do {
beep_multiBeeps[i++] = 5; beep_multiBeeps[i++] = 5;
beep_multiBeeps[i++] = 10; beep_multiBeeps[i++] = 10;
} while (i < MAX_MULTI_BEEPS && GPS_numSat > i / 2); } while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2);
beep_multiBeeps[i-1] = 50; // extend last pause beep_multiBeeps[i-1] = 50; // extend last pause
beep_multiBeeps[i] = BEEPER_COMMAND_STOP; beep_multiBeeps[i] = BEEPER_COMMAND_STOP;

8
src/main/io/dashboard.c
View File

@ -365,22 +365,22 @@ void showGpsPage() {
char fixChar = STATE(GPS_FIX) ? 'Y' : 'N'; char fixChar = STATE(GPS_FIX) ? 'Y' : 'N';
tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar); tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar);
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER); tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER);
padLineBuffer(); padLineBuffer();
i2c_OLED_set_line(rowIndex++); i2c_OLED_set_line(rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed); tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
padHalfLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_line(rowIndex); i2c_OLED_set_line(rowIndex);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);
tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course); tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
padHalfLineBuffer(); padHalfLineBuffer();
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
i2c_OLED_send_string(lineBuffer); i2c_OLED_send_string(lineBuffer);

40
src/main/io/gps.c
View File

@ -71,18 +71,11 @@ static char *gpsPacketLogChar = gpsPacketLog;
// ********************** // **********************
// GPS // GPS
// ********************** // **********************
int32_t GPS_coord[2]; // LAT/LON gpsSolutionData_t gpsSol;
uint8_t GPS_numSat;
uint16_t GPS_hdop = 9999; // Compute GPS quality signal
uint32_t GPS_packetCount = 0; uint32_t GPS_packetCount = 0;
uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received. uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
uint8_t GPS_numCh; // Number of channels uint8_t GPS_numCh; // Number of channels
uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
@ -424,7 +417,7 @@ void gpsInitHardware(void)
static void updateGpsIndicator(timeUs_t currentTimeUs) static void updateGpsIndicator(timeUs_t currentTimeUs)
{ {
static uint32_t GPSLEDTime; static uint32_t GPSLEDTime;
if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) {
GPSLEDTime = currentTimeUs + 150000; GPSLEDTime = currentTimeUs + 150000;
LED1_TOGGLE; LED1_TOGGLE;
} }
@ -456,8 +449,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
gpsData.baudrateIndex %= GPS_INIT_ENTRIES; gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
} }
gpsData.lastMessage = millis(); gpsData.lastMessage = millis();
// TODO - move some / all of these into gpsData gpsSol.numSat = 0;
GPS_numSat = 0;
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
gpsSetState(GPS_INITIALIZING); gpsSetState(GPS_INITIALIZING);
break; break;
@ -746,16 +738,16 @@ static bool gpsNewFrameNMEA(char c)
*gpsPacketLogChar = LOG_NMEA_GGA; *gpsPacketLogChar = LOG_NMEA_GGA;
frameOK = 1; frameOK = 1;
if (STATE(GPS_FIX)) { if (STATE(GPS_FIX)) {
GPS_coord[LAT] = gps_Msg.latitude; gpsSol.llh.lat = gps_Msg.latitude;
GPS_coord[LON] = gps_Msg.longitude; gpsSol.llh.lon = gps_Msg.longitude;
GPS_numSat = gps_Msg.numSat; gpsSol.numSat = gps_Msg.numSat;
GPS_altitude = gps_Msg.altitude; gpsSol.llh.alt = gps_Msg.altitude;
} }
break; break;
case FRAME_RMC: case FRAME_RMC:
*gpsPacketLogChar = LOG_NMEA_RMC; *gpsPacketLogChar = LOG_NMEA_RMC;
GPS_speed = gps_Msg.speed; gpsSol.groundSpeed = gps_Msg.speed;
GPS_ground_course = gps_Msg.ground_course; gpsSol.groundCourse = gps_Msg.ground_course;
break; break;
} // end switch } // end switch
} else { } else {
@ -952,9 +944,9 @@ static bool UBLOX_parse_gps(void)
case MSG_POSLLH: case MSG_POSLLH:
*gpsPacketLogChar = LOG_UBLOX_POSLLH; *gpsPacketLogChar = LOG_UBLOX_POSLLH;
//i2c_dataset.time = _buffer.posllh.time; //i2c_dataset.time = _buffer.posllh.time;
GPS_coord[LON] = _buffer.posllh.longitude; gpsSol.llh.lon = _buffer.posllh.longitude;
GPS_coord[LAT] = _buffer.posllh.latitude; gpsSol.llh.lat = _buffer.posllh.latitude;
GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
if (next_fix) { if (next_fix) {
ENABLE_STATE(GPS_FIX); ENABLE_STATE(GPS_FIX);
} else { } else {
@ -973,14 +965,14 @@ static bool UBLOX_parse_gps(void)
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix) if (!next_fix)
DISABLE_STATE(GPS_FIX); DISABLE_STATE(GPS_FIX);
GPS_numSat = _buffer.solution.satellites; gpsSol.numSat = _buffer.solution.satellites;
GPS_hdop = _buffer.solution.position_DOP; gpsSol.hdop = _buffer.solution.position_DOP;
break; break;
case MSG_VELNED: case MSG_VELNED:
*gpsPacketLogChar = LOG_UBLOX_VELNED; *gpsPacketLogChar = LOG_UBLOX_VELNED;
// speed_3d = _buffer.velned.speed_3d; // cm/s // speed_3d = _buffer.velned.speed_3d; // cm/s
GPS_speed = _buffer.velned.speed_2d; // cm/s gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s
GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
_new_speed = true; _new_speed = true;
break; break;
case MSG_SVINFO: case MSG_SVINFO:

24
src/main/io/gps.h
View File

@ -31,8 +31,6 @@ typedef enum {
GPS_UBLOX GPS_UBLOX
} gpsProvider_e; } gpsProvider_e;
#define GPS_PROVIDER_MAX GPS_UBLOX
typedef enum { typedef enum {
SBAS_AUTO = 0, SBAS_AUTO = 0,
SBAS_EGNOS, SBAS_EGNOS,
@ -77,6 +75,21 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
int16_t mmmm; int16_t mmmm;
} gpsCoordinateDDDMMmmmm_t; } gpsCoordinateDDDMMmmmm_t;
/* LLH Location in NEU axis system */
typedef struct gpsLocation_s {
int32_t lat; // latitude * 1e+7
int32_t lon; // longitude * 1e+7
uint16_t alt; // altitude in 0.1m
} gpsLocation_t;
typedef struct gpsSolutionData_s {
uint8_t numSat;
gpsLocation_t llh;
uint16_t GPS_altitude; // altitude in 0.1m
uint16_t groundSpeed; // speed in 0.1m/s
uint16_t groundCourse; // degrees * 10
uint16_t hdop; // generic HDOP value (*100)
} gpsSolutionData_t;
typedef enum { typedef enum {
GPS_MESSAGE_STATE_IDLE = 0, GPS_MESSAGE_STATE_IDLE = 0,
@ -102,16 +115,11 @@ typedef struct gpsData_s {
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
extern gpsData_t gpsData; extern gpsData_t gpsData;
extern int32_t GPS_coord[2]; // LAT/LON extern gpsSolutionData_t gpsSol;
extern uint8_t GPS_numSat;
extern uint16_t GPS_hdop; // GPS signal quality
extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update
extern uint32_t GPS_packetCount; extern uint32_t GPS_packetCount;
extern uint32_t GPS_svInfoReceivedCount; extern uint32_t GPS_svInfoReceivedCount;
extern uint16_t GPS_altitude; // altitude in 0.1m
extern uint16_t GPS_speed; // speed in 0.1m/s
extern uint16_t GPS_ground_course; // degrees * 10
extern uint8_t GPS_numCh; // Number of channels extern uint8_t GPS_numCh; // Number of channels
extern uint8_t GPS_svinfo_chn[16]; // Channel number extern uint8_t GPS_svinfo_chn[16]; // Channel number
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID extern uint8_t GPS_svinfo_svid[16]; // Satellite ID

4
src/main/io/ledstrip.c
View File

@ -731,7 +731,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
static uint8_t gpsFlashCounter = 0; static uint8_t gpsFlashCounter = 0;
if (gpsPauseCounter > 0) { if (gpsPauseCounter > 0) {
gpsPauseCounter--; gpsPauseCounter--;
} else if (gpsFlashCounter >= GPS_numSat) { } else if (gpsFlashCounter >= gpsSol.numSat) {
gpsFlashCounter = 0; gpsFlashCounter = 0;
gpsPauseCounter = blinkPauseLength; gpsPauseCounter = blinkPauseLength;
} else { } else {
@ -743,7 +743,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
const hsvColor_t *gpsColor; const hsvColor_t *gpsColor;
if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) { if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
gpsColor = getSC(LED_SCOLOR_GPSNOSATS); gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
} else { } else {
bool colorOn = gpsPauseCounter == 0; // each interval starts with pause bool colorOn = gpsPauseCounter == 0; // each interval starts with pause

10
src/main/io/osd.c
View File

@ -277,12 +277,12 @@ static void osdDrawSingleElement(uint8_t item)
#ifdef GPS #ifdef GPS
case OSD_GPS_SATS: case OSD_GPS_SATS:
buff[0] = 0x1f; buff[0] = 0x1f;
tfp_sprintf(buff + 1, "%d", GPS_numSat); tfp_sprintf(buff + 1, "%d", gpsSol.numSat);
break; break;
case OSD_GPS_SPEED: case OSD_GPS_SPEED:
// FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K. // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K.
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(GPS_speed)); tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
break; break;
case OSD_GPS_LAT: case OSD_GPS_LAT:
@ -291,10 +291,10 @@ static void osdDrawSingleElement(uint8_t item)
int32_t val; int32_t val;
if (item == OSD_GPS_LAT) { if (item == OSD_GPS_LAT) {
buff[0] = SYM_ARROW_EAST; buff[0] = SYM_ARROW_EAST;
val = GPS_coord[LAT]; val = gpsSol.llh.lat;
} else { } else {
buff[0] = SYM_ARROW_SOUTH; buff[0] = SYM_ARROW_SOUTH;
val = GPS_coord[LON]; val = gpsSol.llh.lon;
} }
char wholeDegreeString[5]; char wholeDegreeString[5];
@ -880,7 +880,7 @@ static void osdUpdateStats(void)
{ {
int16_t value = 0; int16_t value = 0;
#ifdef GPS #ifdef GPS
value = CM_S_TO_KM_H(GPS_speed); value = CM_S_TO_KM_H(gpsSol.groundSpeed);
#endif #endif
if (stats.max_speed < value) if (stats.max_speed < value)
stats.max_speed = value; stats.max_speed = value;

14
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -778,15 +778,15 @@ static uint8_t numOfSat = 0;
#ifdef GPS #ifdef GPS
bool writeGpsPositionPrameToBST(void) bool writeGpsPositionPrameToBST(void)
{ {
if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) { if((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) {
lat = GPS_coord[LAT]; lat = gpsSol.llh.lat;
lon = GPS_coord[LON]; lon = gpsSol.llh.lon;
alt = GPS_altitude; alt = gpsSol.llh.alt;
numOfSat = GPS_numSat; numOfSat = gpsSol.numSat;
uint16_t speed = (GPS_speed * 9 / 25); uint16_t speed = (gpsSol.groundSpeed * 9 / 25);
uint16_t gpsHeading = 0; uint16_t gpsHeading = 0;
uint16_t altitude = 0; uint16_t altitude = 0;
gpsHeading = GPS_ground_course * 10; gpsHeading = gpsSol.groundCourse * 10;
altitude = alt * 10 + 1000; altitude = alt * 10 + 1000;
bstMasterStartBuffer(PUBLIC_ADDRESS); bstMasterStartBuffer(PUBLIC_ADDRESS);

16
src/main/telemetry/crsf.c
View File

@ -111,7 +111,7 @@ int32_t Latitude ( degree / 10`000`000 )
int32_t Longitude (degree / 10`000`000 ) int32_t Longitude (degree / 10`000`000 )
uint16_t Groundspeed ( km/h / 10 ) uint16_t Groundspeed ( km/h / 10 )
uint16_t GPS heading ( degree / 100 ) uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset ) uint16 Altitude ( meter ­1000m offset )
uint8_t Satellites in use ( counter ) uint8_t Satellites in use ( counter )
*/ */
void crsfFrameGps(sbuf_t *dst) void crsfFrameGps(sbuf_t *dst)
@ -119,14 +119,14 @@ void crsfFrameGps(sbuf_t *dst)
// use sbufWrite since CRC does not include frame length // use sbufWrite since CRC does not include frame length
sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
sbufWriteU8(dst, CRSF_FRAMETYPE_GPS); sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
sbufWriteU32BigEndian(dst, GPS_coord[LON]); sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
sbufWriteU16BigEndian(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s
sbufWriteU16BigEndian(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10 sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10
//Send real GPS altitude only if it's reliable (there's a GPS fix) //Send real GPS altitude only if it's reliable (there's a GPS fix)
const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000; const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt : 0) + 1000;
sbufWriteU16BigEndian(dst, altitude); sbufWriteU16BigEndian(dst, altitude);
sbufWriteU8(dst, GPS_numSat); sbufWriteU8(dst, gpsSol.numSat);
} }
/* /*
@ -204,7 +204,7 @@ void crsfFrameAttitude(sbuf_t *dst)
/* /*
0x21 Flight mode text based 0x21 Flight mode text based
Payload: Payload:
char[] Flight mode ( Null­terminated string ) char[] Flight mode ( Null terminated string )
*/ */
void crsfFrameFlightMode(sbuf_t *dst) void crsfFrameFlightMode(sbuf_t *dst)
{ {

17
src/main/telemetry/frsky.c
View File

@ -183,7 +183,7 @@ static void sendBaro(void)
#ifdef GPS #ifdef GPS
static void sendGpsAltitude(void) static void sendGpsAltitude(void)
{ {
uint16_t altitude = GPS_altitude; uint16_t altitude = gpsSol.llh.alt;
//Send real GPS altitude only if it's reliable (there's a GPS fix) //Send real GPS altitude only if it's reliable (there's a GPS fix)
if (!STATE(GPS_FIX)) { if (!STATE(GPS_FIX)) {
altitude = 0; altitude = 0;
@ -230,9 +230,9 @@ static void sendTemperature1(void)
#ifdef GPS #ifdef GPS
static void sendSatalliteSignalQualityAsTemperature2(void) static void sendSatalliteSignalQualityAsTemperature2(void)
{ {
uint16_t satellite = GPS_numSat; uint16_t satellite = gpsSol.numSat;
if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL); satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
} }
sendDataHead(ID_TEMPRATURE2); sendDataHead(ID_TEMPRATURE2);
@ -254,9 +254,9 @@ static void sendSpeed(void)
//Speed should be sent in knots (GPS speed is in cm/s) //Speed should be sent in knots (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP); sendDataHead(ID_GPS_SPEED_BP);
//convert to knots: 1cm/s = 0.0194384449 knots //convert to knots: 1cm/s = 0.0194384449 knots
serialize16(GPS_speed * 1944 / 100000); serialize16(gpsSol.groundSpeed * 1944 / 100000);
sendDataHead(ID_GPS_SPEED_AP); sendDataHead(ID_GPS_SPEED_AP);
serialize16((GPS_speed * 1944 / 100) % 100); serialize16((gpsSol.groundSpeed * 1944 / 100) % 100);
} }
#endif #endif
@ -337,11 +337,14 @@ static void sendFakeLatLong(void)
static void sendGPSLatLong(void) static void sendGPSLatLong(void)
{ {
static uint8_t gpsFixOccured = 0; static uint8_t gpsFixOccured = 0;
int32_t coord[2] = {0,0};
if (STATE(GPS_FIX) || gpsFixOccured == 1) { if (STATE(GPS_FIX) || gpsFixOccured == 1) {
// If we have ever had a fix, send the last known lat/long // If we have ever had a fix, send the last known lat/long
gpsFixOccured = 1; gpsFixOccured = 1;
sendLatLong(GPS_coord); coord[LAT] = gpsSol.llh.lat;
coord[LON] = gpsSol.llh.lon;
sendLatLong(coord);
} else { } else {
// otherwise send fake lat/long in order to display compass value // otherwise send fake lat/long in order to display compass value
sendFakeLatLong(); sendFakeLatLong();

12
src/main/telemetry/hott.c
View File

@ -185,35 +185,35 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
{ {
hottGPSMessage->gps_satelites = GPS_numSat; hottGPSMessage->gps_satelites = gpsSol.numSat;
if (!STATE(GPS_FIX)) { if (!STATE(GPS_FIX)) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
return; return;
} }
if (GPS_numSat >= 5) { if (gpsSol.numSat >= 5) {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D; hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
} else { } else {
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D; hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D;
} }
addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]); 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) // GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement)
const uint16_t speed = (GPS_speed * 36) / 1000; const uint16_t speed = (gpsSol.groundSpeed * 36) / 1000;
hottGPSMessage->gps_speed_L = speed & 0x00FF; hottGPSMessage->gps_speed_L = speed & 0x00FF;
hottGPSMessage->gps_speed_H = speed >> 8; hottGPSMessage->gps_speed_H = speed >> 8;
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF; hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8; hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
uint16_t altitude = GPS_altitude; uint16_t altitude = gpsSol.llh.alt;
if (!STATE(GPS_FIX)) { if (!STATE(GPS_FIX)) {
altitude = getEstimatedAltitude() / 100; altitude = getEstimatedAltitude() / 100;
} }
const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // gpsSol.llh.alt in m ; offset = 500 -> O m
hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF; hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF;
hottGPSMessage->altitude_H = hottGpsAltitude >> 8; hottGPSMessage->altitude_H = hottGpsAltitude >> 8;

14
src/main/telemetry/ltm.c
View File

@ -133,23 +133,23 @@ static void ltm_gframe(void)
if (!STATE(GPS_FIX)) if (!STATE(GPS_FIX))
gps_fix_type = 1; gps_fix_type = 1;
else if (GPS_numSat < 5) else if (gpsSol.numSat < 5)
gps_fix_type = 2; gps_fix_type = 2;
else else
gps_fix_type = 3; gps_fix_type = 3;
ltm_initialise_packet('G'); ltm_initialise_packet('G');
ltm_serialise_32(GPS_coord[LAT]); ltm_serialise_32(gpsSol.llh.lat);
ltm_serialise_32(GPS_coord[LON]); ltm_serialise_32(gpsSol.llh.lon);
ltm_serialise_8((uint8_t)(GPS_speed / 100)); ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
#if defined(BARO) || defined(SONAR) #if defined(BARO) || defined(SONAR)
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : GPS_altitude * 100; ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
#else #else
ltm_alt = GPS_altitude * 100; ltm_alt = gpsSol.llh.alt * 100;
#endif #endif
ltm_serialise_32(ltm_alt); ltm_serialise_32(ltm_alt);
ltm_serialise_8((GPS_numSat << 2) | gps_fix_type); ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type);
ltm_finalise(); ltm_finalise();
#endif #endif
} }

30
src/main/telemetry/mavlink.c
View File

@ -286,7 +286,7 @@ void mavlinkSendPosition(void)
gpsFixType = 1; gpsFixType = 1;
} }
else { else {
if (GPS_numSat < 5) { if (gpsSol.numSat < 5) {
gpsFixType = 2; gpsFixType = 2;
} }
else { else {
@ -300,21 +300,21 @@ void mavlinkSendPosition(void)
// 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. // 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, gpsFixType,
// lat Latitude in 1E7 degrees // lat Latitude in 1E7 degrees
GPS_coord[LAT], gpsSol.llh.lat,
// lon Longitude in 1E7 degrees // lon Longitude in 1E7 degrees
GPS_coord[LON], gpsSol.llh.lon,
// alt Altitude in 1E3 meters (millimeters) above MSL // alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 1000, gpsSol.llh.alt * 1000,
// eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535, 65535,
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
65535, 65535,
// vel GPS ground speed (m/s * 100). If unknown, set to: 65535 // vel GPS ground speed (m/s * 100). If unknown, set to: 65535
GPS_speed, 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 // 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, gpsSol.groundCourse * 10,
// satellites_visible Number of satellites visible. If unknown, set to 255 // satellites_visible Number of satellites visible. If unknown, set to 255
GPS_numSat); gpsSol.numSat);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
mavlinkSerialWrite(mavBuffer, msgLength); mavlinkSerialWrite(mavBuffer, msgLength);
@ -323,16 +323,16 @@ void mavlinkSendPosition(void)
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
micros(), micros(),
// lat Latitude in 1E7 degrees // lat Latitude in 1E7 degrees
GPS_coord[LAT], gpsSol.llh.lat,
// lon Longitude in 1E7 degrees // lon Longitude in 1E7 degrees
GPS_coord[LON], gpsSol.llh.lon,
// alt Altitude in 1E3 meters (millimeters) above MSL // alt Altitude in 1E3 meters (millimeters) above MSL
GPS_altitude * 1000, gpsSol.llh.alt * 1000,
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
#if defined(BARO) || defined(SONAR) #if defined(BARO) || defined(SONAR)
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : GPS_altitude * 1000, (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000,
#else #else
GPS_altitude * 1000, gpsSol.llh.alt * 1000,
#endif #endif
// Ground X Speed (Latitude), expressed as m/s * 100 // Ground X Speed (Latitude), expressed as m/s * 100
0, 0,
@ -391,7 +391,7 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(GPS) #if defined(GPS)
// use ground speed if source available // use ground speed if source available
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
mavGroundSpeed = GPS_speed / 100.0f; mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
} }
#endif #endif
@ -404,13 +404,13 @@ void mavlinkSendHUDAndHeartbeat(void)
#if defined(GPS) #if defined(GPS)
else if (sensors(SENSOR_GPS)) { else if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS // No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude; mavAltitude = gpsSol.llh.alt;
} }
#endif #endif
#elif defined(GPS) #elif defined(GPS)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
// No sonar or baro, just display altitude above MLS // No sonar or baro, just display altitude above MLS
mavAltitude = GPS_altitude; mavAltitude = gpsSol.llh.alt;
} }
#endif #endif

14
src/main/telemetry/smartport.c
View File

@ -602,7 +602,7 @@ void handleSmartPortTelemetry(void)
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
//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 = GPS_speed * 1944 / 100; uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
smartPortSendPackage(id, tmpui); smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
@ -649,14 +649,14 @@ void handleSmartPortTelemetry(void)
// the MSB of the sent uint32_t helps FrSky keep track // the MSB of the sent uint32_t helps FrSky keep track
// the even/odd bit of our counter helps us keep track // the even/odd bit of our counter helps us keep track
if (smartPortIdCnt & 1) { if (smartPortIdCnt & 1) {
tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare 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 tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LON] < 0) tmpui |= 0x40000000; if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
} }
else { else {
tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare 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 tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast
if (GPS_coord[LAT] < 0) tmpui |= 0x40000000; if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
} }
smartPortSendPackage(id, tmpui); smartPortSendPackage(id, tmpui);
smartPortHasRequest = 0; smartPortHasRequest = 0;
@ -735,7 +735,7 @@ void handleSmartPortTelemetry(void)
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
#ifdef GPS #ifdef GPS
// provide GPS lock status // provide GPS lock status
smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat);
smartPortHasRequest = 0; smartPortHasRequest = 0;
#endif #endif
} else if (feature(FEATURE_GPS)) { } else if (feature(FEATURE_GPS)) {
@ -776,7 +776,7 @@ void handleSmartPortTelemetry(void)
#ifdef GPS #ifdef GPS
case FSSP_DATAID_GPS_ALT : case FSSP_DATAID_GPS_ALT :
if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7)
smartPortHasRequest = 0; smartPortHasRequest = 0;
} }
break; break;

24
src/test/unit/flight_imu_unittest.cc
View File

@ -35,22 +35,24 @@ extern "C" {
#include "drivers/compass/compass.h" #include "drivers/compass/compass.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "sensors/sensors.h"
#include "sensors/gyro.h"
#include "sensors/compass.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "fc/runtime_config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "rx/rx.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "sensors/acceleration.h"
#include "sensors/barometer.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/sensors.h"
void imuComputeRotationMatrix(void); void imuComputeRotationMatrix(void);
void imuUpdateEulerAngles(void); void imuUpdateEulerAngles(void);
@ -206,9 +208,7 @@ gyro_t gyro;
acc_t acc; acc_t acc;
mag_t mag; mag_t mag;
uint8_t GPS_numSat; gpsSolutionData_t gpsSol;
uint16_t GPS_speed;
uint16_t GPS_ground_course;
uint8_t debugMode; uint8_t debugMode;
int16_t debug[DEBUG16_VALUE_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT];

19
src/test/unit/ledstrip_unittest.cc
View File

@ -24,27 +24,28 @@
extern "C" { extern "C" {
#include "build/build_config.h" #include "build/build_config.h"
#include "common/color.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/color.h"
#include "config/parameter_group_ids.h" #include "config/parameter_group_ids.h"
#include "sensors/battery.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "rx/rx.h" #include "drivers/light_ws2811strip.h"
#include "drivers/timer.h"
#include "drivers/timer_def.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "drivers/light_ws2811strip.h" #include "io/gps.h"
#include "drivers/timer.h"
#include "drivers/timer_def.h"
#include "io/ledstrip.h" #include "io/ledstrip.h"
#include "rx/rx.h"
#include "sensors/battery.h"
#include "target.h" #include "target.h"
} }
@ -298,7 +299,7 @@ float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t rcModeActivationMask; uint32_t rcModeActivationMask;
uint16_t rssi = 0; uint16_t rssi = 0;
uint8_t GPS_numSat = 0; gpsSolutionData_t gpsSol;
batteryState_e getBatteryState(void) { batteryState_e getBatteryState(void) {
return BATTERY_OK; return BATTERY_OK;

24
src/test/unit/telemetry_crsf_unittest.cc
View File

@ -82,12 +82,7 @@ uint16_t Groundspeed ( km/h / 10 )
uint16_t GPS heading ( degree / 100 ) uint16_t GPS heading ( degree / 100 )
uint16 Altitude ( meter ­ 1000m offset ) uint16 Altitude ( meter ­ 1000m offset )
uint8_t Satellites in use ( counter ) uint8_t Satellites in use ( counter )
uint8_t GPS_numSat;
int32_t GPS_coord[2];
uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in m
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
*/ */
#define FRAME_HEADER_FOOTER_LEN 4 #define FRAME_HEADER_FOOTER_LEN 4
@ -114,13 +109,13 @@ TEST(TelemetryCrsfTest, TestGPS)
EXPECT_EQ(0, satelliteCount); EXPECT_EQ(0, satelliteCount);
EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]); EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]);
GPS_coord[LAT] = 56 * GPS_DEGREES_DIVIDER; gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER;
GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER; gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
ENABLE_STATE(GPS_FIX); ENABLE_STATE(GPS_FIX);
GPS_altitude = 2345; // altitude in m gpsSol.llh.alt = 2345; // altitude in m
GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
GPS_numSat = 9; gpsSol.numSat = 9;
GPS_ground_course = 1479; // degrees * 10 gpsSol.groundCourse = 1479; // degrees * 10
frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6]; lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6];
EXPECT_EQ(560000000, lattitude); EXPECT_EQ(560000000, lattitude);
@ -279,12 +274,8 @@ uint8_t useHottAlarmSoundPeriod (void) { return 0; }
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
uint8_t GPS_numSat;
int32_t GPS_coord[2];
uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in m gpsSolutionData_t gpsSol;
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_ground_course = 0; // degrees * 10
void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
@ -327,4 +318,3 @@ uint8_t calculateBatteryPercentageRemaining(void) {
} }
} }

5
src/test/unit/telemetry_hott_unittest.cc
View File

@ -171,11 +171,8 @@ uint16_t batteryWarningVoltage;
uint8_t useHottAlarmSoundPeriod (void) { return 0; } uint8_t useHottAlarmSoundPeriod (void) { return 0; }
uint8_t GPS_numSat; gpsSolutionData_t gpsSol;
int32_t GPS_coord[2];
uint16_t GPS_speed; // speed in 0.1m/s
uint16_t GPS_distanceToHome; // distance to home point in meters uint16_t GPS_distanceToHome; // distance to home point in meters
uint16_t GPS_altitude; // altitude in 0.1m
int16_t GPS_directionToHome; // direction to home or hol point in degrees int16_t GPS_directionToHome; // direction to home or hol point in degrees