From 66fce423bbe75f60c813319e07b120798f0b4a0d Mon Sep 17 00:00:00 2001 From: Ben Hitchcock Date: Fri, 5 Sep 2014 11:28:55 +0800 Subject: [PATCH 1/2] Code allowing the use of sonar without requiring a barometer. This code has been flight tested on a Naze32 acro, with no barometer onboard. It also works when the target doesn't have BARO defined. --- src/main/config/runtime_config.h | 1 + src/main/flight/altitudehold.c | 20 +++++++++++-- src/main/flight/altitudehold.h | 1 + src/main/flight/imu.c | 10 ++++++- src/main/io/rc_controls.h | 2 +- src/main/io/serial_msp.c | 7 ++++- src/main/mw.c | 48 ++++++++++++++++++++++--------- src/main/sensors/initialisation.c | 2 +- 8 files changed, 72 insertions(+), 19 deletions(-) diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h index 676e924a2..74e2f9077 100644 --- a/src/main/config/runtime_config.h +++ b/src/main/config/runtime_config.h @@ -40,6 +40,7 @@ typedef enum { HEADFREE_MODE = (1 << 6), AUTOTUNE_MODE = (1 << 7), PASSTHRU_MODE = (1 << 8), + SONAR_MODE = (1 << 9), } flightModeFlags_e; extern uint16_t flightModeFlags; diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 7b818056a..f7af885c1 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -58,7 +58,7 @@ #include "config/config_profile.h" #include "config/config_master.h" -#ifdef BARO +#if defined(BARO) || defined(SONAR) static int16_t initialThrottleHold; static void multirotorAltHold(void) @@ -79,7 +79,7 @@ static void multirotorAltHold(void) rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle); } } else { - // slow alt changes for apfags + // slow alt changes, mostly used for aerial photography if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile->alt_hold_deadband) { // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2; @@ -128,4 +128,20 @@ void updateAltHoldState(void) } } +void updateSonarAltHoldState(void) +{ + // Sonar alt hold activate + if (rcOptions[BOXSONAR]) { + if (!FLIGHT_MODE(SONAR_MODE)) { + ENABLE_FLIGHT_MODE(SONAR_MODE); + AltHold = EstAlt; + initialThrottleHold = rcCommand[THROTTLE]; + errorVelocityI = 0; + BaroPID = 0; // TODO: Change naming of BaroPID to "AltPID" as this is used by both sonar and baro + } + } else { + DISABLE_FLIGHT_MODE(SONAR_MODE); + } +} + #endif diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index 93aa14a04..cf835a435 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -18,4 +18,5 @@ void updateAltHold(void); void updateAltHoldState(void); +void updateSonarAltHoldState(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 582041346..b8634d18f 100755 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -372,7 +372,8 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f))); } -#ifdef BARO +#if defined(BARO) || defined(SONAR) + // 40hz update rate (20hz LPF on acc) #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) @@ -453,6 +454,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) previousTime = currentTime; +#ifdef BARO if (!isBaroCalibrationComplete()) { performBaroCalibrationCycle(); vel = 0; @@ -460,6 +462,10 @@ void calculateEstimatedAltitude(uint32_t currentTime) } BaroAlt = baroCalculateAltitude(); +#else + BaroAlt = 0; +#endif + #ifdef SONAR tiltAngle = calculateTiltAngle(&inclination); @@ -496,9 +502,11 @@ void calculateEstimatedAltitude(uint32_t currentTime) accSum_reset(); +#ifdef BARO if (!isBaroCalibrationComplete()) { return; } +#endif if (sonarAlt > 0 && sonarAlt < 200) { // the sonar has the best range diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index c48076f37..a27092e04 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -22,7 +22,7 @@ enum { BOXANGLE, BOXHORIZON, BOXBARO, - // BOXVARIO, + BOXSONAR, BOXMAG, BOXHEADFREE, BOXHEADADJ, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 089ee70f1..2be0b4884 100755 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -149,7 +149,7 @@ static const box_t const boxes[] = { { BOXANGLE, "ANGLE;", 1 }, { BOXHORIZON, "HORIZON;", 2 }, { BOXBARO, "BARO;", 3 }, - //{ BOXVARIO, "VARIO;", 4 }, + { BOXSONAR, "SONAR;", 4 }, { BOXMAG, "MAG;", 5 }, { BOXHEADFREE, "HEADFREE;", 6 }, { BOXHEADADJ, "HEADADJ;", 7 }, @@ -421,6 +421,10 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXBARO; } + if (feature(FEATURE_SONAR)){ + activeBoxIds[activeBoxIdCount++] = BOXSONAR; + } + if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { activeBoxIds[activeBoxIdCount++] = BOXMAG; activeBoxIds[activeBoxIdCount++] = BOXHEADFREE; @@ -491,6 +495,7 @@ static bool processOutCommand(uint8_t cmdMSP) tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE | IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON | IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO | + IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR | IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG | IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ | diff --git a/src/main/mw.c b/src/main/mw.c index cdd230350..3df9a045a 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -375,10 +375,12 @@ typedef enum { #endif #ifdef BARO UPDATE_BARO_TASK, - CALCULATE_ALTITUDE_TASK, #endif #ifdef SONAR UPDATE_SONAR_TASK, +#endif +#if defined(BARO) || defined(SONAR) + CALCULATE_ALTITUDE_TASK, #endif UPDATE_GPS_TASK } periodicTasks; @@ -405,9 +407,20 @@ void executePeriodicTasks(void) baroUpdate(currentTime); } break; +#endif +#if defined(BARO) || defined(SONAR) case CALCULATE_ALTITUDE_TASK: + +#if defined(BARO) && !defined(SONAR) if (sensors(SENSOR_BARO) && isBaroReady()) { +#endif +#if defined(BARO) && defined(SONAR) + if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) { +#endif +#if !defined(BARO) && defined(SONAR) + if (sensors(SENSOR_SONAR)) { +#endif calculateEstimatedAltitude(currentTime); } break; @@ -548,7 +561,7 @@ void processRx(void) void loop(void) { static uint32_t loopTime; -#ifdef BARO +#if defined(BARO) || defined(SONAR) static bool haveProcessedAnnexCodeOnce = false; #endif @@ -558,13 +571,23 @@ void loop(void) processRx(); #ifdef BARO - // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. - if (haveProcessedAnnexCodeOnce) { - if (sensors(SENSOR_BARO)) { - updateAltHoldState(); - } - } + // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. + if (haveProcessedAnnexCodeOnce) { + if (sensors(SENSOR_BARO)) { + updateAltHoldState(); + } + } #endif + +#ifdef SONAR + // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. + if (haveProcessedAnnexCodeOnce) { + if (sensors(SENSOR_SONAR)) { + updateSonarAltHoldState(); + } + } +#endif + } else { // not processing rx this iteration executePeriodicTasks(); @@ -582,7 +605,7 @@ void loop(void) previousTime = currentTime; annexCode(); -#ifdef BARO +#if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif @@ -596,13 +619,12 @@ void loop(void) } #endif -#ifdef BARO - if (sensors(SENSOR_BARO)) { - if (FLIGHT_MODE(BARO_MODE)) { +#if defined(BARO) || defined(SONAR) + if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { + if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { updateAltHold(); } } - #endif if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 5de14deed..39787ad6c 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -363,8 +363,8 @@ static void detectBaro() return; } #endif - sensorsClear(SENSOR_BARO); #endif + sensorsClear(SENSOR_BARO); } void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) From 004b158af3e3f47406677bd69a168bc491231968 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 26 Sep 2014 13:37:35 +0100 Subject: [PATCH 2/2] Bump config version for new SONAR flight mode. --- src/main/config/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 197edd9d3..b90e49e5e 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -99,7 +99,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon master_t masterConfig; // master config struct with data independent from profiles profile_t *currentProfile; // profile config struct -static const uint8_t EEPROM_CONF_VERSION = 79; +static const uint8_t EEPROM_CONF_VERSION = 80; static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims) {