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

precommand consider bank angle

Browse Source 
correct nav_circle_radians
This commit is contained in:
Gautier Hattenberger
2010-07-02 12:06:06 +00:00
parent e9b225cb3c
commit b2cb589ab1
2 changed files with 18 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/booz/booz2_navigation.c
+9 -3
View File
@@ -155,9 +155,15 @@ void nav_circle(uint8_t wp_center, int32_t radius) {
// compute qdr
INT32_ATAN2(nav_circle_qdr, pos_diff.y, pos_diff.x);
// increment circle radians
int32_t angle_diff = nav_circle_qdr - last_qdr;
INT32_ANGLE_NORMALIZE(angle_diff);
nav_circle_radians += angle_diff;
if (nav_circle_radians != 0) {
int32_t angle_diff = nav_circle_qdr - last_qdr;
INT32_ANGLE_NORMALIZE(angle_diff);
nav_circle_radians += angle_diff;
}
else {
// Smallest angle to increment at next step
nav_circle_radians = 1;
}
// direction of rotation
int8_t sign_radius = radius > 0 ? 1 : -1;
sw/airborne/booz/guidance/booz2_guidance_v.c
+9 -1
View File
@@ -28,6 +28,7 @@
#include "booz_radio_control.h"
#include "booz_stabilization.h"
#include "booz_ahrs.h"
#include "booz_fms.h"
#include "booz2_navigation.h"
@@ -225,6 +226,8 @@ void booz2_guidance_v_run(bool_t in_flight) {
#define FF_CMD_FRAC 18
#define BOOZ2_MAX_BANK_COEF (BFP_OF_REAL(RadOfDeg(30.),INT32_TRIG_FRAC))
static inline void run_hover_loop(bool_t in_flight) {
/* convert our reference to generic representation */
@@ -260,8 +263,13 @@ static inline void run_hover_loop(bool_t in_flight) {
booz2_guidance_v_ff_cmd = ( g_m_zdd - (inv_m>>1)) / inv_m;
#else
booz2_guidance_v_ff_cmd = g_m_zdd / inv_m;
int32_t cphi,ctheta,cphitheta;
PPRZ_ITRIG_COS(cphi, booz_ahrs.ltp_to_body_euler.phi);
PPRZ_ITRIG_COS(ctheta, booz_ahrs.ltp_to_body_euler.theta);
cphitheta = (cphi * ctheta) >> INT32_TRIG_FRAC;
if (cphitheta < BOOZ2_MAX_BANK_COEF) cphitheta = BOOZ2_MAX_BANK_COEF;
booz2_guidance_v_ff_cmd = (booz2_guidance_v_ff_cmd << INT32_TRIG_FRAC) / cphitheta;
#endif
// booz2_guidance_v_ff_cmd = BOOZ2_GUIDANCE_V_HOVER_POWER;
/* our error command */
booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) +