Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[guidance_v] slightly more correct and efficient thrust_coeff calc

Browse Source
This commit is contained in:
Felix Ruess
2013-09-03 22:23:33 +02:00
parent 70454b1111
commit 5a4b39a417
1 changed files with 22 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+22 -10
View File
@@ -266,17 +266,29 @@ void guidance_v_run(bool_t in_flight) {
}
}
#define MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
/// get the cosine of the angle between thrust vector and gravity vector
static int32_t get_vertical_thrust_coeff(void) {
int32_t cphi,ctheta,cphitheta;
struct Int32Eulers* att_euler = stateGetNedToBodyEulers_i();
PPRZ_ITRIG_COS(cphi, att_euler->phi);
PPRZ_ITRIG_COS(ctheta, att_euler->theta);
cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
if (cphitheta < MAX_BANK_COEF)
cphitheta = MAX_BANK_COEF;
return cphitheta;
static const int32_t max_bank_coef = BFP_OF_REAL(RadOfDeg(30.), INT32_TRIG_FRAC);
struct Int32RMat* att = stateGetNedToBodyRMat_i();
/* thrust vector:
* INT32_RMAT_VMULT(thrust_vect, att, zaxis)
* same as last colum of rmat with INT32_TRIG_FRAC
* struct Int32Vect thrust_vect = {att.m[2], att.m[5], att.m[8]};
*
* Angle between two vectors v1 and v2:
* angle = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
* since here both are already of unit length:
* angle = acos(dot(v1, v2))
* since we we want the cosine of the angle we simply need
* thrust_coeff = dot(v1, v2)
* also can be simplified considering: v1 is zaxis with (0,0,1)
* dot(v1, v2) = v1.z * v2.z = v2.z
*/
int32_t coef = att->m[8];
if (coef < max_bank_coef)
coef = max_bank_coef;
return coef;
}