From 34cd8f466eb01430eaea0c60e6d71ad2f38a3fd2 Mon Sep 17 00:00:00 2001 From: Krzysztof Rosinski Date: Wed, 28 Jan 2015 22:43:37 +0100 Subject: [PATCH] MSP command for sonar altitude --- src/main/flight/altitudehold.c | 12 +++++--- src/main/flight/altitudehold.h | 10 ++++--- src/main/flight/flight.h | 1 - src/main/io/rc_controls.h | 2 ++ src/main/io/serial.c | 1 - src/main/io/serial_msp.c | 52 ++++++++++++++++++++++------------ src/main/io/serial_msp.h | 5 ++++ src/main/main.c | 2 +- src/main/mw.c | 2 +- src/main/sensors/sonar.c | 26 +++++++++++------ src/main/sensors/sonar.h | 7 ++--- 11 files changed, 78 insertions(+), 42 deletions(-) diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index 5fa40d624..bfd7be91d 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -53,7 +53,6 @@ uint8_t velocityControl = 0; int32_t errorVelocityI = 0; int32_t altHoldThrottleAdjustment = 0; int32_t AltHold; -int32_t EstAlt; // in cm int32_t vario = 0; // variometer in cm/s @@ -78,7 +77,7 @@ void configureAltitudeHold( #if defined(BARO) || defined(SONAR) static int16_t initialThrottleHold; - +static int32_t EstAlt; // in cm // 40hz update rate (20hz LPF on acc) #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25) @@ -230,6 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) float vel_acc; int32_t vel_tmp; float accZ_tmp; + int32_t sonarAlt = -1; static float accZ_old = 0.0f; static float vel = 0.0f; static float accAlt = 0.0f; @@ -242,8 +242,6 @@ void calculateEstimatedAltitude(uint32_t currentTime) int16_t tiltAngle; #endif - - dTime = currentTime - previousTime; if (dTime < BARO_UPDATE_FREQUENCY_40HZ) return; @@ -329,5 +327,11 @@ void calculateEstimatedAltitude(uint32_t currentTime) accZ_old = accZ_tmp; } + +int32_t altitudeHoldGetEstimatedAltitude(void) +{ + return EstAlt; +} + #endif diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h index fefbb7582..1e42c65ef 100644 --- a/src/main/flight/altitudehold.h +++ b/src/main/flight/altitudehold.h @@ -15,14 +15,16 @@ * along with Cleanflight. If not, see . */ -//extern int32_t errorVelocityI; -//extern int32_t altHoldThrottleAdjustment; -//extern uint8_t velocityControl; -//extern int32_t setVelocity; +#include "flight/flight.h" +#include "io/escservo.h" +#include "io/rc_controls.h" + +#include "sensors/barometer.h" void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig); void applyAltHold(airplaneConfig_t *airplaneConfig); void updateAltHoldState(void); void updateSonarAltHoldState(void); +int32_t altitudeHoldGetEstimatedAltitude(void); diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 95877badf..51fd74a0a 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -111,7 +111,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; extern int16_t heading, magHold; extern int32_t AltHold; -extern int32_t EstAlt; extern int32_t vario; void setPIDController(int type); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 8441ea90f..a1c2a10a5 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -17,6 +17,8 @@ #pragma once +#include "rx/rx.h" + typedef enum { BOXARM = 0, BOXANGLE, diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 397d4ee74..56fec19a9 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -44,7 +44,6 @@ void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate); -void mspInit(serialConfig_t *serialConfig); void cliInit(serialConfig_t *serialConfig); // this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask. diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 739cef67d..31ab095ac 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -45,6 +45,8 @@ #include "flight/mixer.h" #include "flight/failsafe.h" #include "flight/navigation.h" +#include "flight/altitudehold.h" + #include "rx/rx.h" #include "rx/msp.h" #include "io/escservo.h" @@ -54,9 +56,11 @@ #include "io/serial.h" #include "io/ledstrip.h" #include "telemetry/telemetry.h" + #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" +#include "sensors/sonar.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" @@ -249,6 +253,7 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; #define MSP_SERVO_CONF 120 //out message Servo settings #define MSP_NAV_STATUS 121 //out message Returns navigation status #define MSP_NAV_CONFIG 122 //out message Returns navigation parameters +#define MSP_SONAR_ALTITUDE 123 //out message Returns sonar altitude [cm] #define MSP_SET_RAW_RC 200 //in message 8 rc chan #define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed @@ -368,7 +373,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT]; static mspPort_t *currentPort; -void serialize32(uint32_t a) +static void serialize32(uint32_t a) { static uint8_t t; t = a; @@ -385,7 +390,7 @@ void serialize32(uint32_t a) currentPort->checksum ^= t; } -void serialize16(int16_t a) +static void serialize16(int16_t a) { static uint8_t t; t = a; @@ -396,32 +401,32 @@ void serialize16(int16_t a) currentPort->checksum ^= t; } -void serialize8(uint8_t a) +static void serialize8(uint8_t a) { serialWrite(mspSerialPort, a); currentPort->checksum ^= a; } -uint8_t read8(void) +static uint8_t read8(void) { return currentPort->inBuf[currentPort->indRX++] & 0xff; } -uint16_t read16(void) +static uint16_t read16(void) { uint16_t t = read8(); t += (uint16_t)read8() << 8; return t; } -uint32_t read32(void) +static uint32_t read32(void) { uint32_t t = read16(); t += (uint32_t)read16() << 16; return t; } -void headSerialResponse(uint8_t err, uint8_t s) +static void headSerialResponse(uint8_t err, uint8_t s) { serialize8('$'); serialize8('M'); @@ -431,36 +436,36 @@ void headSerialResponse(uint8_t err, uint8_t s) serialize8(currentPort->cmdMSP); } -void headSerialReply(uint8_t s) +static void headSerialReply(uint8_t s) { headSerialResponse(0, s); } -void headSerialError(uint8_t s) +static void headSerialError(uint8_t s) { headSerialResponse(1, s); } -void tailSerialReply(void) +static void tailSerialReply(void) { serialize8(currentPort->checksum); } -void s_struct(uint8_t *cb, uint8_t siz) +static void s_struct(uint8_t *cb, uint8_t siz) { headSerialReply(siz); while (siz--) serialize8(*cb++); } -void serializeNames(const char *s) +static void serializeNames(const char *s) { const char *c; for (c = s; *c; c++) serialize8(*c); } -const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) +static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) { uint8_t boxIndex; const box_t *candidate; @@ -473,7 +478,7 @@ const box_t *findBoxByActiveBoxId(uint8_t activeBoxId) return NULL; } -const box_t *findBoxByPermenantId(uint8_t permenantId) +static const box_t *findBoxByPermenantId(uint8_t permenantId) { uint8_t boxIndex; const box_t *candidate; @@ -486,7 +491,7 @@ const box_t *findBoxByPermenantId(uint8_t permenantId) return NULL; } -void serializeBoxNamesReply(void) +static void serializeBoxNamesReply(void) { int i, activeBoxId, j, flag = 1, count = 0, len; const box_t *box; @@ -625,7 +630,7 @@ void mspInit(serialConfig_t *serialConfig) activeBoxIds[activeBoxIdCount++] = BOXLEDLOW; } #endif - + if (feature(FEATURE_INFLIGHT_ACC_CAL)) activeBoxIds[activeBoxIdCount++] = BOXCALIB; @@ -652,7 +657,6 @@ static bool processOutCommand(uint8_t cmdMSP) { uint32_t i, tmp, junk; - #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0; @@ -821,9 +825,21 @@ static bool processOutCommand(uint8_t cmdMSP) break; case MSP_ALTITUDE: headSerialReply(6); - serialize32(EstAlt); +#if defined(BARO) || defined(SONAR) + serialize32(altitudeHoldGetEstimatedAltitude()); +#else + serialize32(0); +#endif serialize16(vario); break; + case MSP_SONAR_ALTITUDE: + headSerialReply(4); +#if defined(SONAR) + serialize32(sonarGetLatestAltitude()); +#else + serialize32(0); +#endif + break; case MSP_ANALOG: headSerialReply(7); serialize8((uint8_t)constrain(vbat, 0, 255)); diff --git a/src/main/io/serial_msp.h b/src/main/io/serial_msp.h index 80abd0213..fd26e9981 100644 --- a/src/main/io/serial_msp.h +++ b/src/main/io/serial_msp.h @@ -17,9 +17,14 @@ #pragma once +#include "io/serial.h" +#include "drivers/serial.h" + // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports. #define MAX_MSP_PORT_COUNT 2 +void mspInit(serialConfig_t *serialConfig); + void mspProcess(void); void sendMspTelemetry(void); void mspSetTelemetryPort(serialPort_t *mspTelemetryPort); diff --git a/src/main/main.c b/src/main/main.c index 3e1052b54..3b1a33e8c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -332,7 +332,7 @@ void init(void) #ifdef SONAR if (feature(FEATURE_SONAR)) { - Sonar_init(); + sonarInit(); } #endif diff --git a/src/main/mw.c b/src/main/mw.c index 12425863a..0d542311c 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -469,7 +469,7 @@ void executePeriodicTasks(void) #ifdef SONAR case UPDATE_SONAR_TASK: if (sensors(SENSOR_SONAR)) { - Sonar_update(); + sonarUpdate(); } break; #endif diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 518cb5605..842df6404 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -33,11 +33,13 @@ #include "sensors/sensors.h" #include "sensors/sonar.h" -int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range - inclination adjusted by imu +// in cm , -1 indicate sonar is not in range - inclination adjusted by imu #ifdef SONAR -void Sonar_init(void) +static int32_t calculatedAltitude; + +void sonarInit(void) { #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R) static const sonarHardware_t const sonarPWM56 = { @@ -74,25 +76,33 @@ void Sonar_init(void) #endif sensorsSet(SENSOR_SONAR); - sonarAlt = 0; + calculatedAltitude = -1; } -void Sonar_update(void) +void sonarUpdate(void) { hcsr04_start_reading(); } +int32_t sonarRead(void) +{ + return hcsr04_get_distance(); +} + int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle) { // calculate sonar altitude only if the sonar is facing downwards(<25deg) if (tiltAngle > 250) - return -1; + calculatedAltitude = -1; + else + calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f; - return sonarAlt * (900.0f - tiltAngle) / 900.0f; + return calculatedAltitude; } -int32_t sonarRead(void) { - return hcsr04_get_distance(); +int32_t sonarGetLatestAltitude(void) +{ + return calculatedAltitude; } #endif diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index a6a2a8b94..3b8674e42 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -17,10 +17,9 @@ #pragma once -extern int32_t sonarAlt; - -void Sonar_init(void); -void Sonar_update(void); +void sonarInit(void); +void sonarUpdate(void); int32_t sonarRead(void); int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle); +int32_t sonarGetLatestAltitude(void);