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->frsky_inversion = SERIAL_NOT_INVERTED;
telemetryConfig->telemetry_switch = 0;
telemetryConfig->gpsNoFixLat = 0;
telemetryConfig->gpsNoFixLon = 0;
telemetryConfig->gpsNoFixLatitude = 0;
telemetryConfig->gpsNoFixLongitude = 0;
telemetryConfig->frsky_coordinate_format = FRSKY_FORMAT_DMS;
telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS;
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.state_position < sizeof(ubloxInit)) {
//Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
//Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
}
gpsData.state_position++;
@ -268,9 +268,9 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
//Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
//Either use specific config file for GPS or let dynamically upload config
if( gpsConfig->gpsAutoConfig == GPS_AUTOCONFIG_ON ) {
serialWrite(gpsPort, ubloxSbas[gpsConfig->sbasMode].message[gpsData.state_position]);
}
gpsData.state_position++;
} else {

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

@ -20,6 +20,8 @@
#define LAT 0
#define LON 1
#define GPS_DEGREES_DIVIDER 10000000L
typedef enum {
GPS_NMEA = 0,
GPS_UBLOX
@ -54,7 +56,7 @@ typedef enum {
typedef struct gpsConfig_s {
gpsProvider_e provider;
sbasMode_e sbasMode;
gpsAutoConfig_e gpsAutoConfig;
gpsAutoConfig_e gpsAutoConfig;
} gpsConfig_t;
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_switch", VAR_UINT8 | MASTER_VALUE, &masterConfig.telemetryConfig.telemetry_switch, 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_lon", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLon, -180.0, 180.0 },
{ "frsky_default_lattitude", VAR_FLOAT | MASTER_VALUE, &masterConfig.telemetryConfig.gpsNoFixLatitude, -90.0, 90.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_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 },

95
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_MAX_HDOP_VAL 9999
#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 uint8_t cycleNum = 0;
@ -164,7 +164,7 @@ static void sendGpsAltitude(void)
{
uint16_t altitude = GPS_altitude;
//Send real GPS altitude only if it's reliable (there's a GPS fix)
if( !STATE(GPS_FIX) ) {
if (!STATE(GPS_FIX)) {
altitude = 0;
}
sendDataHead(ID_GPS_ALTIDUTE_BP);
@ -174,36 +174,36 @@ static void sendGpsAltitude(void)
}
static void sendRpm(void)
static void sendThrottleOrBatterySizeAsRpm(void)
{
sendDataHead(ID_RPM);
if( ARMING_FLAG(ARMED) ) {
if (ARMING_FLAG(ARMED)) {
serialize16(rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER);
} else {
serialize16((telemetryConfig->batterySize / BLADE_NUMBER_DIVIDER));
}
}
static void sendTemperature1(void)
{
sendDataHead(ID_TEMPRATURE1);
#ifdef BARO
#ifdef BARO
serialize16((baroTemperature + 50)/ 100); //Airmamaf
#else
#else
serialize16(telemTemperature1 / 10);
#endif
#endif
}
static void sendTemperature2(void)
static void sendSatalliteSignalQualityAsTemperature2(void)
{
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);
}
sendDataHead(ID_TEMPRATURE2);
if( telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS ) {
if (telemetryConfig->frsky_unit == FRSKY_UNIT_METRICS) {
serialize16(satellite);
} else {
float tmp = (satellite - 32) / 1.8;
@ -212,16 +212,19 @@ static void sendTemperature2(void)
serialize16(tmp);
}
}
static void sendSpeed(void)
{
if( STATE(GPS_FIX) ) {
//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
if (!STATE(GPS_FIX)) {
return;
}
//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)
{
uint32_t seconds = millis() / 1000;
@ -241,37 +244,37 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
{
int32_t absgps, deg, min;
absgps = abs(mwiigps);
deg = absgps / 10000000;
absgps = (absgps - deg * 10000000) * 60; // absgps = Minutes left * 10^7
min = absgps / 10000000; // minutes left
deg = absgps / GPS_DEGREES_DIVIDER;
absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
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;
} else {
result->dddmm = deg * 60 + min;
}
result->mmmm = (absgps - min * 10000000) / 1000;
result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
}
static void sendGPS(void)
{
int32_t localGPS_coord[2] = {0,0};
// Don't set dummy GPS data, if we already had a GPS fix
// it can be usefull to keep last valid coordinates
static uint8_t gpsFixOccured = 0;
int32_t localGPS_coord[2] = {0,0};
// Don't set dummy GPS data, if we already had a GPS fix
// it can be usefull to keep last valid coordinates
static uint8_t gpsFixOccured = 0;
//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[LON] = GPS_coord[LON];
gpsFixOccured = 1;
} else {
// Send dummy GPS Data in order to display compass value
localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLat * 10000000);
localGPS_coord[LON] = (telemetryConfig->gpsNoFixLon * 10000000);
localGPS_coord[LAT] = (telemetryConfig->gpsNoFixLatitude * GPS_DEGREES_DIVIDER);
localGPS_coord[LON] = (telemetryConfig->gpsNoFixLongitude * GPS_DEGREES_DIVIDER);
}
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
@ -367,11 +370,11 @@ static void sendAmperage(void)
static void sendFuelLevel(void)
{
uint16_t batterySize = telemetryConfig->batterySize;
uint16_t batterySize = telemetryConfig->batterySize;
sendDataHead(ID_FUEL_LEVEL);
if( batterySize > 0 ) {
if (batterySize > 0) {
serialize16((uint16_t)constrain((batterySize - constrain(mAhDrawn, 0, 0xFFFF)) * 100.0 / batterySize , 0, 100));
} else {
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
@ -457,7 +460,7 @@ void handleFrSkyTelemetry(void)
sendTelemetryTail();
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();
}
sendHeading();
@ -466,7 +469,7 @@ void handleFrSkyTelemetry(void)
if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1();
sendRpm();
sendThrottleOrBatterySizeAsRpm();
if (feature(FEATURE_VBAT)) {
sendVoltage();
@ -477,16 +480,16 @@ void handleFrSkyTelemetry(void)
#ifdef GPS
if (sensors(SENSOR_GPS)) {
sendSpeed();
sendGpsAltitude();
sendTemperature2();
}
sendSpeed();
sendGpsAltitude();
sendSatalliteSignalQualityAsTemperature2();
}
#endif
// Send GPS information to display compass information
if( (telemetryConfig->gpsNoFixLat != 0 && telemetryConfig->gpsNoFixLon != 0) || sensors(SENSOR_GPS) ) {
sendGPS();
}
// Send GPS information to display compass information
if (sensors(SENSOR_GPS) || (telemetryConfig->gpsNoFixLatitude != 0 && telemetryConfig->gpsNoFixLongitude != 0)) {
sendGPS();
}
sendTelemetryTail();
}

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

@ -146,8 +146,8 @@ static void initialiseMessages(void)
#ifdef GPS
void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude)
{
int16_t deg = latitude / 10000000L;
int32_t sec = (latitude - (deg * 10000000L)) * 6;
int16_t deg = latitude / GPS_DEGREES_DIVIDER;
int32_t sec = (latitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
int8_t min = sec / 1000000L;
sec = (sec % 1000000L) / 100L;
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_H = sec >> 8;
deg = longitude / 10000000L;
sec = (longitude - (deg * 10000000L)) * 6;
deg = longitude / GPS_DEGREES_DIVIDER;
sec = (longitude - (deg * GPS_DEGREES_DIVIDER)) * 6;
min = sec / 1000000L;
sec = (sec % 1000000L) / 100L;
degMin = (deg * 100L) + min;

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

@ -45,8 +45,8 @@ typedef struct telemetryConfig_s {
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.
serialInversion_e frsky_inversion;
float gpsNoFixLat;
float gpsNoFixLon;
float gpsNoFixLatitude;
float gpsNoFixLongitude;
frskyGpsCoordFormat_e frsky_coordinate_format;
frskyUnit_e frsky_unit;
uint16_t batterySize;