Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[guidance] fix fully actuated indi guidance (#3650)
Issues due date / Add labels to issues (push) Has been cancelled
Details
Doxygen / build (push) Has been cancelled
Details

Browse Source 
The projection matrix was wrong (using ZYX instead of YXZ).
This commit is contained in:
Gautier Hattenberger
2026-04-22 18:47:31 +02:00
committed by GitHub
parent 733d13c1b5
commit 5dc6e55924
2 changed files with 59 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+14 -4
View File
@@ -114,6 +114,16 @@ static float Gmat[GUIDANCE_INDI_NV][GUIDANCE_INDI_NU];
#if GUIDANCE_INDI_USE_WLS
#include "math/wls/wls_alloc.h"
// priorities on control inputs (ax, ay, az)
#ifndef GUIDANCE_INDI_WLS_PRIORITIES
#define GUIDANCE_INDI_WLS_PRIORITIES {100.f, 100.f, 100.f}
#endif
// weighting of prefered control outputs (phi, theta, psi, Tx, Ty, Tz)
#ifndef GUIDANCE_INDI_WLS_WU
#define GUIDANCE_INDI_WLS_WU {1000.f,1000.f,10.f,1.f,1.f,1.f}
#endif
#define NU_MAX 6 // Example constant value // [dtheta, dphi, dthrust, dtx , dty]
#define NV_MAX 3 // Example constant value (ax,ay,az)
static float du_guidance[GUIDANCE_INDI_NU];
@@ -122,10 +132,10 @@ static float *Bwls_gi[GUIDANCE_INDI_NV];
static struct WLS_t wls_guid_p = {
.nu = GUIDANCE_INDI_NU,
.nv = GUIDANCE_INDI_NV,
.gamma_sq = 1000.0,
.gamma_sq = 100.0,
.v = {0.0},
.Wv = {100.f, 100.f, 100.f}, // x,y,z
.Wu = {10.f,10.f,0.f,0.f,0.f,10.f}, // minimize the control input (thetq,phi,Tx,Ty,Tz,psi)
.Wv = GUIDANCE_INDI_WLS_PRIORITIES,
.Wu = GUIDANCE_INDI_WLS_WU,
.u_pref = {0.0},
.u_min = {0.0},
.u_max = {0.0},
@@ -266,7 +276,7 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
#endif
#if GUIDANCE_INDI_USE_WLS
float m = GUIDANCE_INDI_MASS;
const float m = GUIDANCE_INDI_MASS;
wls_guid_p.v[0] = m*a_diff.x;
wls_guid_p.v[1] = m*a_diff.y;
wls_guid_p.v[2] = m*a_diff.z;
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_fully_actuated.c
+45 -45
View File
@@ -35,7 +35,7 @@
// FIXME should be a function of the mass
#ifndef GUIDANCE_INDI_MAX_H_THRUST
#define GUIDANCE_INDI_MAX_H_THRUST 1.f
#define GUIDANCE_INDI_MAX_H_THRUST 2.f
#endif
#ifndef GUIDANCE_INDI_MAX_V_THRUST
@@ -55,27 +55,27 @@ float guidance_indi_max_v_thrust = GUIDANCE_INDI_MAX_V_THRUST;
static void guidance_indi_calcG_yxz(float Gmat[3][6], struct FloatEulers euler_yxz)
{
// Euler Angles
float sphi = sinf(euler_yxz.phi);
float cphi = cosf(euler_yxz.phi);
float stheta = sinf(euler_yxz.theta);
float ctheta = cosf(euler_yxz.theta);
float cpsi = cosf(euler_yxz.psi);
float spsi = sinf(euler_yxz.psi);
const float sphi = sinf(euler_yxz.phi);
const float cphi = cosf(euler_yxz.phi);
const float stheta = sinf(euler_yxz.theta);
const float ctheta = cosf(euler_yxz.theta);
const float cpsi = cosf(euler_yxz.psi);
const float spsi = sinf(euler_yxz.psi);
// Estimated Thrust
float Tx = stab_thrust_filt.x;
float Ty = stab_thrust_filt.y;
float Tz = stab_thrust_filt.z;
const float Tx = stab_thrust_filt.x;
const float Ty = stab_thrust_filt.y;
const float Tz = stab_thrust_filt.z;
// dPhi (Roll)
Gmat[0][0] = Tx * (-spsi * cphi * stheta) + Ty * (spsi * sphi) + Tz * (spsi * cphi * ctheta);
Gmat[1][0] = Tx * (cpsi * cphi * stheta) + Ty * (-cpsi * sphi) + Tz * (-cpsi * cphi * ctheta);
Gmat[2][0] = Tx * (sphi * stheta) + Ty * (cphi) + Tz * (-sphi * ctheta);
Gmat[0][0] = Tx * (stheta * cphi * spsi) + Ty * (stheta * cphi * cpsi) + Tz * (-stheta * sphi);
Gmat[1][0] = Tx * (-sphi * spsi) + Ty * (-sphi * cpsi) + Tz * (-cphi);
Gmat[2][0] = Tx * (ctheta * cphi * spsi) + Ty * (ctheta * cphi * cpsi) + Tz * (-ctheta * sphi);
// dTheta (Pitch)
Gmat[0][1] = Tx * (-cpsi * stheta - spsi * sphi * ctheta) + Tz * (cpsi * ctheta - spsi * sphi * stheta);
Gmat[1][1] = Tx * (-spsi * stheta + cpsi * sphi * ctheta) + Tz * (spsi * ctheta + cpsi * sphi * stheta);
Gmat[2][1] = Tx * (-cphi * ctheta) + Tz * (-cphi * stheta);
Gmat[0][1] = Tx * (-stheta * cpsi + ctheta * sphi * spsi) + Ty * (stheta * spsi + ctheta * sphi * cpsi) + Tz * (ctheta * cphi);
Gmat[1][1] = 0.f;
Gmat[2][1] = Tx * (-ctheta * cpsi - stheta * sphi * spsi) +Ty * (ctheta * spsi - stheta * sphi * cpsi) + Tz * (-stheta * cphi);
// dPsi added to add constraints on psi
Gmat[0][2] = 0.f;
@@ -83,19 +83,19 @@ static void guidance_indi_calcG_yxz(float Gmat[3][6], struct FloatEulers euler_y
Gmat[2][2] = 0.f;
// dTx
Gmat[0][3] = cpsi * ctheta - spsi * sphi * stheta;
Gmat[1][3] = spsi * ctheta + cpsi * sphi * stheta;
Gmat[2][3] = -stheta * cphi;
Gmat[0][3] = ctheta * cpsi - stheta * sphi * spsi;
Gmat[1][3] = ctheta * spsi;
Gmat[2][3] = -stheta * cpsi + ctheta * sphi * spsi;
// dTy
Gmat[0][4] = -spsi * cphi;
Gmat[1][4] = cpsi * cphi;
Gmat[2][4] = sphi;
Gmat[0][4] = -ctheta * spsi + stheta * sphi * cpsi;
Gmat[1][4] = cphi * cpsi;
Gmat[2][4] = stheta * spsi + ctheta * sphi * cpsi;
// dTz
Gmat[0][5] = cpsi * stheta + spsi * sphi * ctheta;
Gmat[1][5] = spsi * stheta - cpsi * sphi * ctheta;
Gmat[2][5] = cphi * ctheta;
Gmat[0][5] = stheta * cphi;
Gmat[1][5] = -sphi;
Gmat[2][5] = ctheta * cphi;
}
/** Compute effectiveness matrix for guidance
@@ -106,37 +106,37 @@ void guidance_indi_calcG(float Gmat[3][6], struct FloatEulers euler) {
guidance_indi_calcG_yxz(Gmat, euler);
}
void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp UNUSED)
void guidance_indi_set_wls_settings(struct WLS_t *wls, struct FloatEulers *euler_yxz, float heading_sp)
{
struct FloatEulers euler_yxz_ref = { 0 };
#if GUIDANCE_INDI_RC_SWITCH_EULER
euler_yxz_ref.phi = (radio_control_get(RADIO_PITCH) / MAX_PPRZ) * guidance_indi_max_bank;
euler_yxz_ref.theta = (radio_control_get(RADIO_ROLL) / MAX_PPRZ) * guidance_indi_max_bank;
euler_yxz_ref.psi = heading_sp // TODO check this one
#endif
euler_yxz_ref.psi = heading_sp;
// Set lower limits
wls->u_min[0] = -guidance_indi_max_bank - euler_yxz->phi; //phi
wls->u_min[1] = -guidance_indi_max_bank - euler_yxz->theta; //theta
wls->u_min[2] = -M_PI - euler_yxz->psi; //psi FIXME M_PI or a lower bound ? (was 1 in initial code)
wls->u_min[3] = -guidance_indi_max_h_thrust - stab_thrust_filt.x; //Tx
wls->u_min[4] = -guidance_indi_max_h_thrust - stab_thrust_filt.y; //Ty
wls->u_min[5] = -guidance_indi_max_v_thrust - stab_thrust_filt.z; //Tz (MAX_PPRZ - stabilization.cmd[COMMAND_THRUST])
wls->u_min[0] = -guidance_indi_max_bank - euler_yxz->phi; // phi
wls->u_min[1] = -guidance_indi_max_bank - euler_yxz->theta; // theta
wls->u_min[2] = -M_PI - euler_yxz->psi; // psi
wls->u_min[3] = -guidance_indi_max_h_thrust - stab_thrust_filt.x; // Tx
wls->u_min[4] = -guidance_indi_max_h_thrust - stab_thrust_filt.y; // Ty
wls->u_min[5] = -guidance_indi_max_v_thrust - stab_thrust_filt.z; // Tz
// ->r limits limits
wls->u_max[0] = guidance_indi_max_bank - euler_yxz->phi; //phi
wls->u_max[1] = guidance_indi_max_bank - euler_yxz->theta; //theta
wls->u_max[2] = M_PI - euler_yxz->psi; //psi
wls->u_max[3] = guidance_indi_max_h_thrust - stab_thrust_filt.x; //Tx
wls->u_max[4] = guidance_indi_max_h_thrust - stab_thrust_filt.y; //Ty
wls->u_max[5] = 9.81f * GUIDANCE_INDI_MASS - stab_thrust_filt.z; //Tz
// Set lower limits limits
wls->u_max[0] = guidance_indi_max_bank - euler_yxz->phi; // phi
wls->u_max[1] = guidance_indi_max_bank - euler_yxz->theta; // theta
wls->u_max[2] = M_PI - euler_yxz->psi; // psi
wls->u_max[3] = guidance_indi_max_h_thrust - stab_thrust_filt.x; // Tx
wls->u_max[4] = guidance_indi_max_h_thrust - stab_thrust_filt.y; // Ty
wls->u_max[5] = 9.81f * GUIDANCE_INDI_MASS - stab_thrust_filt.z; // Tz
// ->ered states
// Set prefered states
wls->u_pref[0] = euler_yxz_ref.phi - euler_yxz->phi; // prefered delta phi
wls->u_pref[1] = euler_yxz_ref.theta - euler_yxz->theta; // prefered delta theta
wls->u_pref[2] = euler_yxz_ref.psi - euler_yxz->psi; // prefered Ty
wls->u_pref[3] = stab_thrust_filt.x; // prefred Tx
wls->u_pref[4] = stab_thrust_filt.y; // prefered Ty
wls->u_pref[5] = stab_thrust_filt.z; // prefered Tz
wls->u_pref[2] = euler_yxz_ref.psi - euler_yxz->psi; // prefered delta psi
wls->u_pref[3] = stab_thrust_filt.x; // prefered delta Tx
wls->u_pref[4] = stab_thrust_filt.y; // prefered delta Ty
wls->u_pref[5] = stab_thrust_filt.z; // prefered delta Tz
}