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

Merge branch 'airmamaf' of github.com:airmamaf/cleanflight into airmamaf-airmamaf

This commit is contained in:
Dominic Clifton 2014-10-18 15:06:42 +01:00
parent 50971667d9 9b84516344
commit 1c501272ab
6 changed files with 151 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/config/config.c
View File

@ -201,6 +201,11 @@ 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->frsky_coordinate_format = FRSKY_FORMAT_DMS;
telemetryConfig->frsky_unit = FRSKY_UNIT_METRICS;
telemetryConfig->batterySize = 0;
}
void resetSerialConfig(serialConfig_t *serialConfig)
@ -305,6 +310,7 @@ static void resetConf(void)
// gps/nav stuff
masterConfig.gpsConfig.provider = GPS_NMEA;
masterConfig.gpsConfig.sbasMode = SBAS_AUTO;
masterConfig.gpsConfig.gpsAutoConfig = GPS_AUTOCONFIG_ON;
#endif
resetSerialConfig(&masterConfig.serialConfig);

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

@ -255,7 +255,10 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_INIT) {
if (gpsData.state_position < sizeof(ubloxInit)) {
serialWrite(gpsPort, ubloxInit[gpsData.state_position]);
//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++;
} else {
gpsData.state_position = 0;
@ -265,7 +268,10 @@ void gpsInitUblox(void)
if (gpsData.messageState == GPS_MESSAGE_STATE_SBAS) {
if (gpsData.state_position < UBLOX_SBAS_MESSAGE_LENGTH) {
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 {
gpsData.messageState++;

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

@ -45,11 +45,16 @@ typedef enum {
GPS_BAUDRATE_9600
} gpsBaudRate_e;
typedef enum {
GPS_AUTOCONFIG_ON = 0,
GPS_AUTOCONFIG_OFF
} gpsAutoConfig_e;
#define GPS_BAUDRATE_MAX GPS_BAUDRATE_9600
typedef struct gpsConfig_s {
gpsProvider_e provider;
sbasMode_e sbasMode;
gpsAutoConfig_e gpsAutoConfig;
} gpsConfig_t;
typedef enum {

6
src/main/io/serial_cli.c
View File

@ -241,6 +241,7 @@ const clivalue_t valueTable[] = {
{ "gps_provider", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.provider, 0, GPS_PROVIDER_MAX },
{ "gps_sbas_mode", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.sbasMode, 0, SBAS_MODE_MAX },
{ "gps_auto_config", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsConfig.gpsAutoConfig, 0, GPS_AUTOCONFIG_OFF },
{ "gps_pos_p", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDPOS], 0, 200 },
@ -264,6 +265,11 @@ 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_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 },
{ "vbat_scale", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatscale, VBAT_SCALE_MIN, VBAT_SCALE_MAX },
{ "vbat_max_cell_voltage", VAR_UINT8 | MASTER_VALUE, &masterConfig.batteryConfig.vbatmaxcellvoltage, 10, 50 },

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

@ -35,6 +35,8 @@
#include "drivers/timer.h"
#include "drivers/serial.h"
#include "io/serial.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "config/runtime_config.h"
#include "config/config.h"
@ -100,6 +102,13 @@ extern int16_t telemTemperature1; // FIXME dependency on mw.c
#define ID_VERT_SPEED 0x30 //opentx vario
#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
static uint32_t lastCycleTime = 0;
static uint8_t cycleNum = 0;
static void sendDataHead(uint8_t id)
{
serialWrite(frskyPort, PROTOCOL_HEADER);
@ -151,12 +160,68 @@ static void sendBaro(void)
serialize16(abs(BaroAlt % 100));
}
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) ) {
altitude = 0;
}
sendDataHead(ID_GPS_ALTIDUTE_BP);
serialize16(altitude);
sendDataHead(ID_GPS_ALTIDUTE_AP);
serialize16(0);
}
static void sendRpm(void)
{
sendDataHead(ID_RPM);
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
serialize16((baroTemperature + 50)/ 100); //Airmamaf
#else
serialize16(telemTemperature1 / 10);
#endif
}
static void sendTemperature2(void)
{
uint16_t satellite = GPS_numSat;
//Send instead Num sat and quality GPS
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 ) {
serialize16(satellite);
} else {
float tmp = (satellite - 32) / 1.8;
//Round the value
tmp += (tmp < 0) ? -0.5 : 0.5;
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
}
}
static void sendTime(void)
{
uint32_t seconds = millis() / 1000;
@ -180,28 +245,49 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
absgps = (absgps - deg * 10000000) * 60; // absgps = Minutes left * 10^7
min = absgps / 10000000; // minutes left
result->dddmm = deg * 100 + min;
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;
}
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;
//Dummy data if no 3D fix, this way we can display heading in Taranis
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);
}
gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(GPS_coord[LAT], &coordinate);
GPStoDDDMM_MMMM(localGPS_coord[LAT], &coordinate);
sendDataHead(ID_LATITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LATITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_N_S);
serialize16(GPS_coord[LAT] < 0 ? 'S' : 'N');
serialize16(localGPS_coord[LAT] < 0 ? 'S' : 'N');
GPStoDDDMM_MMMM(GPS_coord[LON], &coordinate);
GPStoDDDMM_MMMM(localGPS_coord[LON], &coordinate);
sendDataHead(ID_LONGITUDE_BP);
serialize16(coordinate.dddmm);
sendDataHead(ID_LONGITUDE_AP);
serialize16(coordinate.mmmm);
sendDataHead(ID_E_W);
serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
serialize16(localGPS_coord[LON] < 0 ? 'W' : 'E');
}
#endif
@ -281,8 +367,15 @@ static void sendAmperage(void)
static void sendFuelLevel(void)
{
uint16_t batterySize = telemetryConfig->batterySize;
sendDataHead(ID_FUEL_LEVEL);
serialize16((uint16_t)constrain(mAhDrawn, 0, 0xFFFF));
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));
}
}
static void sendHeading(void)
@ -331,8 +424,6 @@ void configureFrSkyTelemetryPort(void)
}
}
static uint32_t lastCycleTime = 0;
static uint8_t cycleNum = 0;
bool canSendFrSkyTelemetry(void)
{
@ -366,13 +457,16 @@ void handleFrSkyTelemetry(void)
sendTelemetryTail();
if ((cycleNum % 4) == 0) { // Sent every 500ms
sendBaro();
if( lastCycleTime > DELAY_FOR_BARO_INITIALISATION ) { //Allow 5s to boot correctly
sendBaro();
}
sendHeading();
sendTelemetryTail();
}
if ((cycleNum % 8) == 0) { // Sent every 1s
sendTemperature1();
sendRpm();
if (feature(FEATURE_VBAT)) {
sendVoltage();
@ -382,10 +476,17 @@ void handleFrSkyTelemetry(void)
}
#ifdef GPS
if (sensors(SENSOR_GPS))
sendGPS();
if (sensors(SENSOR_GPS)) {
sendSpeed();
sendGpsAltitude();
sendTemperature2();
}
#endif
// Send GPS information to display compass information
if( (telemetryConfig->gpsNoFixLat != 0 && telemetryConfig->gpsNoFixLon != 0) || sensors(SENSOR_GPS) ) {
sendGPS();
}
sendTelemetryTail();
}

14
src/main/telemetry/telemetry.h
View File

@ -32,10 +32,24 @@ typedef enum {
TELEMETRY_PROVIDER_MAX = TELEMETRY_PROVIDER_MSP
} telemetryProvider_e;
typedef enum {
FRSKY_FORMAT_DMS = 0,
FRSKY_FORMAT_NMEA
} frskyGpsCoordFormat_e;
typedef enum {
FRSKY_UNIT_METRICS = 0,
FRSKY_UNIT_IMPERIALS
} frskyUnit_e;
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;
frskyGpsCoordFormat_e frsky_coordinate_format;
frskyUnit_e frsky_unit;
uint16_t batterySize;
} telemetryConfig_t;
void checkTelemetryState(void);