Merge branch 'remove-pos-vel-controller' into 'master'

Removed position and velocity control from student controller

See merge request !67

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

@@ -4,7 +4,6 @@
 
 #include "attitude_controller.h"
 #include "sensfusion6.h"
-#include "position_controller.h"
 #include "controller_student.h"
 
 #include "log.h"
@@ -36,7 +35,6 @@ static float accelz;
 void controllerStudentInit(void)
 {
   attitudeControllerInit(STUDENT_UPDATE_DT);
-  positionControllerInit();
 }
 
 bool controllerStudentTest(void)
@@ -91,15 +89,19 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
 
   //TODO MICROCART: student written
 
-  //Check if time to update position/velocity controller with RATE_DO_EXECUTE
-  //then update the position controller to find the desired attitude
-  if (RATE_DO_EXECUTE(POSITION_RATE, tick)) {
-    //note does not calculate desired yaw or yaw rate
-    positionController(&thrustDesired, &attitudeDesired, setpoint, state);
-  }
-
   // check if time to update the attutide controller
   if (RATE_DO_EXECUTE(ATTITUDE_RATE, tick)) {
+
+    //only support attitude and attitude rate control
+    if(setpoint->mode.x != modeDisable || setpoint->mode.y != modeDisable || setpoint->mode.z != modeDisable){
+      DEBUG_PRINT("Student controller does not support vehicle position or velocity mode. Check flight mode.");
+      control->thrust = 0;
+      control->roll = 0;
+      control->pitch = 0;
+      control->yaw = 0;
+      return;
+    }
+
     // update desired yaw from setpoint
     // Rate-controlled YAW is moving YAW angle setpoint
     if (setpoint->mode.yaw == modeVelocity) {
@@ -110,17 +112,13 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
     }
     attitudeDesired.yaw = capAngle(attitudeDesired.yaw);
     
-    // if z position control disabled, set the output thrust to be the same as the setpoint thrust
-    // disreguarding the results of the position controller
-    if (setpoint->mode.z == modeDisable) {
-      thrustDesired = setpoint->thrust;
-    }
-    // if x or y position control is disabled, set the desired 
-    // roll and pitch to equal their setpoints, disreguarding the results of the position controller
-    if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
-      attitudeDesired.roll = setpoint->attitude.roll;
-      attitudeDesired.pitch = setpoint->attitude.pitch;
-    }
+    //set desired roll and pitch
+    attitudeDesired.roll = setpoint->attitude.roll;
+    attitudeDesired.pitch = setpoint->attitude.pitch;
+
+    //set desired thrust
+    thrustDesired = setpoint->thrust;
+
 
     // Run the attitude controller with the actual attitude and desired attitude
     // outputs the desired attitude rates
@@ -128,7 +126,7 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
                                 attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
                                 &rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
 
-    // For roll and pitch, if velocity mode, overwrite rateDesired output 
+    // if velocity mode, overwrite rateDesired output 
     // from the attitude controller with the setpoint value
     // Also reset the PID to avoid error buildup, which can lead to unstable
     // behavior if level mode is engaged later
@@ -154,16 +152,6 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
 
     //invert yaw control
     control->yaw = -control->yaw;
-
-    //copy values for logging
-    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;
   }
 
   //tilt compensation, increases thrust when tilting more to maintain height
@@ -186,18 +174,23 @@ void controllerStudent(control_t *control, setpoint_t *setpoint, const sensorDat
     control->pitch = 0;
     control->yaw = 0;
 
-    cmd_thrust = control->thrust;
-    cmd_roll = control->roll;
-    cmd_pitch = control->pitch;
-    cmd_yaw = control->yaw;
-
     //reset all PID variables
     attitudeControllerResetAllPID();
-    positionControllerResetAllPID();
 
-    // Reset the YAW angle for rate control
+    // Reset the YAW angle desired 
     attitudeDesired.yaw = state->attitude.yaw;
   }
+
+  //copy values for logging
+  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;
+
 }
 
 /**