#102 Refactoring

Extracted code into position estimator, position controller and attitude controller

Makefile

@@ -144,7 +144,7 @@ PROJ_OBJ_CF2 += libdw1000.o libdw1000Spi.o
 
 # Modules
 PROJ_OBJ += system.o comm.o console.o pid.o crtpservice.o param.o mem.o
-PROJ_OBJ += commander.o controller.o sensfusion6.o stabilizer.o altitudehold.o
+PROJ_OBJ += commander.o attitude_pid_controller.o sensfusion6.o stabilizer.o position_estimator_altitude.o position_controller_altitude.o
 PROJ_OBJ += log.o worker.o trigger.o sitaw.o queuemonitor.o
 PROJ_OBJ_CF1 += sound_cf1.o
 PROJ_OBJ_CF2 += platformservice.o sound_cf2.o extrx.o

src/modules/interface/controller.h → src/modules/interface/attitude_controller.h

@@ -21,25 +21,26 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
+ * attitude_controller.h: PID-based attitude controller
  */
 
-#ifndef CONTROLLER_H_
-#define CONTROLLER_H_
+#ifndef ATTITUDE_CONTROLLER_H_
+#define ATTITUDE_CONTROLLER_H_
 
 #include <stdbool.h>
 #include "commander.h"
 
 
-void controllerInit(void);
-bool controllerTest(void);
+void attitudeControllerInit(void);
+bool attitudeControllerTest(void);
 
 /**
  * Make the controller run an update of the attitude PID. The output is
  * the desired rate which should be fed into a rate controller. The
  * attitude controller can be run in a slower update rate then the rate
  * controller.
  */
-void controllerCorrectAttitudePID(
+void attitudeControllerCorrectAttitudePID(
        float eulerRollActual, float eulerPitchActual, float eulerYawActual,
        float eulerRollDesired, float eulerPitchDesired, float eulerYawDesired,
        float* rollRateDesired, float* pitchRateDesired, float* yawRateDesired);
@@ -48,19 +49,19 @@ void controllerCorrectAttitudePID(
  * Make the controller run an update of the rate PID. The output is
  * the actuator force.
  */
-void controllerCorrectRatePID(
+void attitudeControllerCorrectRatePID(
        float rollRateActual, float pitchRateActual, float yawRateActual,
        float rollRateDesired, float pitchRateDesired, float yawRateDesired);
 
 /**
  * Reset controller roll, pitch and yaw PID's.
  */
-void controllerResetAllPID(void);
+void attitudeControllerResetAllPID(void);
 
 /**
  * Get the actuator output.
  */
-void controllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw);
+void attitudeControllerGetActuatorOutput(int16_t* roll, int16_t* pitch, int16_t* yaw);
 
 
-#endif /* CONTROLLER_H_ */
+#endif /* ATTITUDE_CONTROLLER_H_ */

src/modules/interface/position_controller.h

@@ -0,0 +1,33 @@
+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2016 Bitcraze AB
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, in version 3.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ *
+ */
+#ifndef POSITION_CONTROLLER_H_
+#define POSITION_CONTROLLER_H_
+
+#include "stabilizer_types.h"
+
+void positionControllerUpdate(uint16_t* actuatorThrust, const estimate_t* estimate, float dt);
+
+#endif /* POSITION_CONTROLLER_H_ */

src/modules/interface/altitudehold.h → src/modules/interface/position_estimator.h

@@ -23,9 +23,11 @@
  *
  *
  */
-#ifndef ALTITUDE_HOLD_H_
-#define ALTITUDE_HOLD_H_
+#ifndef POSITION_ESTIMATOR_H_
+#define POSITION_ESTIMATOR_H_
 
-void altHoldUpdate(uint16_t* actuatorThrust, float asl, float velocityZ, float dt);
+#include "stabilizer_types.h"
 
-#endif /* ALTITUDE_HOLD_H_ */
+void positionEstimate(estimate_t* estimate, float asl, float velocityZ, float dt);
+
+#endif /* POSITION_ESTIMATOR_H_ */

src/modules/interface/stabilizer.h

@@ -21,12 +21,13 @@
  * You should have received a copy of the GNU General Public License
  * along with this program. If not, see <http://www.gnu.org/licenses/>.
  *
- *
+ * stabilizer.h: Stabilizer orchestrator
  */
 #ifndef STABALIZER_H_
 #define STABALIZER_H_
 
 #include <stdbool.h>
+#include <stdint.h>
 
 void stabilizerInit(void);
 

src/modules/interface/stabilizer_types.h

@@ -70,4 +70,11 @@ typedef struct quaternion_s {
   };
 } quaternion_t;
 
+/** Estimate of position */
+typedef struct estimate_s {
+  uint32_t timestamp; // Timestamp when the data was computed
+
+  point_t position;
+} estimate_t;
+
 #endif