1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00

GPS can now be conditionally compiled in.

This commit is contained in:
Dominic Clifton 2014-06-28 20:20:16 +09:00
parent b3a718882c
commit 3b629d58a0
19 changed files with 144 additions and 29 deletions

View file

@ -132,6 +132,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->H_level = 3.0f; pidProfile->H_level = 3.0f;
} }
#ifdef GPS
void resetGpsProfile(gpsProfile_t *gpsProfile) void resetGpsProfile(gpsProfile_t *gpsProfile)
{ {
gpsProfile->gps_wp_radius = 200; gpsProfile->gps_wp_radius = 200;
@ -142,6 +143,7 @@ void resetGpsProfile(gpsProfile_t *gpsProfile)
gpsProfile->nav_speed_max = 300; gpsProfile->nav_speed_max = 300;
gpsProfile->ap_mode = 40; gpsProfile->ap_mode = 40;
} }
#endif
void resetBarometerConfig(barometerConfig_t *barometerConfig) void resetBarometerConfig(barometerConfig_t *barometerConfig)
{ {
@ -257,9 +259,11 @@ static void resetConf(void)
masterConfig.motor_pwm_rate = 400; masterConfig.motor_pwm_rate = 400;
masterConfig.servo_pwm_rate = 50; masterConfig.servo_pwm_rate = 50;
#ifdef GPS
// gps/nav stuff // gps/nav stuff
masterConfig.gpsConfig.provider = GPS_NMEA; masterConfig.gpsConfig.provider = GPS_NMEA;
masterConfig.gpsConfig.sbasMode = SBAS_AUTO; masterConfig.gpsConfig.sbasMode = SBAS_AUTO;
#endif
resetSerialConfig(&masterConfig.serialConfig); resetSerialConfig(&masterConfig.serialConfig);
@ -323,7 +327,9 @@ static void resetConf(void)
// gimbal // gimbal
currentProfile.gimbalConfig.gimbal_flags = GIMBAL_NORMAL; currentProfile.gimbalConfig.gimbal_flags = GIMBAL_NORMAL;
#ifdef GPS
resetGpsProfile(&currentProfile.gpsProfile); resetGpsProfile(&currentProfile.gpsProfile);
#endif
// custom mixer. clear by defaults. // custom mixer. clear by defaults.
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) for (i = 0; i < MAX_SUPPORTED_MOTORS; i++)
@ -376,8 +382,10 @@ void activateConfig(void)
useGyroConfig(&masterConfig.gyroConfig); useGyroConfig(&masterConfig.gyroConfig);
useTelemetryConfig(&masterConfig.telemetryConfig); useTelemetryConfig(&masterConfig.telemetryConfig);
setPIDController(currentProfile.pidController); setPIDController(currentProfile.pidController);
#ifdef GPS
gpsUseProfile(&currentProfile.gpsProfile); gpsUseProfile(&currentProfile.gpsProfile);
gpsUsePIDs(&currentProfile.pidProfile); gpsUsePIDs(&currentProfile.pidProfile);
#endif
useFailsafeConfig(&currentProfile.failsafeConfig); useFailsafeConfig(&currentProfile.failsafeConfig);
setAccelerationTrims(&masterConfig.accZero); setAccelerationTrims(&masterConfig.accZero);
mixerUseConfigs( mixerUseConfigs(

View file

@ -64,7 +64,9 @@ typedef struct master_t {
airplaneConfig_t airplaneConfig; airplaneConfig_t airplaneConfig;
int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign int8_t fixedwing_althold_dir; // +1 or -1 for pitch/althold gain. later check if need more than just sign
#ifdef GPS
gpsConfig_t gpsConfig; gpsConfig_t gpsConfig;
#endif
serialConfig_t serialConfig; serialConfig_t serialConfig;

View file

@ -64,5 +64,7 @@ typedef struct profile_s {
// gimbal-related configuration // gimbal-related configuration
gimbalConfig_t gimbalConfig; gimbalConfig_t gimbalConfig;
#ifdef GPS
gpsProfile_t gpsProfile; gpsProfile_t gpsProfile;
#endif
} profile_t; } profile_t;

View file

@ -102,9 +102,13 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control
AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f; AngleRate = (float)((controlRateConfig->yawRate + 10) * rcCommand[YAW]) / 50.0f;
} else { } else {
// calculate error and limit the angle to the max inclination // calculate error and limit the angle to the max inclination
#ifdef GPS
errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), errorAngle = (constrain(rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#else
errorAngle = (constrain(rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here
#endif
if (shouldAutotune()) { if (shouldAutotune()) {
errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle); errorAngle = autotune(rcAliasToAngleIndexMap[axis], &inclination, errorAngle);
} }
@ -174,8 +178,13 @@ static void pidMultiWii(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC if ((f.ANGLE_MODE || f.HORIZON_MODE) && (axis == FD_ROLL || axis == FD_PITCH)) { // MODE relying on ACC
// observe max inclination // observe max inclination
#ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];
#endif
if (shouldAutotune()) { if (shouldAutotune()) {
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
@ -243,9 +252,13 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[YAW]) >> 5); AngleRateTmp = (((int32_t)(controlRateConfig->yawRate + 27) * rcCommand[YAW]) >> 5);
} else { } else {
// calculate error and limit the angle to max configured inclination // calculate error and limit the angle to max configured inclination
errorAngle = constrain((rcCommand[axis] << 1) + GPS_angle[axis], -((int) max_angle_inclination), #ifdef GPS
errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
#else
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; // 16 bits is ok here
#endif
if (shouldAutotune()) { if (shouldAutotune()) {
errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle))); errorAngle = DEGREES_TO_DECIDEGREES(autotune(rcAliasToAngleIndexMap[axis], &inclination, DECIDEGREES_TO_DEGREES(errorAngle)));
} }

View file

@ -58,7 +58,9 @@ void beepcodeInit(failsafe_t *initialFailsafe)
void beepcodeUpdateState(bool warn_vbat) void beepcodeUpdateState(bool warn_vbat)
{ {
static uint8_t beeperOnBox; static uint8_t beeperOnBox;
#ifdef GPS
static uint8_t warn_noGPSfix = 0; static uint8_t warn_noGPSfix = 0;
#endif
static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_IDLE; static failsafeBeeperWarnings_e warn_failsafe = FAILSAFE_IDLE;
//===================== BeeperOn via rcOptions ===================== //===================== BeeperOn via rcOptions =====================
@ -86,6 +88,7 @@ void beepcodeUpdateState(bool warn_vbat)
} }
} }
#ifdef GPS
//===================== GPS fix notification handling ===================== //===================== GPS fix notification handling =====================
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if ((rcOptions[BOXGPSHOME] || rcOptions[BOXGPSHOLD]) && !f.GPS_FIX) { // if no fix and gps funtion is activated: do warning beeps if ((rcOptions[BOXGPSHOME] || rcOptions[BOXGPSHOLD]) && !f.GPS_FIX) { // if no fix and gps funtion is activated: do warning beeps
@ -94,6 +97,7 @@ void beepcodeUpdateState(bool warn_vbat)
warn_noGPSfix = 0; warn_noGPSfix = 0;
} }
} }
#endif
//===================== Priority driven Handling ===================== //===================== Priority driven Handling =====================
// beepcode(length1,length2,length3,pause) // beepcode(length1,length2,length3,pause)
@ -102,8 +106,10 @@ void beepcodeUpdateState(bool warn_vbat)
beep_code('L','N','N','D'); // failsafe "find me" signal beep_code('L','N','N','D'); // failsafe "find me" signal
else if (warn_failsafe == 1) else if (warn_failsafe == 1)
beep_code('S','M','L','M'); // failsafe landing active beep_code('S','M','L','M'); // failsafe landing active
#ifdef GPS
else if (warn_noGPSfix == 1) else if (warn_noGPSfix == 1)
beep_code('S','S','N','M'); beep_code('S','S','N','M');
#endif
else if (beeperOnBox == 1) else if (beeperOnBox == 1)
beep_code('S','S','S','M'); // beeperon beep_code('S','S','S','M'); // beeperon
else if (warn_vbat) else if (warn_vbat)

View file

@ -15,6 +15,7 @@
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>. * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/ */
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <ctype.h> #include <ctype.h>
@ -45,6 +46,8 @@
#include "gps_conversion.h" #include "gps_conversion.h"
#include "io/gps.h" #include "io/gps.h"
#ifdef GPS
extern int16_t debug[4]; extern int16_t debug[4];
@ -647,7 +650,7 @@ static void gpsNewData(uint16_t c)
} }
break; break;
default: default:
; break;
} }
} //end of gps calcs } //end of gps calcs
} }
@ -1460,3 +1463,4 @@ void updateGpsIndicator(uint32_t currentTime)
LED1_TOGGLE; LED1_TOGGLE;
} }
} }
#endif

View file

@ -20,6 +20,10 @@
#include <ctype.h> #include <ctype.h>
#include <string.h> #include <string.h>
#include "platform.h"
#ifdef GPS
#define DIGIT_TO_VAL(_x) (_x - '0') #define DIGIT_TO_VAL(_x) (_x - '0')
uint32_t GPS_coord_to_degrees(char* coordinateString) uint32_t GPS_coord_to_degrees(char* coordinateString)
@ -61,3 +65,4 @@ uint32_t GPS_coord_to_degrees(char* coordinateString)
} }
return degress * 10000000UL + (minutes * 1000000UL + fractionalMinutes * 100UL) / 6; return degress * 10000000UL + (minutes * 1000000UL + fractionalMinutes * 100UL) / 6;
} }
#endif

View file

@ -113,12 +113,18 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration // GYRO calibration
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES); gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
if (feature(FEATURE_GPS))
#ifdef GPS
if (feature(FEATURE_GPS)) {
GPS_reset_home_position(); GPS_reset_home_position();
}
#endif
#ifdef BARO #ifdef BARO
if (sensors(SENSOR_BARO)) if (sensors(SENSOR_BARO))
baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking) baroSetCalibrationCycles(10); // calibrate baro to new ground level (10 * 25 ms = ~250 ms non blocking)
#endif #endif
if (!sensors(SENSOR_MAG)) if (!sensors(SENSOR_MAG))
heading = 0; // reset heading to zero after gyro calibration heading = 0; // reset heading to zero after gyro calibration

View file

@ -67,7 +67,9 @@ static void cliDefaults(char *cmdline);
static void cliDump(char *cmdLine); static void cliDump(char *cmdLine);
static void cliExit(char *cmdline); static void cliExit(char *cmdline);
static void cliFeature(char *cmdline); static void cliFeature(char *cmdline);
#ifdef GPS
static void cliGpsPassthrough(char *cmdline); static void cliGpsPassthrough(char *cmdline);
#endif
static void cliHelp(char *cmdline); static void cliHelp(char *cmdline);
static void cliMap(char *cmdline); static void cliMap(char *cmdline);
static void cliMixer(char *cmdline); static void cliMixer(char *cmdline);
@ -130,7 +132,9 @@ const clicmd_t cmdTable[] = {
{ "dump", "print configurable settings in a pastable form", cliDump }, { "dump", "print configurable settings in a pastable form", cliDump },
{ "exit", "", cliExit }, { "exit", "", cliExit },
{ "feature", "list or -val or val", cliFeature }, { "feature", "list or -val or val", cliFeature },
#ifdef GPS
{ "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough }, { "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough },
#endif
{ "help", "", cliHelp }, { "help", "", cliHelp },
{ "map", "mapping of rc channel order", cliMap }, { "map", "mapping of rc channel order", cliMap },
{ "mixer", "mixer name or list", cliMixer }, { "mixer", "mixer name or list", cliMixer },
@ -199,12 +203,31 @@ const clivalue_t valueTable[] = {
{ "reboot_character", VAR_UINT8, &masterConfig.serialConfig.reboot_character, 48, 126 }, { "reboot_character", VAR_UINT8, &masterConfig.serialConfig.reboot_character, 48, 126 },
{ "msp_baudrate", VAR_UINT32, &masterConfig.serialConfig.msp_baudrate, 1200, 115200 }, { "msp_baudrate", VAR_UINT32, &masterConfig.serialConfig.msp_baudrate, 1200, 115200 },
{ "cli_baudrate", VAR_UINT32, &masterConfig.serialConfig.cli_baudrate, 1200, 115200 }, { "cli_baudrate", VAR_UINT32, &masterConfig.serialConfig.cli_baudrate, 1200, 115200 },
#ifdef GPS
{ "gps_baudrate", VAR_UINT32, &masterConfig.serialConfig.gps_baudrate, 0, 115200 }, { "gps_baudrate", VAR_UINT32, &masterConfig.serialConfig.gps_baudrate, 0, 115200 },
{ "gps_passthrough_baudrate", VAR_UINT32, &masterConfig.serialConfig.gps_passthrough_baudrate, 1200, 115200 }, { "gps_passthrough_baudrate", VAR_UINT32, &masterConfig.serialConfig.gps_passthrough_baudrate, 1200, 115200 },
{ "gps_provider", VAR_UINT8, &masterConfig.gpsConfig.provider, 0, GPS_PROVIDER_MAX }, { "gps_provider", VAR_UINT8, &masterConfig.gpsConfig.provider, 0, GPS_PROVIDER_MAX },
{ "gps_sbas_mode", VAR_UINT8, &masterConfig.gpsConfig.sbasMode, 0, SBAS_MODE_MAX }, { "gps_sbas_mode", VAR_UINT8, &masterConfig.gpsConfig.sbasMode, 0, SBAS_MODE_MAX },
{ "gps_pos_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDPOS], 0, 200 },
{ "gps_posr_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDPOSR], 0, 200 },
{ "gps_posr_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDPOSR], 0, 200 },
{ "gps_posr_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDPOSR], 0, 200 },
{ "gps_nav_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDNAVR], 0, 200 },
{ "gps_nav_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDNAVR], 0, 200 },
{ "gps_nav_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDNAVR], 0, 200 },
{ "gps_wp_radius", VAR_UINT16, &currentProfile.gpsProfile.gps_wp_radius, 0, 2000 },
{ "nav_controls_heading", VAR_UINT8, &currentProfile.gpsProfile.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16, &currentProfile.gpsProfile.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16, &currentProfile.gpsProfile.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8, &currentProfile.gpsProfile.nav_slew_rate, 0, 100 },
#endif
{ "serialrx_provider", VAR_UINT8, &masterConfig.rxConfig.serialrx_provider, 0, SERIALRX_PROVIDER_MAX }, { "serialrx_provider", VAR_UINT8, &masterConfig.rxConfig.serialrx_provider, 0, SERIALRX_PROVIDER_MAX },
{ "telemetry_provider", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX }, { "telemetry_provider", VAR_UINT8, &masterConfig.telemetryConfig.telemetry_provider, 0, TELEMETRY_PROVIDER_MAX },
@ -279,21 +302,6 @@ const clivalue_t valueTable[] = {
{ "mag_declination", VAR_INT16, &currentProfile.mag_declination, -18000, 18000 }, { "mag_declination", VAR_INT16, &currentProfile.mag_declination, -18000, 18000 },
{ "gps_pos_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDPOS], 0, 200 },
{ "gps_pos_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDPOS], 0, 200 },
{ "gps_pos_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDPOS], 0, 200 },
{ "gps_posr_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDPOSR], 0, 200 },
{ "gps_posr_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDPOSR], 0, 200 },
{ "gps_posr_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDPOSR], 0, 200 },
{ "gps_nav_p", VAR_UINT8, &currentProfile.pidProfile.P8[PIDNAVR], 0, 200 },
{ "gps_nav_i", VAR_UINT8, &currentProfile.pidProfile.I8[PIDNAVR], 0, 200 },
{ "gps_nav_d", VAR_UINT8, &currentProfile.pidProfile.D8[PIDNAVR], 0, 200 },
{ "gps_wp_radius", VAR_UINT16, &currentProfile.gpsProfile.gps_wp_radius, 0, 2000 },
{ "nav_controls_heading", VAR_UINT8, &currentProfile.gpsProfile.nav_controls_heading, 0, 1 },
{ "nav_speed_min", VAR_UINT16, &currentProfile.gpsProfile.nav_speed_min, 10, 2000 },
{ "nav_speed_max", VAR_UINT16, &currentProfile.gpsProfile.nav_speed_max, 10, 2000 },
{ "nav_slew_rate", VAR_UINT8, &currentProfile.gpsProfile.nav_slew_rate, 0, 100 },
{ "pid_controller", VAR_UINT8, &currentProfile.pidController, 0, 2 }, { "pid_controller", VAR_UINT8, &currentProfile.pidController, 0, 2 },
{ "p_pitch", VAR_UINT8, &currentProfile.pidProfile.P8[PITCH], 0, 200 }, { "p_pitch", VAR_UINT8, &currentProfile.pidProfile.P8[PITCH], 0, 200 },
@ -606,12 +614,27 @@ static void cliFeature(char *cmdline)
cliPrint("Invalid feature name...\r\n"); cliPrint("Invalid feature name...\r\n");
break; break;
} }
if (strncasecmp(cmdline, featureNames[i], len) == 0) { if (strncasecmp(cmdline, featureNames[i], len) == 0) {
mask = 1 << i;
#ifndef GPS
if (mask & FEATURE_GPS) {
cliPrint("GPS unavailable\r\n");
break;
}
#endif
#ifndef SONAR
if (mask & FEATURE_SONAR) {
cliPrint("SONAR unavailable\r\n");
break;
}
#endif
if (remove) { if (remove) {
featureClear(1 << i); featureClear(mask);
cliPrint("Disabled "); cliPrint("Disabled ");
} else { } else {
featureSet(1 << i); featureSet(mask);
cliPrint("Enabled "); cliPrint("Enabled ");
} }
printf("%s\r\n", featureNames[i]); printf("%s\r\n", featureNames[i]);
@ -621,6 +644,7 @@ static void cliFeature(char *cmdline)
} }
} }
#ifdef GPS
static void cliGpsPassthrough(char *cmdline) static void cliGpsPassthrough(char *cmdline)
{ {
gpsEnablePassthroughResult_e result = gpsEnablePassthrough(); gpsEnablePassthroughResult_e result = gpsEnablePassthrough();
@ -638,6 +662,7 @@ static void cliGpsPassthrough(char *cmdline)
break; break;
} }
} }
#endif
static void cliHelp(char *cmdline) static void cliHelp(char *cmdline)
{ {

View file

@ -369,10 +369,12 @@ void mspInit(serialConfig_t *serialConfig)
if (feature(FEATURE_SERVO_TILT)) if (feature(FEATURE_SERVO_TILT))
availableBoxes[idx++] = BOXCAMSTAB; availableBoxes[idx++] = BOXCAMSTAB;
#ifdef GPS
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
availableBoxes[idx++] = BOXGPSHOME; availableBoxes[idx++] = BOXGPSHOME;
availableBoxes[idx++] = BOXGPSHOLD; availableBoxes[idx++] = BOXGPSHOLD;
} }
#endif
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE)
availableBoxes[idx++] = BOXPASSTHRU; availableBoxes[idx++] = BOXPASSTHRU;
@ -397,8 +399,10 @@ void mspInit(serialConfig_t *serialConfig)
static void evaluateCommand(void) static void evaluateCommand(void)
{ {
uint32_t i, tmp, junk; uint32_t i, tmp, junk;
#ifdef GPS
uint8_t wp_no; uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0; int32_t lat = 0, lon = 0, alt = 0;
#endif
switch (cmdMSP) { switch (cmdMSP) {
case MSP_SET_RAW_RC: case MSP_SET_RAW_RC:
@ -413,6 +417,7 @@ static void evaluateCommand(void)
currentProfile.accelerometerTrims.values.roll = read16(); currentProfile.accelerometerTrims.values.roll = read16();
headSerialReply(0); headSerialReply(0);
break; break;
#ifdef GPS
case MSP_SET_RAW_GPS: case MSP_SET_RAW_GPS:
f.GPS_FIX = read8(); f.GPS_FIX = read8();
GPS_numSat = read8(); GPS_numSat = read8();
@ -423,6 +428,7 @@ static void evaluateCommand(void)
GPS_update |= 2; // New data signalisation to GPS functions GPS_update |= 2; // New data signalisation to GPS functions
headSerialReply(0); headSerialReply(0);
break; break;
#endif
case MSP_SET_PID: case MSP_SET_PID:
if (currentProfile.pidController == 2) { if (currentProfile.pidController == 2) {
for (i = 0; i < 3; i++) { for (i = 0; i < 3; i++) {
@ -609,6 +615,7 @@ static void evaluateCommand(void)
for (i = 0; i < rxRuntimeConfig.channelCount; i++) for (i = 0; i < rxRuntimeConfig.channelCount; i++)
serialize16(rcData[i]); serialize16(rcData[i]);
break; break;
#ifdef GPS
case MSP_RAW_GPS: case MSP_RAW_GPS:
headSerialReply(16); headSerialReply(16);
serialize8(f.GPS_FIX); serialize8(f.GPS_FIX);
@ -625,6 +632,7 @@ static void evaluateCommand(void)
serialize16(GPS_directionToHome); serialize16(GPS_directionToHome);
serialize8(GPS_update & 1); serialize8(GPS_update & 1);
break; break;
#endif
case MSP_ATTITUDE: case MSP_ATTITUDE:
headSerialReply(6); headSerialReply(6);
for (i = 0; i < 2; i++) for (i = 0; i < 2; i++)
@ -723,6 +731,7 @@ static void evaluateCommand(void)
for (i = 0; i < 8; i++) for (i = 0; i < 8; i++)
serialize8(i + 1); serialize8(i + 1);
break; break;
#ifdef GPS
case MSP_WP: case MSP_WP:
wp_no = read8(); // get the wp number wp_no = read8(); // get the wp number
headSerialReply(18); headSerialReply(18);
@ -766,6 +775,7 @@ static void evaluateCommand(void)
} }
headSerialReply(0); headSerialReply(0);
break; break;
#endif /* GPS */
case MSP_RESET_CONF: case MSP_RESET_CONF:
if (!f.ARMED) { if (!f.ARMED) {
resetEEPROM(); resetEEPROM();
@ -813,6 +823,7 @@ static void evaluateCommand(void)
serialize32(U_ID_1); serialize32(U_ID_1);
serialize32(U_ID_2); serialize32(U_ID_2);
break; break;
#ifdef GPS
case MSP_GPSSVINFO: case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4)); headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh); serialize8(GPS_numCh);
@ -823,6 +834,7 @@ static void evaluateCommand(void)
serialize8(GPS_svinfo_cno[i]); serialize8(GPS_svinfo_cno[i]);
} }
break; break;
#endif
default: // we do not know how to handle the (valid) message, indicate error MSP $M! default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0); headSerialError(0);
break; break;

View file

@ -215,6 +215,7 @@ void init(void)
beepcodeInit(failsafe); beepcodeInit(failsafe);
rxInit(&masterConfig.rxConfig, failsafe); rxInit(&masterConfig.rxConfig, failsafe);
#ifdef GPS
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
gpsInit( gpsInit(
&masterConfig.serialConfig, &masterConfig.serialConfig,
@ -223,6 +224,7 @@ void init(void)
&currentProfile.pidProfile &currentProfile.pidProfile
); );
} }
#endif
#ifdef SONAR #ifdef SONAR
if (feature(FEATURE_SONAR)) { if (feature(FEATURE_SONAR)) {

View file

@ -278,9 +278,11 @@ void annexCode(void)
handleSerial(); handleSerial();
#ifdef GPS
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime); updateGpsIndicator(currentTime);
} }
#endif
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature) if (gyro.temperature)
@ -409,6 +411,7 @@ void executePeriodicTasks(void)
break; break;
#endif #endif
#ifdef GPS
case UPDATE_GPS_TASK: case UPDATE_GPS_TASK:
// if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck
// hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will
@ -417,6 +420,7 @@ void executePeriodicTasks(void)
gpsThread(); gpsThread();
} }
break; break;
#endif
#ifdef SONAR #ifdef SONAR
case UPDATE_SONAR_TASK: case UPDATE_SONAR_TASK:
if (sensors(SENSOR_SONAR)) { if (sensors(SENSOR_SONAR)) {
@ -539,9 +543,11 @@ void processRx(void)
} }
#endif #endif
#ifdef GPS
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
updateGpsWaypointsAndMode(); updateGpsWaypointsAndMode();
} }
#endif
if (rcOptions[BOXPASSTHRU]) { if (rcOptions[BOXPASSTHRU]) {
f.PASSTHRU_MODE = 1; f.PASSTHRU_MODE = 1;
@ -615,11 +621,13 @@ void loop(void)
rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile.throttle_correction_value); rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile.throttle_correction_value);
} }
#ifdef GPS
if (sensors(SENSOR_GPS)) { if (sensors(SENSOR_GPS)) {
if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) { if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) {
updateGpsStateForHomeAndHoldMode(); updateGpsStateForHomeAndHoldMode();
} }
} }
#endif
// PID - note this is function pointer set by setPIDController() // PID - note this is function pointer set by setPIDController()
pid_controller( pid_controller(

View file

@ -39,5 +39,6 @@
#define BEEPER #define BEEPER
#define LED0 #define LED0
#define LED1 #define LED1
#define GPS
#define SENSORS_SET (SENSOR_ACC) #define SENSORS_SET (SENSOR_ACC)

View file

@ -40,6 +40,7 @@
#define BEEPER #define BEEPER
#define LED0 #define LED0
#define LED1 #define LED1
#define GPS
// #define SOFT_I2C // enable to test software i2c // #define SOFT_I2C // enable to test software i2c
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)

View file

@ -29,5 +29,6 @@
#define GYRO #define GYRO
#define ACC #define ACC
#define GPS
#define SENSORS_SET (SENSOR_ACC) #define SENSORS_SET (SENSOR_ACC)

View file

@ -39,6 +39,7 @@
#define BARO #define BARO
#define GYRO #define GYRO
#define MAG #define MAG
#define GPS
// #define SOFT_I2C // enable to test software i2c // #define SOFT_I2C // enable to test software i2c
// #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011) // #define SOFT_I2C_PB1011 // If SOFT_I2C is enabled above, need to define pinout as well (I2C1 = PB67, I2C2 = PB1011)

View file

@ -41,5 +41,6 @@
#define BEEPER #define BEEPER
#define LED0 #define LED0
#define LED1 #define LED1
#define GPS
#define SENSORS_SET (SENSOR_ACC) #define SENSORS_SET (SENSOR_ACC)

View file

@ -167,6 +167,7 @@ static void sendTime(void)
serialize16(seconds % 60); serialize16(seconds % 60);
} }
#ifdef GPS
// Frsky pdf: dddmm.mmmm // Frsky pdf: dddmm.mmmm
// .mmmm is returned in decimal fraction of minutes. // .mmmm is returned in decimal fraction of minutes.
static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
@ -200,6 +201,8 @@ static void sendGPS(void)
sendDataHead(ID_E_W); sendDataHead(ID_E_W);
serialize16(GPS_coord[LON] < 0 ? 'W' : 'E'); serialize16(GPS_coord[LON] < 0 ? 'W' : 'E');
} }
#endif
/* /*
* Send vertical speed for opentx. ID_VERT_SPEED * Send vertical speed for opentx. ID_VERT_SPEED
@ -362,8 +365,10 @@ void handleFrSkyTelemetry(void)
sendVoltageAmp(); sendVoltageAmp();
} }
#ifdef GPS
if (sensors(SENSOR_GPS)) if (sensors(SENSOR_GPS))
sendGPS(); sendGPS();
#endif
sendTelemetryTail(); sendTelemetryTail();
} }

View file

@ -105,13 +105,6 @@ static telemetryConfig_t *telemetryConfig;
static HOTT_GPS_MSG_t hottGPSMessage; static HOTT_GPS_MSG_t hottGPSMessage;
static HOTT_EAM_MSG_t hottEAMMessage; static HOTT_EAM_MSG_t hottEAMMessage;
typedef enum {
GPS_FIX_CHAR_NONE = '-',
GPS_FIX_CHAR_2D = '2',
GPS_FIX_CHAR_3D = '3',
GPS_FIX_CHAR_DGPS = 'D',
} gpsFixChar_e;
static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size) static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
{ {
memset(msg, 0, size); memset(msg, 0, size);
@ -121,6 +114,14 @@ static void initialiseEAMMessage(HOTT_EAM_MSG_t *msg, size_t size)
msg->stop_byte = 0x7D; msg->stop_byte = 0x7D;
} }
#ifdef GPS
typedef enum {
GPS_FIX_CHAR_NONE = '-',
GPS_FIX_CHAR_2D = '2',
GPS_FIX_CHAR_3D = '3',
GPS_FIX_CHAR_DGPS = 'D',
} gpsFixChar_e;
static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size) static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size)
{ {
memset(msg, 0, size); memset(msg, 0, size);
@ -129,13 +130,17 @@ static void initialiseGPSMessage(HOTT_GPS_MSG_t *msg, size_t size)
msg->sensor_id = HOTT_GPS_SENSOR_TEXT_ID; msg->sensor_id = HOTT_GPS_SENSOR_TEXT_ID;
msg->stop_byte = 0x7D; msg->stop_byte = 0x7D;
} }
#endif
static void initialiseMessages(void) static void initialiseMessages(void)
{ {
initialiseEAMMessage(&hottEAMMessage, sizeof(hottEAMMessage)); initialiseEAMMessage(&hottEAMMessage, sizeof(hottEAMMessage));
#ifdef GPS
initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage)); initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage));
#endif
} }
#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 / 10000000L;
@ -195,6 +200,7 @@ void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage)
hottGPSMessage->home_direction = GPS_directionToHome; hottGPSMessage->home_direction = GPS_directionToHome;
} }
#endif
static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage) static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage)
{ {
@ -282,7 +288,9 @@ static inline void hottSendEAMResponse(void)
static void hottPrepareMessages(void) { static void hottPrepareMessages(void) {
hottPrepareEAMResponse(&hottEAMMessage); hottPrepareEAMResponse(&hottEAMMessage);
#ifdef GPS
hottPrepareGPSResponse(&hottGPSMessage); hottPrepareGPSResponse(&hottGPSMessage);
#endif
} }
static void processBinaryModeRequest(uint8_t address) { static void processBinaryModeRequest(uint8_t address) {
@ -294,6 +302,7 @@ static void processBinaryModeRequest(uint8_t address) {
#endif #endif
switch (address) { switch (address) {
#ifdef GPS
case 0x8A: case 0x8A:
#ifdef HOTT_DEBUG #ifdef HOTT_DEBUG
hottGPSRequests++; hottGPSRequests++;
@ -302,6 +311,7 @@ static void processBinaryModeRequest(uint8_t address) {
hottSendGPSResponse(); hottSendGPSResponse();
} }
break; break;
#endif
case 0x8E: case 0x8E:
#ifdef HOTT_DEBUG #ifdef HOTT_DEBUG
hottEAMRequests++; hottEAMRequests++;
@ -314,7 +324,9 @@ static void processBinaryModeRequest(uint8_t address) {
#ifdef HOTT_DEBUG #ifdef HOTT_DEBUG
hottBinaryRequests++; hottBinaryRequests++;
debug[0] = hottBinaryRequests; debug[0] = hottBinaryRequests;
#ifdef GPS
debug[1] = hottGPSRequests; debug[1] = hottGPSRequests;
#endif
debug[2] = hottEAMRequests; debug[2] = hottEAMRequests;
#endif #endif