I am relatively new to coding in C, and I am trying to add a custom controller to the crazyflie firmware. I wanted to do some troubleshooting to make sure that the controller was receiving the correct state, and so I decided to log the state information in stabilizer.c after it was passed to the controller. From what I understand, the address of the state information is passed to the controller function in the main stabilizer loop here:
Code: Select all
controller(&control, &setpoint, &sensorData, &state, tick);
Then, the controllers use the information as necessary. However, when logging the output of the state information directly from the controller script, the values seem to change drastically. I tried logging some state information from controller_pid.c and controller_mellinger.c to see if I made a mistake, but they were outputting the same conflated values. The controllers are all working fine, so I assume that I am somehow accessing the variables incorrectly when logging. I should note that the same state values are all outputted properly in the stateEstimate log block in stabilizer.c.
This picture shows the output of q1 (quanterion) from the the stateEstimate log group as I rotate the crazyflie, and it looks as expected: This picture shows the output of q1 from the log group in controller_pid.c as a I rotate the crazyflie. If you notice the y axis, the values have become huge, on the order of 10^9. Also, its hard to see in the picture, but if you look really closely, you can see that the value is changing as I rotate the crazyflie. It also jumps around +/- 10^9. This all leads me to believe that I am accessing the correct variable, but I am somehow scaling/changing it from what is originally in the state variable.
The following is my attempt at logging the state variables in controller_pid.c
Code: Select all
#include "stabilizer.h"
#include "stabilizer_types.h"
#include "attitude_controller.h"
#include "sensfusion6.h"
#include "position_controller.h"
#include "controller_pid.h"
#include "log.h"
#include "param.h"
#include "math3d.h"
#define ATTITUDE_UPDATE_DT (float)(1.0f/ATTITUDE_RATE)
static bool tiltCompensationEnabled = false;
static bool pitchWithoutThrust = 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;
static float check;
static state_t statet;
void controllerPidInit(void)
{
attitudeControllerInit(ATTITUDE_UPDATE_DT);
positionControllerInit();
}
bool controllerPidTest(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 controllerPid(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;
}
if (setpoint->mode.pitch == modeVelocity) {
attitudeDesired.pitch += setpoint->attitudeRate.pitch * ATTITUDE_UPDATE_DT;
} else {
attitudeDesired.pitch = setpoint->attitude.pitch;
}
if (setpoint->mode.roll == modeVelocity) {
attitudeDesired.roll += setpoint->attitudeRate.roll * ATTITUDE_UPDATE_DT;
} else {
attitudeDesired.roll = setpoint->attitude.roll;
}
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;
statet.attitude.roll = state->attitude.roll;
float a = 0.1;
check = state->attitude.roll;
//check = statet->attitudeQuaternion.x;
}
if (tiltCompensationEnabled)
{
control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
}
else
{
control->thrust = actuatorThrust;
}
cmd_thrust = control->thrust; // added
if (control->thrust == 0 && !pitchWithoutThrust)
{
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;
}
}
LOG_GROUP_START(controller)
LOG_ADD(LOG_FLOAT, cmd_thrust, &cmd_thrust)
LOG_ADD(LOG_FLOAT, cmd_roll, &cmd_roll)
LOG_ADD(LOG_FLOAT, cmd_pitch, &cmd_pitch)
LOG_ADD(LOG_FLOAT, cmd_yaw, &cmd_yaw)
LOG_ADD(LOG_FLOAT, r_roll, &r_roll)
LOG_ADD(LOG_FLOAT, r_pitch, &r_pitch)
LOG_ADD(LOG_FLOAT, r_yaw, &r_yaw)
LOG_ADD(LOG_FLOAT, accelz, &accelz)
LOG_ADD(LOG_FLOAT, actuatorThrust, &actuatorThrust)
LOG_ADD(LOG_FLOAT, roll, &attitudeDesired.roll)
LOG_ADD(LOG_FLOAT, pitch, &attitudeDesired.pitch)
LOG_ADD(LOG_FLOAT, yaw, &attitudeDesired.yaw)
LOG_ADD(LOG_FLOAT, rollRate, &rateDesired.roll)
LOG_ADD(LOG_FLOAT, pitchRate, &rateDesired.pitch)
LOG_ADD(LOG_FLOAT, yawRate, &rateDesired.yaw)
LOG_GROUP_STOP(controller)
PARAM_GROUP_START(controller)
PARAM_ADD(PARAM_UINT8, tiltComp, &tiltCompensationEnabled)
PARAM_ADD(PARAM_UINT8, noThrustNeeded, &pitchWithoutThrust)
PARAM_GROUP_STOP(controller)
LOG_GROUP_START(ctrlpidex)
LOG_ADD(PARAM_FLOAT,q1, &statet.attitude.roll)
LOG_ADD(PARAM_FLOAT,q2, &check)//state_full[4])
LOG_ADD(PARAM_FLOAT,q3, &check)
LOG_ADD(PARAM_FLOAT,q4, &check)
LOG_GROUP_STOP(ctrlpidex)