Support for altitude hold based on barometer

PCB/README.md

@@ -0,0 +1,5 @@
+# LaunchPad Flight Controller
+#### Developed by Kristian Sloth Lauszus, 2015
+_________
+
+This is the design files for the PCB that is designed to plug onto the headers on the Tiva LaunchPad. You can order the PCB directly from the following link: <https://www.oshpark.com/shared_projects/9DC5cp8e>.

README.md

@@ -23,7 +23,7 @@ The report I wrote for my Bachelor's these can be found in the [docs](docs) fold
 
 * Rate mode, self level mode, heading hold and altitude hold
     - __AUX1:__ Use 3-POS switch for self level and heading hold. At first position both are off, at second position self level is on and at third position both are on
-    - __AUX2:__ Use a 2-POS switch for altitude hold. Activated when switch is high. Note that self level mode must be activated for altitude hold to work
+    - __AUX2:__ Use a 3-POS switch for altitude hold. Note that self level mode must be activated for altitude hold to work! At first position altitude hold is turned off, at second position altitude hold will use the distance measured using the sonar and at the third position altitude hold will be using the altitude estimated using the barometer and accelerometer
 * Store PID values, calibration values etc. in EEPROM
 * Gyro, accelerometer & magnetometer calibration routine
     - Gyro is calibrated at startup
@@ -71,7 +71,9 @@ The report I wrote for my Bachelor's these can be found in the [docs](docs) fold
 | PD2  |          Buzzer         |
 | PE3  |       HMC5883L DRDY     |
 
-\* UART1 is connected to a HC-06 Bluetooth module running at a baudrate of 115200. __Not 5V tolerant!__, so make sure your Bluetooth module outputs 3.3 voltage level or use logic level converter.
+\* UART1 is connected to a HC-06 Bluetooth module running at a baudrate of 115200. __Not 5V tolerant!__, so make sure your Bluetooth module outputs 3.3 voltage level or use a logic level converter.
+
+The MPU-6500/MPU-9250, HMC5883L, BMP180 are connected via I2C if they are used.
 
 # Notes
 

docs/README.md

@@ -0,0 +1,5 @@
+# LaunchPad Flight Controller
+#### Developed by Kristian Sloth Lauszus, 2015
+_________
+
+This is the report I wrote for my bachelor thesis including the appendix I attached as a [CD-ROM](CD-ROM).

src/AltitudeHold.c

@@ -46,6 +46,7 @@
 
 #if USE_BARO
 static bmp180_t bmp180; // Barometer readings
+static float altitudeSetPoint;
 #endif
 
 static bool altHoldActive;
@@ -117,7 +118,7 @@ void getAltitude(angle_t *angle, mpu6500_t *mpu6500, altitude_t *altitude, uint3
 #if USE_SONAR && 1
     // TODO: Add smooth transaction between sonar and barometer
     static float altitudeBaroOffset;
-    if (altitude->sonarDistance > 0 && altitude->sonarDistance < /*3000*/100) {
+    if (altitude->sonarDistance > 0 && altitude->sonarDistance < /*2000*/100) {
         float sonarHeight = (float)altitude->sonarDistance / 10.0f; // Convert sonar distance to cm
         altitudeBaroOffset = baroAltitude - sonarHeight; // Set altitude offset
         baroAltitude = sonarHeight; // Set altitude estimate equal to sonar height
@@ -154,15 +155,16 @@ void getAltitude(angle_t *angle, mpu6500_t *mpu6500, altitude_t *altitude, uint3
 #endif // USE_BARO
 }
 
-float updateAltitudeHold(altitude_t *altitude, float throttle, uint32_t __attribute__((unused)) now, float dt) {
+float updateAltitudeHold(float aux, altitude_t *altitude, float throttle, uint32_t __attribute__((unused)) now, float dt) {
+    static const float MIN_MOTOR_OFFSET = (MAX_MOTOR_OUT - MIN_MOTOR_OUT) * 0.05f; // Add 5% to minimum, so the motors are never completely shut off
+    static float altHoldInitialThrottle = -30.0f; // Throttle when altitude hold was activated
+
 #if USE_SONAR
     static const float throttle_noise_lpf = 1000.0f; // TODO: Set via app
     static float altHoldThrottle; // Low pass filtered throttle input
-    static float altHoldInitialThrottle; // Throttle when altitude hold was activated
     static int16_t altHoldSetPoint; // Altitude hold set point
 
-    // TODO: Use barometer when it exceeds 3m
-    if (altitude->sonarDistance >= 0) { // Make sure the distance is valid
+    if (aux < 60 && altitude->sonarDistance >= 0/* && altitude->sonarDistance > 2000*/) { // Make sure the distance is valid
         if (!altHoldActive) { // We just went from deactivated to active
             altHoldActive = true;
             resetPIDAltHold();
@@ -200,20 +202,38 @@ float updateAltitudeHold(altitude_t *altitude, float throttle, uint32_t __attrib
         setPoint = mapf(altHoldThrottle, MIN_MOTOR_OUT, MAX_MOTOR_OUT, SONAR_MIN_DIST, SONAR_MAX_DIST);
 #endif
 
-        float altHoldOut = updatePID(&pidAltHold, setPoint, altitude->sonarDistance, dt);
-        static const float MIN_MOTOR_OFFSET = (MAX_MOTOR_OUT - MIN_MOTOR_OUT) * 0.05f; // Add 5% to minimum, so the motors are never completely shut off
+        float altHoldOut = updatePID(&pidSonarAltHold, setPoint, altitude->sonarDistance, dt);
         throttle = constrain(altHoldInitialThrottle + altHoldOut, MIN_MOTOR_OUT + MIN_MOTOR_OFFSET, MAX_MOTOR_OUT); // Throttle value is set to throttle when altitude hold were first activated plus output from PID controller
         /*UARTprintf("%u %d %d %d - %d %d %d %d\n", altHoldActive, (int32_t)altHoldThrottle, (int32_t)altHoldInitialThrottle, altHoldSetPoint,     (int32_t)setPoint, altitude->sonarDistance, (int32_t)altHoldOut, (int32_t)throttle);
         UARTFlushTx(false);*/
-    } else
-        buzzer(true); // Turn on buzzer in case sonar sensor return an error
+    }
+#if USE_BARO
+    else
+#endif // USE_BARO
 #endif // USE_SONAR
 
+#if USE_BARO
+    if (aux > 60/* && altitude->sonarDistance > 2000*/) {
+        if (!altHoldActive) { // We just went from deactivated to active
+            altHoldActive = true;
+            altHoldInitialThrottle = throttle; // Save current throttle
+            resetPIDAltHold();
+        }
+        float altHoldOut = updatePID(&pidBaroAltHold, altitudeSetPoint * 10.0f, altitude->altitudeLpf * 10.0f, dt); // Multiply by 10 in order to convert it from cm to mm
+        throttle = constrain(altHoldInitialThrottle + altHoldOut, MIN_MOTOR_OUT + MIN_MOTOR_OFFSET, MAX_MOTOR_OUT); // Throttle value is set to throttle when altitude hold were first activated plus output from PID controller
+        /*UARTprintf1("%d %d %d %d %d\n", (int32_t)altitudeSetPoint, (int32_t)altitude->altitudeLpf, (int32_t)altHoldInitialThrottle, (int32_t)altHoldOut, (int32_t)throttle);
+        UARTFlushTx1(false);*/
+    }
+#endif // USE_BARO
+
     return throttle;
 }
 
-void resetAltitudeHold(void) {
+void resetAltitudeHold(altitude_t *altitude) {
     altHoldActive = false;
+#if USE_BARO
+    altitudeSetPoint = altitude->altitudeLpf;
+#endif // USE_BARO
 }
 
 #endif // USE_SONAR || USE_BARO

src/AltitudeHold.h

@@ -36,8 +36,8 @@ typedef struct {
 
 void initAltitudeHold(void);
 void getAltitude(angle_t *angle, mpu6500_t *mpu6500, altitude_t *altitude, uint32_t now, float dt);
-float updateAltitudeHold(altitude_t *altitude, float throttle, uint32_t now, float dt);
-void resetAltitudeHold(void);
+float updateAltitudeHold(float aux, altitude_t *altitude, float throttle, uint32_t now, float dt);
+void resetAltitudeHold(altitude_t *altitude);
 
 #ifdef __cplusplus
 }

src/Bluetooth.c

@@ -46,8 +46,10 @@ enum {
     GET_PID_ROLL_PITCH,
     SET_PID_YAW,
     GET_PID_YAW,
-    SET_PID_ALT_HOLD,
-    GET_PID_ALT_HOLD,
+    SET_PID_SONAR_ALT_HOLD,
+    GET_PID_SONAR_ALT_HOLD,
+    SET_PID_BARO_ALT_HOLD,
+    GET_PID_BARO_ALT_HOLD,
     SET_SETTINGS,
     GET_SETTINGS,
     SEND_ANGLES,
@@ -121,7 +123,8 @@ bool readBluetoothData(mpu6500_t *mpu6500, angle_t *angle) {
             switch (msg.cmd) {
                 case SET_PID_ROLL_PITCH:
                 case SET_PID_YAW:
-                case SET_PID_ALT_HOLD:
+                case SET_PID_SONAR_ALT_HOLD:
+                case SET_PID_BARO_ALT_HOLD:
                     if (msg.length == sizeof(pid_values_bt_t)) { // Make sure that it has the right length
                         pid_values_t *pidValues = getPidValuesPointer(msg.cmd);
                         if (!pidValues)
@@ -152,7 +155,8 @@ bool readBluetoothData(mpu6500_t *mpu6500, angle_t *angle) {
 
                 case GET_PID_ROLL_PITCH:
                 case GET_PID_YAW:
-                case GET_PID_ALT_HOLD:
+                case GET_PID_SONAR_ALT_HOLD:
+                case GET_PID_BARO_ALT_HOLD:
                     if (msg.length == 0 && getData(msg, NULL)) { // Check length and the checksum
                         pid_values_t *pidValues = getPidValuesPointer(msg.cmd);
                         if (!pidValues)
@@ -395,8 +399,10 @@ static pid_values_t* getPidValuesPointer(uint8_t cmd) {
         return &cfg.pidRollPitchValues;
     else if (cmd == SET_PID_YAW || cmd == GET_PID_YAW)
         return &cfg.pidYawValues;
-    else if (cmd == SET_PID_ALT_HOLD || cmd == GET_PID_ALT_HOLD)
-        return &cfg.pidAltHoldValues;
+    else if (cmd == SET_PID_SONAR_ALT_HOLD || cmd == GET_PID_SONAR_ALT_HOLD)
+        return &cfg.pidSonarAltHoldValues;
+    else if (cmd == SET_PID_BARO_ALT_HOLD || cmd == GET_PID_BARO_ALT_HOLD)
+        return &cfg.pidBaroAltHoldValues;
 
     return NULL;
 }

src/EEPROM.c

@@ -27,7 +27,7 @@
 #include "utils/uartstdio.h" // Add "UART_BUFFERED" to preprocessor
 #endif
 
-static const uint32_t configVersion = 16; // Must be bumped every time config_t is changed
+static const uint32_t configVersion = 17; // Must be bumped every time config_t is changed
 config_t cfg;
 
 void initEEPROM(void) {
@@ -72,10 +72,15 @@ void setDefaultConfig(void) {
     cfg.pidYawValues.Kd = 0.00040f;
     cfg.pidYawValues.integrationLimit = 10.0f; // Prevent windup
 
-    cfg.pidAltHoldValues.Kp = 0.040f;
-    cfg.pidAltHoldValues.Ki = 0.03f;
-    cfg.pidAltHoldValues.Kd = 0.00330f;
-    cfg.pidAltHoldValues.integrationLimit = 10.0f; // Prevent windup
+    cfg.pidSonarAltHoldValues.Kp = 0.040f;
+    cfg.pidSonarAltHoldValues.Ki = 0.03f;
+    cfg.pidSonarAltHoldValues.Kd = 0.00330f;
+    cfg.pidSonarAltHoldValues.integrationLimit = 10.0f; // Prevent windup
+
+    cfg.pidBaroAltHoldValues.Kp = 0.006f;
+    cfg.pidBaroAltHoldValues.Ki = 0.00f;
+    cfg.pidBaroAltHoldValues.Kd = 0.00450f;
+    cfg.pidBaroAltHoldValues.integrationLimit = 10.0f; // Prevent windup
 
     resetPIDRollPitchYaw();
     resetPIDAltHold();

src/EEPROM.h

@@ -28,7 +28,7 @@ extern "C" {
 typedef struct {
     // Can be set by the user
     pid_values_t pidRollPitchValues, pidYawValues; // Use same PID values for both pitch and roll, but different values for yaw
-    pid_values_t pidAltHoldValues; // PID values for altitude hold
+    pid_values_t pidSonarAltHoldValues, pidBaroAltHoldValues; // PID values for altitude hold
     float angleKp; // Self level mode Kp value
     float headKp; // Heading mode Kp value
     uint8_t maxAngleInclination; // Max angle in self level mode

src/Makefile

@@ -106,7 +106,9 @@ ENTRY_$(TARGET) = ResetISR
 #
 # CFLAGS passed to the compiler.
 #
-CFLAGSgcc = -DTARGET_IS_BLIZZARD_RB1 -DUART_BUFFERED -DUART_BUFFERED1 -DUART_DEBUG=$(UART_DEBUG) -DUSE_MAG=$(USE_MAG) -DUSE_BARO=$(USE_BARO) -DUSE_SONAR=$(USE_SONAR) -DSTEP_ACRO_SELF_LEVEL=$(STEP_ACRO_SELF_LEVEL) -DSTEP_ALTITUDE_HOLD=$(STEP_ALTITUDE_HOLD) -DSTEP_HEADING_HOLD=$(STEP_HEADING_HOLD)
+CFLAGSgcc =  -DTARGET_IS_BLIZZARD_RB1 -DUART_BUFFERED -DUART_BUFFERED1 -DUART_DEBUG=$(UART_DEBUG)
+CFLAGSgcc += -DUSE_MAG=$(USE_MAG) -DUSE_BARO=$(USE_BARO) -DUSE_SONAR=$(USE_SONAR)
+CFLAGSgcc += -DSTEP_ACRO_SELF_LEVEL=$(STEP_ACRO_SELF_LEVEL) -DSTEP_ALTITUDE_HOLD=$(STEP_ALTITUDE_HOLD) -DSTEP_HEADING_HOLD=$(STEP_HEADING_HOLD)
 CFLAGSgcc += -Wdouble-promotion -Wunsafe-loop-optimizations -Wunused-parameter
 
 #

src/PID.c

@@ -21,14 +21,15 @@
 #include "EEPROM.h"
 #include "PID.h"
 
-pid_t pidRoll, pidPitch, pidYaw, pidAltHold;
+pid_t pidRoll, pidPitch, pidYaw, pidSonarAltHold, pidBaroAltHold;
 
 void initPID(void) {
     // Set PID values to point at values read from EEPROM
     pidRoll.values = &cfg.pidRollPitchValues;
     pidPitch.values = &cfg.pidRollPitchValues;
     pidYaw.values = &cfg.pidYawValues;
-    pidAltHold.values = &cfg.pidAltHoldValues;
+    pidSonarAltHold.values = &cfg.pidSonarAltHoldValues;
+    pidBaroAltHold.values = &cfg.pidBaroAltHoldValues;
 }
 
 float updatePID(pid_t *pid, float setPoint, float input, float dt) {
@@ -61,5 +62,6 @@ void resetPIDRollPitchYaw(void) {
 }
 
 void resetPIDAltHold(void) {
-    pidAltHold.iTerm = pidAltHold.lastError = pidAltHold.deltaError1 = pidAltHold.deltaError2 = 0.0f;
+    pidSonarAltHold.iTerm = pidSonarAltHold.lastError = pidSonarAltHold.deltaError1 = pidSonarAltHold.deltaError2 = 0.0f;
+    pidBaroAltHold.iTerm = pidBaroAltHold.lastError = pidBaroAltHold.deltaError1 = pidBaroAltHold.deltaError2 = 0.0f;
 }

src/PID.h

@@ -41,7 +41,7 @@ float updatePID(pid_t *pid, float setPoint, float input, float dt);
 void resetPIDRollPitchYaw(void);
 void resetPIDAltHold(void);
 
-extern pid_t pidRoll, pidPitch, pidYaw, pidAltHold;
+extern pid_t pidRoll, pidPitch, pidYaw, pidSonarAltHold, pidBaroAltHold;
 
 #ifdef __cplusplus
 }

src/main.c

@@ -94,7 +94,8 @@ int main(void) {
 #if UART_DEBUG
     printPIDValues(pidRoll.values); // Print PID Values
     printPIDValues(pidYaw.values);
-    printPIDValues(pidAltHold.values);
+    printPIDValues(pidSonarAltHold.values);
+    printPIDValues(pidBaroAltHold.values);
     printSettings(); // Print settings
 #endif
 
@@ -225,9 +226,9 @@ int main(void) {
 
 #if USE_SONAR || USE_BARO
                 if (altitudeMode)
-                    throttle = updateAltitudeHold(&altitude, throttle, now, dt);
+                    throttle = updateAltitudeHold(getRXChannel(RX_AUX2_CHAN), &altitude, throttle, now, dt);
                 else
-                    resetAltitudeHold();
+                    resetAltitudeHold(&altitude);
 #endif
 
                 float motors[4]; // Motor 0 is bottom right, motor 1 is top right, motor 2 is bottom left and motor 3 is top left
@@ -263,7 +264,7 @@ int main(void) {
                 writePPMAllOff();
                 resetPIDRollPitchYaw();
 #if USE_SONAR || USE_BARO
-                resetAltitudeHold();
+                resetAltitudeHold(&altitude);
 #endif
 #if USE_MAG
                 resetHeadingHold(&angle);