aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 09:39:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Better code for yaw target

Browse Source
This commit is contained in:
Oleg Kalachev 2023-12-13 07:49:57 +03:00
parent 860db237b7
commit 46579ce8a4
3 changed files with 30 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
flix/control.ino
View File

@ -36,6 +36,7 @@
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
enum { MANUAL, ACRO, STAB } mode = STAB;
enum { YAW, YAW_RATE } yawMode = YAW;
bool armed = false;
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
@ -51,9 +52,6 @@ Quaternion attitudeTarget;
Vector ratesTarget;
Vector torqueTarget;
float thrustTarget;
// TODO: ugly
float yawTarget = NAN;
bool controlYaw = false;
void control()
{
@ -82,23 +80,26 @@ void interpretRC()
thrustTarget = controls[RC_CHANNEL_THROTTLE];
controlYaw = armed && mode == STAB && controls[RC_CHANNEL_YAW] == 0;
if (!controlYaw) {
yawTarget = attitude.getYaw();
}
if (mode == ACRO) {
yawMode = YAW_RATE;
ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
ratesTarget.y = -controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX; // up pitch stick means tilt clockwise in frd
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
} else if (mode == STAB) {
yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
attitudeTarget = Quaternion::fromEulerZYX(
controls[RC_CHANNEL_ROLL] * MAX_TILT,
-controls[RC_CHANNEL_PITCH] * MAX_TILT,
yawTarget); // attitude.getYaw());
attitudeTarget.getYaw());
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
}
if (yawMode == YAW_RATE || !motorsActive()) {
// update yaw target as we have not control over the yaw
attitudeTarget.setYaw(attitude.getYaw());
}
}
void controlAttitude()
@ -134,8 +135,8 @@ void controlAttitude()
ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt);
ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt);
if (controlYaw) {
ratesTarget.z = yawPID.update(yawTarget - attitude.getYaw(), dt); // WARNING:
if (yawMode == YAW) {
ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);
}
if (!ratesTarget.finite()) {
@ -212,6 +213,11 @@ void controlRate()
motors[3] = constrain(motors[3], 0, 1);
}
bool motorsActive()
{
return motors[0] > 0 || motors[1] > 0 || motors[2] > 0 || motors[3] > 0;
}
const char* getModeName()
{
switch (mode) {

12
flix/util.ino
View File

@ -22,3 +22,15 @@ float randomFloat(float min, float max)
{
return min + (max - min) * (float)rand() / RAND_MAX;
}
// wrap angle to [-PI, PI)
float wrapAngle(float angle)
{
angle = fmodf(angle, 2 * PI);
if (angle > PI) {
angle -= 2 * PI;
} else if (angle < -PI) {
angle += 2 * PI;
}
return angle;
}

1
gazebo/flix.h
View File

@ -37,6 +37,7 @@ void controlAttitude();
void controlManual();
void controlRate();
void showTable();
bool motorsActive();
void cliTestMotor(uint8_t n);
// mocks