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

Clean up pull request #108 from @Airmamaf.

Browse source
This commit is contained in:
Dominic Clifton 2014-10-18 15:52:04 +01:00
parent df5a61b433
commit 1b47d3cbae
7 changed files with 67 additions and 62 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/config/config.c
View file

@ -201,8 +201,8 @@ void resetTelemetryConfig(telemetryConfig_t *telemetryConfig)
telemetryConfig->telemetry_provider = TELEMETRY_PROVIDER_FRSKY; telemetryConfig->telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
telemetryConfig->frsky_inversion = SERIAL_NOT_INVERTED; telemetryConfig->frsky_inversion = SERIAL_NOT_INVERTED;
telemetryConfig->telemetry_switch = 0; telemetryConfig->telemetry_switch = 0;
telemetryConfig->gpsNoFixLat = 0; telemetryConfig->gpsNoFixLatitude = 0;
telemetryConfig->gpsNoFixLon = 0; telemetryConfig->gpsNoFixLongitude = 0;
telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS; telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS;
telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS; telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS;
telemetryConfig->batterySize = 0; telemetryConfig->batterySize = 0;

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

@ -255,8 +255,8 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) { if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
if (gpsData.state_position < sizeof(ubloxInit)) { if (gpsData.state_position < sizeof(ubloxInit)) {
//Either use specific config file for GPS or let dynamically upload config //Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) { if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
serialWrite(gpsPort, ubloxInit[gpsData.state_position]); serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
} }
gpsData.state_position++; gpsData.state_position++;
@ -268,9 +268,9 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) { if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) { if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
//Either use specific config file for GPS or let dynamically upload config //Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) { if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]); serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
} }
gpsData.state_position++; gpsData.state_position++;
} else { } else {

4
src/main/io/gps.h
View file

@ -20,6 +20,8 @@
#define LAT 0 #define LAT 0
#define LON 1 #define LON 1
#define GPS_DEGREES_DIVIDER 10000000L
typedef enum { typedef enum {
GPS_NMEA = 0, GPS_NMEA = 0,
GPS_UBLOX GPS_UBLOX
@ -54,7 +56,7 @@ typedef enum {
typedef struct gpsConfig_s { typedef struct gpsConfig_s {
gpsProvider_e provider; gpsProvider_e provider;
sbasMode_e sbasMode; sbasMode_e sbasMode;
gpsAutoConfig_e gpsAutoConfig; gpsAutoConfig_e gpsAutoConfig;
} gpsConfig_t; } gpsConfig_t;
typedef enum { typedef enum {

4
src/main/io/serial_cli.c
View file

@ -265,8 +265,8 @@ const clivalue_t valueTable[] = {
{ "telemetry_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, { "telemetry_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
{ "telemetry_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_switch, 0, 1 }, { "telemetry_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_switch, 0, 1 },
{ "frsky_inversion", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_inversion, 0, 1 }, { "frsky_inversion", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_inversion, 0, 1 },
{ "frsky_default_lat", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLat, -90.0, 90.0 }, { "frsky_default_lattitude", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLatitude, -90.0, 90.0 },
{ "frsky_default_lon", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLon, -180.0, 180.0 }, { "frsky_default_longitude", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLongitude, -180.0, 180.0 },
{ "frsky_coordinates_format", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_coordinate_format, 0, FRSKY_FORMAT_NMEA }, { "frsky_coordinates_format", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_coordinate_format, 0, FRSKY_FORMAT_NMEA },
{ "frsky_unit", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_unit, 0, FRSKY_UNIT_IMPERIALS }, { "frsky_unit", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.frsky_unit, 0, FRSKY_UNIT_IMPERIALS },
{ "frsky_battery_size", VAR_UINT16 | MASTER_VALUE, &masterConfig.telemetryConfig.batterySize, 0, 20000 }, { "frsky_battery_size", VAR_UINT16 | MASTER_VALUE, &masterConfig.telemetryConfig.batterySize, 0, 20000 },

85
src/main/telemetry/frsky.c
View file

@ -105,7 +105,7 @@ extern int16_t telemTemperature1; // FIXME dependency on mw.c
#define GPS_BAD_QUALITY 300 #define GPS_BAD_QUALITY 300
#define GPS_MAX_HDOP_VAL 9999 #define GPS_MAX_HDOP_VAL 9999
#define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s #define DELAY_FOR_BARO_INITIALISATION (5 * 1000) //5s
#define BLADE_NUMBER_DIVIDER 5 //Should set 12 blades in Taranis #define BLADE_NUMBER_DIVIDER 5 // should set 12 blades in Taranis
static uint32_t lastCycleTime = 0; static uint32_t lastCycleTime = 0;
static uint8_t cycleNum = 0; static uint8_t cycleNum = 0;
@ -164,7 +164,7 @@ static void sendGpsAltitude(void)
{ {
uint16_t altitude = GPS_altitude; uint16_t altitude = GPS_altitude;
//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;
} }
sendDataHead(ID_GPS_ALTIDUTE_BP); sendDataHead(ID_GPS_ALTIDUTE_BP);
@ -174,36 +174,36 @@ static void sendGpsAltitude(void)
} }
static void sendRpm(void) static void sendThrottleOrBatterySizeAsRpm(void)
{ {
sendDataHead(ID_RPM); sendDataHead(ID_RPM);
if( ARMING_FLAG(ARMED) ) { if (ARMING_FLAG(ARMED)) {
serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER); serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER);
} else { } else {
serialize16((telemetryConfig->batterySize / BLADE_NUMBER_DIVIDER)); serialize16((telemetryConfig->batterySize / BLADE_NUMBER_DIVIDER));
} }
} }
static void sendTemperature1(void) static void sendTemperature1(void)
{ {
sendDataHead(ID_TEMPRATURE1); sendDataHead(ID_TEMPRATURE1);
#ifdef BARO #ifdef BARO
serialize16((baroTemperature + 50)/ 100); //Airmamaf serialize16((baroTemperature + 50)/ 100); //Airmamaf
#else #else
serialize16(telemTemperature1 / 10); serialize16(telemTemperature1 / 10);
#endif #endif
} }
static void sendTemperature2(void) static void sendSatalliteSignalQualityAsTemperature2(void)
{ {
uint16_t satellite = GPS_numSat; uint16_t satellite = GPS_numSat;
//Send instead Num sat and quality GPS if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s
if( GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8) ) {//Every 1s
satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL); satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL);
} }
sendDataHead(ID_TEMPRATURE2); sendDataHead(ID_TEMPRATURE2);
if( telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS ) { if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
serialize16(satellite); serialize16(satellite);
} else { } else {
float tmp = (satellite - 32) / 1.8; float tmp = (satellite - 32) / 1.8;
@ -212,16 +212,19 @@ static void sendTemperature2(void)
serialize16(tmp); serialize16(tmp);
} }
} }
static void sendSpeed(void) static void sendSpeed(void)
{ {
if( STATE(GPS_FIX) ) { if (!STATE(GPS_FIX)) {
//Speed should be sent in m/s (GPS speed is in cm/s) return;
sendDataHead(ID_GPS_SPEED_BP);
serialize16((GPS_speed * 0.01 + 0.5));
sendDataHead(ID_GPS_SPEED_AP);
serialize16(0); //Not dipslayed
} }
//Speed should be sent in m/s (GPS speed is in cm/s)
sendDataHead(ID_GPS_SPEED_BP);
serialize16((GPS_speed * 0.01 + 0.5));
sendDataHead(ID_GPS_SPEED_AP);
serialize16(0); //Not dipslayed
} }
static void sendTime(void) static void sendTime(void)
{ {
uint32_t seconds = millis() / 1000; uint32_t seconds = millis() / 1000;
@ -241,35 +244,35 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
{ {
int32_t absgps, deg, min; int32_t absgps, deg, min;
absgps = abs(mwiigps); absgps = abs(mwiigps);
deg = absgps / 10000000; deg = absgps / GPS_DEGREES_DIVIDER;
absgps = (absgps - deg * 10000000) * 60; // absgps = Minutes left * 10^7 absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
min = absgps / 10000000; // minutes left min = absgps / GPS_DEGREES_DIVIDER; // minutes left
if( telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS ) { if (telemetryConfig->frsky_coordinate_format == FRSKY_FORMAT_DMS) {
result->dddmm = deg * 100 + min; result->dddmm = deg * 100 + min;
} else { } else {
result->dddmm = deg * 60 + min; result->dddmm = deg * 60 + min;
} }
result->mmmm = (absgps - min * 10000000) / 1000; result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
} }
static void sendGPS(void) static void sendGPS(void)
{ {
int32_t localGPS_coord[2] = {0,0}; int32_t localGPS_coord[2] = {0,0};
// Don't set dummy GPS data, if we already had a GPS fix // Don't set dummy GPS data, if we already had a GPS fix
// it can be usefull to keep last valid coordinates // it can be usefull to keep last valid coordinates
static uint8_t gpsFixOccured = 0; static uint8_t gpsFixOccured = 0;
//Dummy data if no 3D fix, this way we can display heading in Taranis //Dummy data if no 3D fix, this way we can display heading in Taranis
if( STATE(GPS_FIX) || gpsFixOccured == 1 ) { if (STATE(GPS_FIX) || gpsFixOccured == 1) {
localGPS_coord[LAT] = GPS_coord[LAT]; localGPS_coord[LAT] = GPS_coord[LAT];
localGPS_coord[LON] = GPS_coord[LON]; localGPS_coord[LON] = GPS_coord[LON];
gpsFixOccured = 1; gpsFixOccured = 1;
} else { } else {
// Send dummy GPS Data in order to display compass value // Send dummy GPS Data in order to display compass value
localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLat * 10000000); localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
localGPS_coord[LON] = (telemetryConfig->gpsNoFixLon * 10000000); localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
} }
gpsCoordinateDDDMMmmmm_t coordinate; gpsCoordinateDDDMMmmmm_t coordinate;
@ -367,11 +370,11 @@ static void sendAmperage(void)
static void sendFuelLevel(void) static void sendFuelLevel(void)
{ {
uint16_t batterySize = telemetryConfig->batterySize; uint16_t batterySize = telemetryConfig->batterySize;
sendDataHead(ID_FUEL_LEVEL); sendDataHead(ID_FUEL_LEVEL);
if( batterySize > 0 ) { if (batterySize > 0) {
serialize16((uint16_t)constrain((batterySize - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0 / batterySize , 0, 100)); serialize16((uint16_t)constrain((batterySize - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0 / batterySize , 0, 100));
} else { } else {
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF)); serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
@ -457,7 +460,7 @@ void handleFrSkyTelemetry(void)
sendTelemetryTail(); sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms if ((cycleNum % 4) == 0) { // Sent every 500ms
if( lastCycleTime > DELAY_FOR_BARO_INITIALISATION ) { //Allow 5s to boot correctly if (lastCycleTime > DELAY_FOR_BARO_INITIALISATION) { //Allow 5s to boot correctly
sendBaro(); sendBaro();
} }
sendHeading(); sendHeading();
@ -466,7 +469,7 @@ void handleFrSkyTelemetry(void)
if ((cycleNum % 8) == 0) { // Sent every 1s if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1(); sendTemperature1();
sendRpm(); sendThrottleOrBatterySizeAsRpm();
if (feature(FEATURE_VBAT)) { if (feature(FEATURE_VBAT)) {
sendVoltage(); sendVoltage();
@ -477,16 +480,16 @@ void handleFrSkyTelemetry(void)
#ifdef GPS #ifdef GPS
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
sendSpeed(); sendSpeed();
sendGpsAltitude(); sendGpsAltitude();
sendTemperature2(); sendSatalliteSignalQualityAsTemperature2();
} }
#endif #endif
// Send GPS information to display compass information // Send GPS information to display compass information
if( (telemetryConfig->gpsNoFixLat != 0 && telemetryConfig->gpsNoFixLon != 0) || sensors(SENSOR_GPS) ) { if (sensors(SENSOR_GPS) || (telemetryConfig->gpsNoFixLatitude != 0 && telemetryConfig->gpsNoFixLongitude != 0)) {
sendGPS(); sendGPS();
} }
sendTelemetryTail(); sendTelemetryTail();
} }

8
src/main/telemetry/hott.c
View file

@ -146,8 +146,8 @@ static void initialiseMessages(void)
#ifdef GPS #ifdef GPS
void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude) void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
{ {
int16_t deg = latitude / 10000000L; int16_t deg = latitude / GPS_DEGREES_DIVIDER;
int32_t sec = (latitude - (deg * 10000000L)) * 6; int32_t sec = (latitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
int8_t min = sec / 1000000L; int8_t min = sec / 1000000L;
sec = (sec % 1000000L) / 100L; sec = (sec % 1000000L) / 100L;
uint16_t degMin = (deg * 100L) + min; uint16_t degMin = (deg * 100L) + min;
@ -158,8 +158,8 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t
hottGPSMessage->pos_NS_sec_L = sec; hottGPSMessage->pos_NS_sec_L = sec;
hottGPSMessage->pos_NS_sec_H = sec >> 8; hottGPSMessage->pos_NS_sec_H = sec >> 8;
deg = longitude / 10000000L; deg = longitude / GPS_DEGREES_DIVIDER;
sec = (longitude - (deg * 10000000L)) * 6; sec = (longitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
min = sec / 1000000L; min = sec / 1000000L;
sec = (sec % 1000000L) / 100L; sec = (sec % 1000000L) / 100L;
degMin = (deg * 100L) + min; degMin = (deg * 100L) + min;

4
src/main/telemetry/telemetry.h
View file

@ -45,8 +45,8 @@ typedef struct telemetryConfig_s {
telemetryProvider_e telemetry_provider; telemetryProvider_e telemetry_provider;
uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed. uint8_t telemetry_switch; // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
serialInversion_e frsky_inversion; serialInversion_e frsky_inversion;
float gpsNoFixLat; float gpsNoFixLatitude;
float gpsNoFixLon; float gpsNoFixLongitude;
frskyGpsCoordFormat_e frsky_coordinate_format; frskyGpsCoordFormat_e frsky_coordinate_format;
frskyUnit_e frsky_unit; frskyUnit_e frsky_unit;
uint16_t batterySize; uint16_t batterySize;