Merge branch 'glick_dev_branch' into 'master'

Added student controller

See merge request !62

.vscode/settings.json

+{
+    "files.associations": {
+        "controller.h": "c",
+        "typeinfo": "c"
+    }
+}
\ No newline at end of file

crazyflie_software/crazyflie-firmware-2021.06/Makefile

@@ -178,6 +178,7 @@ PROJ_OBJ += attitude_pid_controller.o sensfusion6.o stabilizer.o
 PROJ_OBJ += position_estimator_altitude.o position_controller_pid.o position_controller_indi.o
 PROJ_OBJ += estimator.o estimator_complementary.o
 PROJ_OBJ += controller.o controller_pid.o controller_mellinger.o controller_indi.o
+PROJ_OBJ += controller_student.o
 PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o
 PROJ_OBJ += collision_avoidance.o health.o
 

crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/controller.h

@@ -33,6 +33,7 @@ typedef enum {
   ControllerTypePID,
   ControllerTypeMellinger,
   ControllerTypeINDI,
+  ControllerTypeStudent,
   ControllerType_COUNT,
 } ControllerType;
 

crazyflie_software/crazyflie-firmware-2021.06/src/modules/interface/controller_student.h

+/**
+ *    ||          ____  _ __
+ * +------+      / __ )(_) /_______________ _____  ___
+ * | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+ * +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+ *  ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+ *
+ * Crazyflie control firmware
+ *
+ * Copyright (C) 2011-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/>.
+ *
+ * controller_student.h - PID Controller Interface
+ * copied from controller_pid.h to start
+ */
+#ifndef __CONTROLLER_STUDENT_H__
+#define __CONTROLLER_STUDENT_H__
+
+#include "stabilizer_types.h"
+
+void controllerStudentInit(void);
+bool controllerStudentTest(void);
+void controllerStudent(control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick);
+
+#endif //__CONTROLLER_STUDENT_H__

crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller.c

@@ -6,6 +6,7 @@
 #include "controller_pid.h"
 #include "controller_mellinger.h"
 #include "controller_indi.h"
+#include "controller_student.h"
 
 #define DEFAULT_CONTROLLER ControllerTypePID
 static ControllerType currentController = ControllerTypeAny;
@@ -24,6 +25,7 @@ static ControllerFcns controllerFunctions[] = {
   {.init = controllerPidInit, .test = controllerPidTest, .update = controllerPid, .name = "PID"},
   {.init = controllerMellingerInit, .test = controllerMellingerTest, .update = controllerMellinger, .name = "Mellinger"},
   {.init = controllerINDIInit, .test = controllerINDITest, .update = controllerINDI, .name = "INDI"},
+  {.init = controllerStudentInit, .test = controllerStudentTest, .update = controllerStudent, .name = "Student"}, 
 };
 
 

crazyflie_software/crazyflie-firmware-2021.06/src/modules/src/controller_student.c

+
+#include "stabilizer.h"
+#include "stabilizer_types.h"
+
+#include "attitude_controller.h"
+#include "sensfusion6.h"
+#include "position_controller.h"
+#include "controller_student.h"
+
+#include "log.h"
+#include "param.h"
+#include "math3d.h"
+
+#define ATTITUDE_UPDATE_DT    (float)(1.0f/ATTITUDE_RATE)
+
+static bool tiltCompensationEnabled = false;
+
+static attitude_t attitudeDesired;
+static attitude_t rateDesired;
+static float actuatorThrust;
+
+static float cmd_thrust;
+static float cmd_roll;
+static float cmd_pitch;
+static float cmd_yaw;
+static float r_roll;
+static float r_pitch;
+static float r_yaw;
+static float accelz;
+
+void controllerStudentInit(void)
+{
+  attitudeControllerInit(ATTITUDE_UPDATE_DT);
+  positionControllerInit();
+}
+
+bool controllerStudentTest(void)
+{
+  bool pass = true;
+
+  pass &= attitudeControllerTest();
+
+  return pass;
+}
+
+static float capAngle(float angle) {
+  float result = angle;
+
+  while (result > 180.0f) {
+    result -= 360.0f;
+  }
+
+  while (result < -180.0f) {
+    result += 360.0f;
+  }
+
+  return result;
+}
+
+void controllerStudent(control_t *control, setpoint_t *setpoint,
+                                         const sensorData_t *sensors,
+                                         const state_t *state,
+                                         const uint32_t tick)
+{
+  if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
+    // Rate-controled YAW is moving YAW angle setpoint
+    if (setpoint->mode.yaw == modeVelocity) {
+       attitudeDesired.yaw += setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT;
+    } else {
+      attitudeDesired.yaw = setpoint->attitude.yaw;
+    }
+
+    attitudeDesired.yaw = capAngle(attitudeDesired.yaw);
+  }
+
+  if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
+    positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
+  }
+
+  if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
+    // Switch between manual and automatic position control
+    if (setpoint->mode.z == modeDisable) {
+      actuatorThrust = setpoint->thrust;
+    }
+    if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
+      attitudeDesired.roll = setpoint->attitude.roll;
+      attitudeDesired.pitch = setpoint->attitude.pitch;
+    }
+
+    attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
+                                attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
+                                &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
+
+    // For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
+    // value. Also reset the PID to avoid error buildup, which can lead to unstable
+    // behavior if level mode is engaged later
+    if (setpoint->mode.roll == modeVelocity) {
+      rateDesired.roll = setpoint->attitudeRate.roll;
+      attitudeControllerResetRollAttitudePID();
+    }
+    if (setpoint->mode.pitch == modeVelocity) {
+      rateDesired.pitch = setpoint->attitudeRate.pitch;
+      attitudeControllerResetPitchAttitudePID();
+    }
+
+    // TODO: Investigate possibility to subtract gyro drift.
+    attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
+                             rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
+
+    attitudeControllerGetActuatorOutput(&control->roll,
+                                        &control->pitch,
+                                        &control->yaw);
+
+    control->yaw = -control->yaw;
+
+    cmd_thrust = control->thrust;
+    cmd_roll = control->roll;
+    cmd_pitch = control->pitch;
+    cmd_yaw = control->yaw;
+    r_roll = radians(sensors->gyro.x);
+    r_pitch = -radians(sensors->gyro.y);
+    r_yaw = radians(sensors->gyro.z);
+    accelz = sensors->acc.z;
+  }
+
+  if (tiltCompensationEnabled)
+  {
+    control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
+  }
+  else
+  {
+    control->thrust = actuatorThrust;
+  }
+
+  if (control->thrust == 0)
+  {
+    control->thrust = 0;
+    control->roll = 0;
+    control->pitch = 0;
+    control->yaw = 0;
+
+    cmd_thrust = control->thrust;
+    cmd_roll = control->roll;
+    cmd_pitch = control->pitch;
+    cmd_yaw = control->yaw;
+
+    attitudeControllerResetAllPID();
+    positionControllerResetAllPID();
+
+    // Reset the calculated YAW angle for rate control
+    attitudeDesired.yaw = state->attitude.yaw;
+  }
+}
+
+/**
+ * Logging variables for the command and reference signals for the
+ * altitude PID controller
+ */
+LOG_GROUP_START(controller)
+/**
+ * @brief Thrust command
+ */
+LOG_ADD(LOG_FLOAT, cmd_thrust, &cmd_thrust)
+/**
+ * @brief Roll command
+ */
+LOG_ADD(LOG_FLOAT, cmd_roll, &cmd_roll)
+/**
+ * @brief Pitch command
+ */
+LOG_ADD(LOG_FLOAT, cmd_pitch, &cmd_pitch)
+/**
+ * @brief yaw command
+ */
+LOG_ADD(LOG_FLOAT, cmd_yaw, &cmd_yaw)
+/**
+ * @brief Gyro roll measurement in radians
+ */
+LOG_ADD(LOG_FLOAT, r_roll, &r_roll)
+/**
+ * @brief Gyro pitch measurement in radians
+ */
+LOG_ADD(LOG_FLOAT, r_pitch, &r_pitch)
+/**
+ * @brief Yaw  measurement in radians
+ */
+LOG_ADD(LOG_FLOAT, r_yaw, &r_yaw)
+/**
+ * @brief Acceleration in the zaxis in G-force
+ */
+LOG_ADD(LOG_FLOAT, accelz, &accelz)
+/**
+ * @brief Thrust command without (tilt)compensation
+ */
+LOG_ADD(LOG_FLOAT, actuatorThrust, &actuatorThrust)
+/**
+ * @brief Desired roll setpoint
+ */
+LOG_ADD(LOG_FLOAT, roll,      &attitudeDesired.roll)
+/**
+ * @brief Desired pitch setpoint
+ */
+LOG_ADD(LOG_FLOAT, pitch,     &attitudeDesired.pitch)
+/**
+ * @brief Desired yaw setpoint
+ */
+LOG_ADD(LOG_FLOAT, yaw,       &attitudeDesired.yaw)
+/**
+ * @brief Desired roll rate setpoint
+ */
+LOG_ADD(LOG_FLOAT, rollRate,  &rateDesired.roll)
+/**
+ * @brief Desired pitch rate setpoint
+ */
+LOG_ADD(LOG_FLOAT, pitchRate, &rateDesired.pitch)
+/**
+ * @brief Desired yaw rate setpoint
+ */
+LOG_ADD(LOG_FLOAT, yawRate,   &rateDesired.yaw)
+LOG_GROUP_STOP(controller)
+
+
+/**
+ * Controller parameters
+ */
+PARAM_GROUP_START(controller)
+/**
+ * @brief Nonzero for tilt compensation enabled (default: 0)
+ */
+PARAM_ADD(PARAM_UINT8, tiltComp, &tiltCompensationEnabled)
+PARAM_GROUP_STOP(controller)
\ No newline at end of file

wiki @ a1340631

-Subproject commit 01af415f2d31fd91731594658fba9a919158decf
+Subproject commit a1340631171c8f573dae6d0e548dafe7bda5241b