mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 03:50:02 +03:00
Add GPS Status Line to CLI Status Output (#12769)
* Add GPS status line to cli status output: connected status, UART + serial port baud + configured baud, configured status. * Fixed unit test link fail. * Really fixed unit test link fail (I hope). * Really fixed unit test link fail (no really this time). * Updated to address code reviews. * Updated to address code reviews. * Updated to address code reviews.
This commit is contained in:
parent
6e05967840
commit
922bc9d593
4 changed files with 54 additions and 0 deletions
|
@ -4731,6 +4731,45 @@ static void cliStatus(const char *cmdName, char *cmdline)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef USE_GPS
|
||||||
|
cliPrint("GPS: ");
|
||||||
|
if (featureIsEnabled(FEATURE_GPS)) {
|
||||||
|
if (gpsIsHealthy()) {
|
||||||
|
cliPrint("connected, ");
|
||||||
|
} else {
|
||||||
|
cliPrint("NOT CONNECTED, ");
|
||||||
|
}
|
||||||
|
if (gpsConfig()->provider == GPS_MSP) {
|
||||||
|
cliPrint("MSP, ");
|
||||||
|
} else {
|
||||||
|
const serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
|
||||||
|
if (!gpsPortConfig) {
|
||||||
|
cliPrint("NO PORT, ");
|
||||||
|
} else {
|
||||||
|
cliPrintf("UART%d %ld (set to ", (gpsPortConfig->identifier + 1), baudRates[getGpsPortActualBaudRateIndex()]);
|
||||||
|
if (gpsConfig()->autoBaud == GPS_AUTOBAUD_ON) {
|
||||||
|
cliPrint("AUTO");
|
||||||
|
} else {
|
||||||
|
cliPrintf("%ld", baudRates[gpsPortConfig->gps_baudrateIndex]);
|
||||||
|
}
|
||||||
|
cliPrint("), ");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (!gpsIsHealthy()) {
|
||||||
|
cliPrint("NOT CONFIGURED");
|
||||||
|
} else {
|
||||||
|
if (gpsConfig()->autoConfig == GPS_AUTOCONFIG_OFF) {
|
||||||
|
cliPrint("auto config OFF");
|
||||||
|
} else {
|
||||||
|
cliPrint("configured");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
cliPrint("NOT ENABLED");
|
||||||
|
}
|
||||||
|
cliPrintLinefeed();
|
||||||
|
#endif // USE_GPS
|
||||||
|
|
||||||
cliPrint("Arming disable flags:");
|
cliPrint("Arming disable flags:");
|
||||||
armingDisableFlags_e flags = getArmingDisableFlags();
|
armingDisableFlags_e flags = getArmingDisableFlags();
|
||||||
while (flags) {
|
while (flags) {
|
||||||
|
|
|
@ -1985,4 +1985,9 @@ float getGpsDataIntervalSeconds(void)
|
||||||
return gpsDataIntervalSeconds;
|
return gpsDataIntervalSeconds;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
baudRate_e getGpsPortActualBaudRateIndex(void)
|
||||||
|
{
|
||||||
|
return lookupBaudRateIndex(serialGetBaudRate(gpsPort));
|
||||||
|
}
|
||||||
|
|
||||||
#endif // USE_GPS
|
#endif // USE_GPS
|
||||||
|
|
|
@ -26,6 +26,8 @@
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/time.h"
|
#include "common/time.h"
|
||||||
|
|
||||||
|
#include "io/serial.h"
|
||||||
|
|
||||||
#include "pg/gps.h"
|
#include "pg/gps.h"
|
||||||
|
|
||||||
#define GPS_DEGREES_DIVIDER 10000000L
|
#define GPS_DEGREES_DIVIDER 10000000L
|
||||||
|
@ -219,3 +221,4 @@ void GPS_calc_longitude_scaling(int32_t lat);
|
||||||
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
|
void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing);
|
||||||
void gpsSetFixState(bool state);
|
void gpsSetFixState(bool state);
|
||||||
float getGpsDataIntervalSeconds(void);
|
float getGpsDataIntervalSeconds(void);
|
||||||
|
baudRate_e getGpsPortActualBaudRateIndex(void);
|
||||||
|
|
|
@ -39,6 +39,7 @@ extern "C" {
|
||||||
#include "flight/pid.h"
|
#include "flight/pid.h"
|
||||||
#include "flight/servos.h"
|
#include "flight/servos.h"
|
||||||
#include "io/beeper.h"
|
#include "io/beeper.h"
|
||||||
|
#include "io/gps.h"
|
||||||
#include "io/ledstrip.h"
|
#include "io/ledstrip.h"
|
||||||
#include "io/serial.h"
|
#include "io/serial.h"
|
||||||
#include "io/vtx.h"
|
#include "io/vtx.h"
|
||||||
|
@ -48,6 +49,7 @@ extern "C" {
|
||||||
#include "pg/pg.h"
|
#include "pg/pg.h"
|
||||||
#include "pg/pg_ids.h"
|
#include "pg/pg_ids.h"
|
||||||
#include "pg/beeper.h"
|
#include "pg/beeper.h"
|
||||||
|
#include "pg/gps.h"
|
||||||
#include "pg/rx.h"
|
#include "pg/rx.h"
|
||||||
#include "rx/rx.h"
|
#include "rx/rx.h"
|
||||||
#include "scheduler/scheduler.h"
|
#include "scheduler/scheduler.h"
|
||||||
|
@ -87,6 +89,7 @@ extern "C" {
|
||||||
PG_REGISTER_ARRAY(rxFailsafeChannelConfig_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs, PG_RX_FAILSAFE_CHANNEL_CONFIG, 0);
|
PG_REGISTER_ARRAY(rxFailsafeChannelConfig_t, MAX_SUPPORTED_RC_CHANNEL_COUNT, rxFailsafeChannelConfigs, PG_RX_FAILSAFE_CHANNEL_CONFIG, 0);
|
||||||
PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0);
|
PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0);
|
||||||
PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
|
PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
|
||||||
|
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
||||||
|
|
||||||
PG_REGISTER_WITH_RESET_FN(int8_t, unitTestData, PG_RESERVED_FOR_TESTING_1, 0);
|
PG_REGISTER_WITH_RESET_FN(int8_t, unitTestData, PG_RESERVED_FOR_TESTING_1, 0);
|
||||||
}
|
}
|
||||||
|
@ -276,6 +279,9 @@ bool resetEEPROM(void) { return true; }
|
||||||
void bufWriterFlush(bufWriter_t *) {}
|
void bufWriterFlush(bufWriter_t *) {}
|
||||||
void mixerResetDisarmedMotors(void) {}
|
void mixerResetDisarmedMotors(void) {}
|
||||||
void gpsEnablePassthrough(struct serialPort_s *) {}
|
void gpsEnablePassthrough(struct serialPort_s *) {}
|
||||||
|
bool gpsIsHealthy(void) { return true; }
|
||||||
|
baudRate_e getGpsPortActualBaudRateIndex(void) { return BAUD_AUTO; }
|
||||||
|
|
||||||
bool parseLedStripConfig(int, const char *){return false; }
|
bool parseLedStripConfig(int, const char *){return false; }
|
||||||
const char rcChannelLetters[] = "AERT12345678abcdefgh";
|
const char rcChannelLetters[] = "AERT12345678abcdefgh";
|
||||||
|
|
||||||
|
@ -293,6 +299,7 @@ serialPortConfig_t *serialFindPortConfigurationMutable(serialPortIdentifier_e) {
|
||||||
baudRate_e lookupBaudRateIndex(uint32_t){return BAUD_9600; }
|
baudRate_e lookupBaudRateIndex(uint32_t){return BAUD_9600; }
|
||||||
serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e){ return NULL; }
|
serialPortUsage_t *findSerialPortUsageByIdentifier(serialPortIdentifier_e){ return NULL; }
|
||||||
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) { return NULL; }
|
serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, void *, uint32_t, portMode_e, portOptions_e) { return NULL; }
|
||||||
|
const serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) { return NULL; }
|
||||||
void serialSetBaudRate(serialPort_t *, uint32_t) {}
|
void serialSetBaudRate(serialPort_t *, uint32_t) {}
|
||||||
void serialSetMode(serialPort_t *, portMode_e) {}
|
void serialSetMode(serialPort_t *, portMode_e) {}
|
||||||
void serialPassthrough(serialPort_t *, serialPort_t *, serialConsumer *, serialConsumer *) {}
|
void serialPassthrough(serialPort_t *, serialPort_t *, serialConsumer *, serialConsumer *) {}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue