Move some GPS variables into struct. Some alignment with iNav
This commit is contained in:
parent
582af3d515
commit
564e0c94b8
|
@ -992,16 +992,16 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
|
|||
blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time);
|
||||
}
|
||||
|
||||
blackboxWriteUnsignedVB(GPS_numSat);
|
||||
blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]);
|
||||
blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]);
|
||||
blackboxWriteUnsignedVB(GPS_altitude);
|
||||
blackboxWriteUnsignedVB(GPS_speed);
|
||||
blackboxWriteUnsignedVB(GPS_ground_course);
|
||||
blackboxWriteUnsignedVB(gpsSol.numSat);
|
||||
blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]);
|
||||
blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]);
|
||||
blackboxWriteUnsignedVB(gpsSol.llh.alt);
|
||||
blackboxWriteUnsignedVB(gpsSol.groundSpeed);
|
||||
blackboxWriteUnsignedVB(gpsSol.groundCourse);
|
||||
|
||||
gpsHistory.GPS_numSat = GPS_numSat;
|
||||
gpsHistory.GPS_coord[0] = GPS_coord[0];
|
||||
gpsHistory.GPS_coord[1] = GPS_coord[1];
|
||||
gpsHistory.GPS_numSat = gpsSol.numSat;
|
||||
gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat;
|
||||
gpsHistory.GPS_coord[LON] = gpsSol.llh.lon;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -1474,8 +1474,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs)
|
|||
if (blackboxShouldLogGpsHomeFrame()) {
|
||||
writeGPSHomeFrame();
|
||||
writeGPSFrame(currentTimeUs);
|
||||
} else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0]
|
||||
|| GPS_coord[1] != gpsHistory.GPS_coord[1]) {
|
||||
} else if (gpsSol.numSat != gpsHistory.GPS_numSat
|
||||
|| 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
|
||||
writeGPSFrame(currentTimeUs);
|
||||
}
|
||||
|
|
|
@ -231,7 +231,7 @@ void mwArm(void)
|
|||
|
||||
//beep to indicate arming
|
||||
#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);
|
||||
else
|
||||
beeper(BEEPER_ARMING);
|
||||
|
|
|
@ -1113,12 +1113,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
|
|||
|
||||
case MSP_RAW_GPS:
|
||||
sbufWriteU8(dst, STATE(GPS_FIX));
|
||||
sbufWriteU8(dst, GPS_numSat);
|
||||
sbufWriteU32(dst, GPS_coord[LAT]);
|
||||
sbufWriteU32(dst, GPS_coord[LON]);
|
||||
sbufWriteU16(dst, GPS_altitude);
|
||||
sbufWriteU16(dst, GPS_speed);
|
||||
sbufWriteU16(dst, GPS_ground_course);
|
||||
sbufWriteU8(dst, gpsSol.numSat);
|
||||
sbufWriteU32(dst, gpsSol.llh.lat);
|
||||
sbufWriteU32(dst, gpsSol.llh.lon);
|
||||
sbufWriteU16(dst, gpsSol.llh.alt);
|
||||
sbufWriteU16(dst, gpsSol.groundSpeed);
|
||||
sbufWriteU16(dst, gpsSol.groundCourse);
|
||||
break;
|
||||
|
||||
case MSP_COMP_GPS:
|
||||
|
@ -1838,11 +1838,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
|
|||
} else {
|
||||
DISABLE_STATE(GPS_FIX);
|
||||
}
|
||||
GPS_numSat = sbufReadU8(src);
|
||||
GPS_coord[LAT] = sbufReadU32(src);
|
||||
GPS_coord[LON] = sbufReadU32(src);
|
||||
GPS_altitude = sbufReadU16(src);
|
||||
GPS_speed = sbufReadU16(src);
|
||||
gpsSol.numSat = sbufReadU8(src);
|
||||
gpsSol.llh.lat = sbufReadU32(src);
|
||||
gpsSol.llh.lon = sbufReadU32(src);
|
||||
gpsSol.llh.alt = sbufReadU16(src);
|
||||
gpsSol.groundSpeed = sbufReadU16(src);
|
||||
GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers
|
||||
break;
|
||||
|
||||
|
|
|
@ -419,9 +419,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
|||
useMag = true;
|
||||
}
|
||||
#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
|
||||
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
|
||||
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
|
||||
useYaw = true;
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -254,7 +254,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(&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_directionToHome = dir / 100;
|
||||
} else {
|
||||
|
@ -270,7 +270,7 @@ void onGpsNewData(void)
|
|||
uint16_t speed;
|
||||
|
||||
|
||||
if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) {
|
||||
if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -284,7 +284,7 @@ void onGpsNewData(void)
|
|||
#if defined(GPS_FILTERING)
|
||||
GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH;
|
||||
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
|
||||
|
||||
// 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_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 (fraction3[axis] > 1 && fraction3[axis] < 999)
|
||||
GPS_coord[axis] = GPS_filtered[axis];
|
||||
if (fraction3[axis] > 1 && fraction3[axis] < 999) {
|
||||
if (axis == LAT) {
|
||||
gpsSol.llh.lat = GPS_filtered[LAT];
|
||||
} else {
|
||||
gpsSol.llh.lon = GPS_filtered[LON];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -320,8 +325,8 @@ void onGpsNewData(void)
|
|||
// we are navigating
|
||||
|
||||
// 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_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]);
|
||||
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], &gpsSol.llh.lat, &gpsSol.llh.lon);
|
||||
|
||||
switch (nav_mode) {
|
||||
case NAV_MODE_POSHOLD:
|
||||
|
@ -359,10 +364,10 @@ void onGpsNewData(void)
|
|||
|
||||
void GPS_reset_home_position(void)
|
||||
{
|
||||
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
|
||||
GPS_home[LAT] = GPS_coord[LAT];
|
||||
GPS_home[LON] = GPS_coord[LON];
|
||||
GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc
|
||||
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
|
||||
GPS_home[LAT] = gpsSol.llh.lat;
|
||||
GPS_home[LON] = gpsSol.llh.lon;
|
||||
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
|
||||
// Set ground altitude
|
||||
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_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;
|
||||
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;
|
||||
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 int32_t last_coord[2] = { 0, 0 };
|
||||
static uint8_t init = 0;
|
||||
// y_GPS_speed positive = Up
|
||||
// x_GPS_speed positive = Right
|
||||
|
||||
if (init) {
|
||||
float tmp = 1.0f / dTnav;
|
||||
actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp;
|
||||
actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * tmp;
|
||||
actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * 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_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2;
|
||||
|
@ -500,8 +503,8 @@ static void GPS_calc_velocity(void)
|
|||
}
|
||||
init = 1;
|
||||
|
||||
last_coord[LON] = GPS_coord[LON];
|
||||
last_coord[LAT] = GPS_coord[LAT];
|
||||
last_coord[LON] = gpsSol.llh.lon;
|
||||
last_coord[LAT] = gpsSol.llh.lat;
|
||||
}
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////
|
||||
|
@ -666,7 +669,7 @@ void updateGpsWaypointsAndMode(void)
|
|||
bool resetNavNow = false;
|
||||
static bool gpsReadyBeepDone = false;
|
||||
|
||||
if (STATE(GPS_FIX) && GPS_numSat >= 5) {
|
||||
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
|
||||
|
||||
//
|
||||
// process HOME mode
|
||||
|
@ -701,8 +704,8 @@ void updateGpsWaypointsAndMode(void)
|
|||
|
||||
// Transition to HOLD mode
|
||||
ENABLE_FLIGHT_MODE(GPS_HOLD_MODE);
|
||||
GPS_hold[LAT] = GPS_coord[LAT];
|
||||
GPS_hold[LON] = GPS_coord[LON];
|
||||
GPS_hold[LAT] = gpsSol.llh.lat;
|
||||
GPS_hold[LON] = gpsSol.llh.lon;
|
||||
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
|
||||
nav_mode = NAV_MODE_POSHOLD;
|
||||
resetNavNow = true;
|
||||
|
|
|
@ -295,12 +295,12 @@ void beeperConfirmationBeeps(uint8_t beepCount)
|
|||
void beeperGpsStatus(void)
|
||||
{
|
||||
// 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;
|
||||
do {
|
||||
beep_multiBeeps[i++] = 5;
|
||||
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] = BEEPER_COMMAND_STOP;
|
||||
|
|
|
@ -365,22 +365,22 @@ void showGpsPage() {
|
|||
|
||||
|
||||
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();
|
||||
i2c_OLED_set_line(rowIndex++);
|
||||
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();
|
||||
i2c_OLED_set_line(rowIndex++);
|
||||
i2c_OLED_send_string(lineBuffer);
|
||||
|
||||
tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed);
|
||||
tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed);
|
||||
padHalfLineBuffer();
|
||||
i2c_OLED_set_line(rowIndex);
|
||||
i2c_OLED_send_string(lineBuffer);
|
||||
|
||||
tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course);
|
||||
tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse);
|
||||
padHalfLineBuffer();
|
||||
i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++);
|
||||
i2c_OLED_send_string(lineBuffer);
|
||||
|
|
|
@ -71,18 +71,11 @@ static char *gpsPacketLogChar = gpsPacketLog;
|
|||
// **********************
|
||||
// GPS
|
||||
// **********************
|
||||
int32_t GPS_coord[2]; // LAT/LON
|
||||
|
||||
uint8_t GPS_numSat;
|
||||
uint16_t GPS_hdop = 9999; // Compute GPS quality signal
|
||||
gpsSolutionData_t gpsSol;
|
||||
uint32_t GPS_packetCount = 0;
|
||||
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
|
||||
|
||||
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_svinfo_chn[GPS_SV_MAXSATS]; // Channel number
|
||||
uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID
|
||||
|
@ -424,7 +417,7 @@ void gpsInitHardware(void)
|
|||
static void updateGpsIndicator(timeUs_t currentTimeUs)
|
||||
{
|
||||
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;
|
||||
LED1_TOGGLE;
|
||||
}
|
||||
|
@ -456,8 +449,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
|
|||
gpsData.baudrateIndex %= GPS_INIT_ENTRIES;
|
||||
}
|
||||
gpsData.lastMessage = millis();
|
||||
// TODO - move some / all of these into gpsData
|
||||
GPS_numSat = 0;
|
||||
gpsSol.numSat = 0;
|
||||
DISABLE_STATE(GPS_FIX);
|
||||
gpsSetState(GPS_INITIALIZING);
|
||||
break;
|
||||
|
@ -746,16 +738,16 @@ static bool gpsNewFrameNMEA(char c)
|
|||
*gpsPacketLogChar = LOG_NMEA_GGA;
|
||||
frameOK = 1;
|
||||
if (STATE(GPS_FIX)) {
|
||||
GPS_coord[LAT] = gps_Msg.latitude;
|
||||
GPS_coord[LON] = gps_Msg.longitude;
|
||||
GPS_numSat = gps_Msg.numSat;
|
||||
GPS_altitude = gps_Msg.altitude;
|
||||
gpsSol.llh.lat = gps_Msg.latitude;
|
||||
gpsSol.llh.lon = gps_Msg.longitude;
|
||||
gpsSol.numSat = gps_Msg.numSat;
|
||||
gpsSol.llh.alt = gps_Msg.altitude;
|
||||
}
|
||||
break;
|
||||
case FRAME_RMC:
|
||||
*gpsPacketLogChar = LOG_NMEA_RMC;
|
||||
GPS_speed = gps_Msg.speed;
|
||||
GPS_ground_course = gps_Msg.ground_course;
|
||||
gpsSol.groundSpeed = gps_Msg.speed;
|
||||
gpsSol.groundCourse = gps_Msg.ground_course;
|
||||
break;
|
||||
} // end switch
|
||||
} else {
|
||||
|
@ -952,9 +944,9 @@ static bool UBLOX_parse_gps(void)
|
|||
case MSG_POSLLH:
|
||||
*gpsPacketLogChar = LOG_UBLOX_POSLLH;
|
||||
//i2c_dataset.time = _buffer.posllh.time;
|
||||
GPS_coord[LON] = _buffer.posllh.longitude;
|
||||
GPS_coord[LAT] = _buffer.posllh.latitude;
|
||||
GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
|
||||
gpsSol.llh.lon = _buffer.posllh.longitude;
|
||||
gpsSol.llh.lat = _buffer.posllh.latitude;
|
||||
gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100; //alt in m
|
||||
if (next_fix) {
|
||||
ENABLE_STATE(GPS_FIX);
|
||||
} 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);
|
||||
if (!next_fix)
|
||||
DISABLE_STATE(GPS_FIX);
|
||||
GPS_numSat = _buffer.solution.satellites;
|
||||
GPS_hdop = _buffer.solution.position_DOP;
|
||||
gpsSol.numSat = _buffer.solution.satellites;
|
||||
gpsSol.hdop = _buffer.solution.position_DOP;
|
||||
break;
|
||||
case MSG_VELNED:
|
||||
*gpsPacketLogChar = LOG_UBLOX_VELNED;
|
||||
// speed_3d = _buffer.velned.speed_3d; // cm/s
|
||||
GPS_speed = _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.groundSpeed = _buffer.velned.speed_2d; // cm/s
|
||||
gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10
|
||||
_new_speed = true;
|
||||
break;
|
||||
case MSG_SVINFO:
|
||||
|
|
|
@ -31,8 +31,6 @@ typedef enum {
|
|||
GPS_UBLOX
|
||||
} gpsProvider_e;
|
||||
|
||||
#define GPS_PROVIDER_MAX GPS_UBLOX
|
||||
|
||||
typedef enum {
|
||||
SBAS_AUTO = 0,
|
||||
SBAS_EGNOS,
|
||||
|
@ -77,6 +75,21 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
|
|||
int16_t mmmm;
|
||||
} 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 {
|
||||
GPS_MESSAGE_STATE_IDLE = 0,
|
||||
|
@ -102,16 +115,11 @@ typedef struct gpsData_s {
|
|||
extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT];
|
||||
|
||||
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 uint32_t GPS_packetCount;
|
||||
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_svinfo_chn[16]; // Channel number
|
||||
extern uint8_t GPS_svinfo_svid[16]; // Satellite ID
|
||||
|
|
|
@ -731,7 +731,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
|
|||
static uint8_t gpsFlashCounter = 0;
|
||||
if (gpsPauseCounter > 0) {
|
||||
gpsPauseCounter--;
|
||||
} else if (gpsFlashCounter >= GPS_numSat) {
|
||||
} else if (gpsFlashCounter >= gpsSol.numSat) {
|
||||
gpsFlashCounter = 0;
|
||||
gpsPauseCounter = blinkPauseLength;
|
||||
} else {
|
||||
|
@ -743,7 +743,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer)
|
|||
|
||||
const hsvColor_t *gpsColor;
|
||||
|
||||
if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) {
|
||||
if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) {
|
||||
gpsColor = getSC(LED_SCOLOR_GPSNOSATS);
|
||||
} else {
|
||||
bool colorOn = gpsPauseCounter == 0; // each interval starts with pause
|
||||
|
|
|
@ -277,12 +277,12 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
#ifdef GPS
|
||||
case OSD_GPS_SATS:
|
||||
buff[0] = 0x1f;
|
||||
tfp_sprintf(buff + 1, "%d", GPS_numSat);
|
||||
tfp_sprintf(buff + 1, "%d", gpsSol.numSat);
|
||||
break;
|
||||
|
||||
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.
|
||||
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(GPS_speed));
|
||||
tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed));
|
||||
break;
|
||||
|
||||
case OSD_GPS_LAT:
|
||||
|
@ -291,10 +291,10 @@ static void osdDrawSingleElement(uint8_t item)
|
|||
int32_t val;
|
||||
if (item == OSD_GPS_LAT) {
|
||||
buff[0] = SYM_ARROW_EAST;
|
||||
val = GPS_coord[LAT];
|
||||
val = gpsSol.llh.lat;
|
||||
} else {
|
||||
buff[0] = SYM_ARROW_SOUTH;
|
||||
val = GPS_coord[LON];
|
||||
val = gpsSol.llh.lon;
|
||||
}
|
||||
|
||||
char wholeDegreeString[5];
|
||||
|
@ -880,7 +880,7 @@ static void osdUpdateStats(void)
|
|||
{
|
||||
int16_t value = 0;
|
||||
#ifdef GPS
|
||||
value = CM_S_TO_KM_H(GPS_speed);
|
||||
value = CM_S_TO_KM_H(gpsSol.groundSpeed);
|
||||
#endif
|
||||
if (stats.max_speed < value)
|
||||
stats.max_speed = value;
|
||||
|
|
|
@ -778,15 +778,15 @@ static uint8_t numOfSat = 0;
|
|||
#ifdef GPS
|
||||
bool writeGpsPositionPrameToBST(void)
|
||||
{
|
||||
if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) {
|
||||
lat = GPS_coord[LAT];
|
||||
lon = GPS_coord[LON];
|
||||
alt = GPS_altitude;
|
||||
numOfSat = GPS_numSat;
|
||||
uint16_t speed = (GPS_speed * 9 / 25);
|
||||
if((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) {
|
||||
lat = gpsSol.llh.lat;
|
||||
lon = gpsSol.llh.lon;
|
||||
alt = gpsSol.llh.alt;
|
||||
numOfSat = gpsSol.numSat;
|
||||
uint16_t speed = (gpsSol.groundSpeed * 9 / 25);
|
||||
uint16_t gpsHeading = 0;
|
||||
uint16_t altitude = 0;
|
||||
gpsHeading = GPS_ground_course * 10;
|
||||
gpsHeading = gpsSol.groundCourse * 10;
|
||||
altitude = alt * 10 + 1000;
|
||||
|
||||
bstMasterStartBuffer(PUBLIC_ADDRESS);
|
||||
|
|
|
@ -119,14 +119,14 @@ void crsfFrameGps(sbuf_t *dst)
|
|||
// use sbufWrite since CRC does not include frame length
|
||||
sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC);
|
||||
sbufWriteU8(dst, CRSF_FRAMETYPE_GPS);
|
||||
sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees
|
||||
sbufWriteU32BigEndian(dst, GPS_coord[LON]);
|
||||
sbufWriteU16BigEndian(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s
|
||||
sbufWriteU16BigEndian(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10
|
||||
sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees
|
||||
sbufWriteU32BigEndian(dst, gpsSol.llh.lon);
|
||||
sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s
|
||||
sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10
|
||||
//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);
|
||||
sbufWriteU8(dst, GPS_numSat);
|
||||
sbufWriteU8(dst, gpsSol.numSat);
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -204,7 +204,7 @@ void crsfFrameAttitude(sbuf_t *dst)
|
|||
/*
|
||||
0x21 Flight mode text based
|
||||
Payload:
|
||||
char[] Flight mode ( Nullterminated string )
|
||||
char[] Flight mode ( Null terminated string )
|
||||
*/
|
||||
void crsfFrameFlightMode(sbuf_t *dst)
|
||||
{
|
||||
|
|
|
@ -183,7 +183,7 @@ static void sendBaro(void)
|
|||
#ifdef GPS
|
||||
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)
|
||||
if (!STATE(GPS_FIX)) {
|
||||
altitude = 0;
|
||||
|
@ -230,9 +230,9 @@ static void sendTemperature1(void)
|
|||
#ifdef GPS
|
||||
static void sendSatalliteSignalQualityAsTemperature2(void)
|
||||
{
|
||||
uint16_t satellite = GPS_numSat;
|
||||
if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
|
||||
satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
|
||||
uint16_t satellite = gpsSol.numSat;
|
||||
if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
|
||||
satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL);
|
||||
}
|
||||
sendDataHead(ID_TEMPRATURE2);
|
||||
|
||||
|
@ -254,9 +254,9 @@ static void sendSpeed(void)
|
|||
//Speed should be sent in knots (GPS speed is in cm/s)
|
||||
sendDataHead(ID_GPS_SPEED_BP);
|
||||
//convert to knots: 1cm/s = 0.0194384449 knots
|
||||
serialize16(GPS_speed * 1944 / 100000);
|
||||
serialize16(gpsSol.groundSpeed * 1944 / 100000);
|
||||
sendDataHead(ID_GPS_SPEED_AP);
|
||||
serialize16((GPS_speed * 1944 / 100) % 100);
|
||||
serialize16((gpsSol.groundSpeed * 1944 / 100) % 100);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -337,11 +337,14 @@ static void sendFakeLatLong(void)
|
|||
static void sendGPSLatLong(void)
|
||||
{
|
||||
static uint8_t gpsFixOccured = 0;
|
||||
int32_t coord[2] = {0,0};
|
||||
|
||||
if (STATE(GPS_FIX) || gpsFixOccured == 1) {
|
||||
// If we have ever had a fix, send the last known lat/long
|
||||
gpsFixOccured = 1;
|
||||
sendLatLong(GPS_coord);
|
||||
coord[LAT] = gpsSol.llh.lat;
|
||||
coord[LON] = gpsSol.llh.lon;
|
||||
sendLatLong(coord);
|
||||
} else {
|
||||
// otherwise send fake lat/long in order to display compass value
|
||||
sendFakeLatLong();
|
||||
|
|
|
@ -185,35 +185,35 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t
|
|||
|
||||
void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
|
||||
{
|
||||
hottGPSMessage->gps_satelites = GPS_numSat;
|
||||
hottGPSMessage->gps_satelites = gpsSol.numSat;
|
||||
|
||||
if (!STATE(GPS_FIX)) {
|
||||
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE;
|
||||
return;
|
||||
}
|
||||
|
||||
if (GPS_numSat >= 5) {
|
||||
if (gpsSol.numSat >= 5) {
|
||||
hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D;
|
||||
} else {
|
||||
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)
|
||||
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_H = speed >> 8;
|
||||
|
||||
hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF;
|
||||
hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8;
|
||||
|
||||
uint16_t altitude = GPS_altitude;
|
||||
uint16_t altitude = gpsSol.llh.alt;
|
||||
if (!STATE(GPS_FIX)) {
|
||||
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_H = hottGpsAltitude >> 8;
|
||||
|
|
|
@ -133,23 +133,23 @@ static void ltm_gframe(void)
|
|||
|
||||
if (!STATE(GPS_FIX))
|
||||
gps_fix_type = 1;
|
||||
else if (GPS_numSat < 5)
|
||||
else if (gpsSol.numSat < 5)
|
||||
gps_fix_type = 2;
|
||||
else
|
||||
gps_fix_type = 3;
|
||||
|
||||
ltm_initialise_packet('G');
|
||||
ltm_serialise_32(GPS_coord[LAT]);
|
||||
ltm_serialise_32(GPS_coord[LON]);
|
||||
ltm_serialise_8((uint8_t)(GPS_speed / 100));
|
||||
ltm_serialise_32(gpsSol.llh.lat);
|
||||
ltm_serialise_32(gpsSol.llh.lon);
|
||||
ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
|
||||
|
||||
#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
|
||||
ltm_alt = GPS_altitude * 100;
|
||||
ltm_alt = gpsSol.llh.alt * 100;
|
||||
#endif
|
||||
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();
|
||||
#endif
|
||||
}
|
||||
|
|
|
@ -286,7 +286,7 @@ void mavlinkSendPosition(void)
|
|||
gpsFixType = 1;
|
||||
}
|
||||
else {
|
||||
if (GPS_numSat < 5) {
|
||||
if (gpsSol.numSat < 5) {
|
||||
gpsFixType = 2;
|
||||
}
|
||||
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.
|
||||
gpsFixType,
|
||||
// lat Latitude in 1E7 degrees
|
||||
GPS_coord[LAT],
|
||||
gpsSol.llh.lat,
|
||||
// lon Longitude in 1E7 degrees
|
||||
GPS_coord[LON],
|
||||
gpsSol.llh.lon,
|
||||
// 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
|
||||
65535,
|
||||
// epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
|
||||
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
|
||||
GPS_ground_course * 10,
|
||||
gpsSol.groundCourse * 10,
|
||||
// satellites_visible Number of satellites visible. If unknown, set to 255
|
||||
GPS_numSat);
|
||||
gpsSol.numSat);
|
||||
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);
|
||||
mavlinkSerialWrite(mavBuffer, msgLength);
|
||||
|
||||
|
@ -323,16 +323,16 @@ void mavlinkSendPosition(void)
|
|||
// time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
|
||||
micros(),
|
||||
// lat Latitude in 1E7 degrees
|
||||
GPS_coord[LAT],
|
||||
gpsSol.llh.lat,
|
||||
// lon Longitude in 1E7 degrees
|
||||
GPS_coord[LON],
|
||||
gpsSol.llh.lon,
|
||||
// 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)
|
||||
#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
|
||||
GPS_altitude * 1000,
|
||||
gpsSol.llh.alt * 1000,
|
||||
#endif
|
||||
// Ground X Speed (Latitude), expressed as m/s * 100
|
||||
0,
|
||||
|
@ -391,7 +391,7 @@ void mavlinkSendHUDAndHeartbeat(void)
|
|||
#if defined(GPS)
|
||||
// use ground speed if source available
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
mavGroundSpeed = GPS_speed / 100.0f;
|
||||
mavGroundSpeed = gpsSol.groundSpeed / 100.0f;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -404,13 +404,13 @@ void mavlinkSendHUDAndHeartbeat(void)
|
|||
#if defined(GPS)
|
||||
else if (sensors(SENSOR_GPS)) {
|
||||
// No sonar or baro, just display altitude above MLS
|
||||
mavAltitude = GPS_altitude;
|
||||
mavAltitude = gpsSol.llh.alt;
|
||||
}
|
||||
#endif
|
||||
#elif defined(GPS)
|
||||
if (sensors(SENSOR_GPS)) {
|
||||
// No sonar or baro, just display altitude above MLS
|
||||
mavAltitude = GPS_altitude;
|
||||
mavAltitude = gpsSol.llh.alt;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -602,7 +602,7 @@ void handleSmartPortTelemetry(void)
|
|||
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 = GPS_speed * 1944 / 100;
|
||||
uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100;
|
||||
smartPortSendPackage(id, tmpui);
|
||||
smartPortHasRequest = 0;
|
||||
}
|
||||
|
@ -649,14 +649,14 @@ void handleSmartPortTelemetry(void)
|
|||
// 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(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
|
||||
if (GPS_coord[LON] < 0) tmpui |= 0x40000000;
|
||||
if (gpsSol.llh.lon < 0) tmpui |= 0x40000000;
|
||||
}
|
||||
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
|
||||
if (GPS_coord[LAT] < 0) tmpui |= 0x40000000;
|
||||
if (gpsSol.llh.lat < 0) tmpui |= 0x40000000;
|
||||
}
|
||||
smartPortSendPackage(id, tmpui);
|
||||
smartPortHasRequest = 0;
|
||||
|
@ -735,7 +735,7 @@ void handleSmartPortTelemetry(void)
|
|||
if (sensors(SENSOR_GPS)) {
|
||||
#ifdef GPS
|
||||
// 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;
|
||||
#endif
|
||||
} else if (feature(FEATURE_GPS)) {
|
||||
|
@ -776,7 +776,7 @@ void handleSmartPortTelemetry(void)
|
|||
#ifdef GPS
|
||||
case FSSP_DATAID_GPS_ALT :
|
||||
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;
|
||||
}
|
||||
break;
|
||||
|
|
|
@ -35,22 +35,24 @@ extern "C" {
|
|||
#include "drivers/compass/compass.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_modes.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.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 imuUpdateEulerAngles(void);
|
||||
|
||||
|
@ -206,9 +208,7 @@ gyro_t gyro;
|
|||
acc_t acc;
|
||||
mag_t mag;
|
||||
|
||||
uint8_t GPS_numSat;
|
||||
uint16_t GPS_speed;
|
||||
uint16_t GPS_ground_course;
|
||||
gpsSolutionData_t gpsSol;
|
||||
|
||||
uint8_t debugMode;
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
|
|
|
@ -24,27 +24,28 @@
|
|||
extern "C" {
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/color.h"
|
||||
#include "common/axis.h"
|
||||
#include "common/color.h"
|
||||
|
||||
#include "config/parameter_group_ids.h"
|
||||
|
||||
#include "sensors/battery.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/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "drivers/light_ws2811strip.h"
|
||||
#include "drivers/timer.h"
|
||||
#include "drivers/timer_def.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
#include "io/ledstrip.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "target.h"
|
||||
}
|
||||
|
||||
|
@ -298,7 +299,7 @@ float rcCommand[4];
|
|||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint32_t rcModeActivationMask;
|
||||
uint16_t rssi = 0;
|
||||
uint8_t GPS_numSat = 0;
|
||||
gpsSolutionData_t gpsSol;
|
||||
|
||||
batteryState_e getBatteryState(void) {
|
||||
return BATTERY_OK;
|
||||
|
|
|
@ -82,12 +82,7 @@ uint16_t Groundspeed ( km/h / 10 )
|
|||
uint16_t GPS heading ( degree / 100 )
|
||||
uint16 Altitude ( meter 1000m offset )
|
||||
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_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
|
||||
|
||||
|
@ -114,13 +109,13 @@ TEST(TelemetryCrsfTest, TestGPS)
|
|||
EXPECT_EQ(0, satelliteCount);
|
||||
EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]);
|
||||
|
||||
GPS_coord[LAT] = 56 * GPS_DEGREES_DIVIDER;
|
||||
GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER;
|
||||
gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER;
|
||||
gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER;
|
||||
ENABLE_STATE(GPS_FIX);
|
||||
GPS_altitude = 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
|
||||
GPS_numSat = 9;
|
||||
GPS_ground_course = 1479; // degrees * 10
|
||||
gpsSol.llh.alt = 2345; // altitude in m
|
||||
gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587
|
||||
gpsSol.numSat = 9;
|
||||
gpsSol.groundCourse = 1479; // degrees * 10
|
||||
frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
|
||||
lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6];
|
||||
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
|
||||
|
||||
uint8_t GPS_numSat;
|
||||
int32_t GPS_coord[2];
|
||||
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
|
||||
gpsSolutionData_t gpsSol;
|
||||
|
||||
void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);}
|
||||
|
||||
|
@ -327,4 +318,3 @@ uint8_t calculateBatteryPercentageRemaining(void) {
|
|||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
@ -171,11 +171,8 @@ uint16_t batteryWarningVoltage;
|
|||
uint8_t useHottAlarmSoundPeriod (void) { return 0; }
|
||||
|
||||
|
||||
uint8_t GPS_numSat;
|
||||
int32_t GPS_coord[2];
|
||||
uint16_t GPS_speed; // speed in 0.1m/s
|
||||
gpsSolutionData_t gpsSol;
|
||||
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
|
||||
|
||||
|
||||
|
|
Loading…
Reference in New Issue