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

[math] VECTx_NORM updates

Browse Source 
- norm functions/macros return the scalar value, don't take norm as arg
- remove some type specific macros where the generic ones can be used
This commit is contained in:
Felix Ruess
2014-09-11 14:45:13 +02:00
parent 3f984923f2
commit 336defae49
10 changed files with 92 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/navigation.c
+5 -6
View File
@@ -189,8 +189,7 @@ static inline void UNUSED nav_advance_carrot(void) {
/* saturate it */
VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
int32_t dist_to_waypoint;
INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
int32_t dist_to_waypoint = int32_vect2_norm(&path_to_waypoint);
if (dist_to_waypoint < CLOSE_TO_WAYPOINT) {
VECT2_COPY(navigation_carrot, navigation_target);
@@ -311,7 +310,7 @@ bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int1
* distance with half metric precision (6.25 cm)
*/
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2);
INT32_VECT2_NORM(dist_to_point, diff);
dist_to_point = int32_vect2_norm(&diff);
/* return TRUE if we have arrived */
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2))
@@ -331,7 +330,7 @@ bool_t nav_approaching_from(struct EnuCoor_i * wp, struct EnuCoor_i * from, int1
bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) {
uint16_t time_at_wp;
int32_t dist_to_point;
uint32_t dist_to_point;
static uint16_t wp_entry_time = 0;
static bool_t wp_reached = FALSE;
static struct EnuCoor_i wp_last = { 0, 0, 0 };
@@ -343,7 +342,7 @@ bool_t nav_check_wp_time(struct EnuCoor_i * wp, uint16_t stay_time) {
}
VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC/2);
INT32_VECT2_NORM(dist_to_point, diff);
dist_to_point = int32_vect2_norm(&diff);
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC/2)){
if (!wp_reached) {
wp_reached = TRUE;
@@ -510,7 +509,7 @@ bool_t nav_set_heading_towards(float x, float y) {
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, target, *stateGetPositionEnu_f());
// don't change heading if closer than 0.5m to target
if (FLOAT_VECT2_NORM2(pos_diff) > 0.25) {
if (VECT2_NORM2(pos_diff) > 0.25) {
float heading_f = atan2f(pos_diff.x, pos_diff.y);
nav_heading = ANGLE_BFP_OF_REAL(heading_f);
}
sw/airborne/math/pprz_algebra.h
+4
View File
@@ -106,6 +106,8 @@ extern "C" {
/* _vo=v1*v2 */
#define VECT2_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y)
#define VECT2_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y)
/*
* Dimension 3 vectors
*/
@@ -238,6 +240,8 @@ extern "C" {
#define VECT3_DOT_PRODUCT(_v1, _v2) ((_v1).x*(_v2).x + (_v1).y*(_v2).y + (_v1).z*(_v2).z)
#define VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
#define VECT3_RATES_CROSS_VECT3(_vo, _r1, _v2) { \
(_vo).x = (_r1).q*(_v2).z - (_r1).r*(_v2).y; \
(_vo).y = (_r1).r*(_v2).x - (_r1).p*(_v2).z; \
sw/airborne/math/pprz_algebra_float.c
+3 -3
View File
@@ -213,11 +213,11 @@ float float_rmat_reorthogonalize(struct FloatRMat* rm)
VECT3_SUM_SCALED(r1_t, r1, r0, _err);
struct FloatVect3 r2_t;
VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
float s = renorm_factor(FLOAT_VECT3_NORM2(r0_t));
float s = renorm_factor(VECT3_NORM2(r0_t));
MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
s = renorm_factor(FLOAT_VECT3_NORM2(r1_t));
s = renorm_factor(VECT3_NORM2(r1_t));
MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
s = renorm_factor(FLOAT_VECT3_NORM2(r2_t));
s = renorm_factor(VECT3_NORM2(r2_t));
MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
return _err;
sw/airborne/math/pprz_algebra_float.h
+45 -12
View File
@@ -109,16 +109,30 @@ struct FloatRates {
#define FLOAT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0., 0.)
#define FLOAT_VECT2_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y)
/* macros also usable if _v is not a FloatVect2, but a different struct with x,y members */
#define FLOAT_VECT2_NORM(_v) sqrtf(VECT2_NORM2(_v))
#define FLOAT_VECT2_NORM(_n, _v) { \
_n = sqrtf(FLOAT_VECT2_NORM2(_v)); \
}
static inline float float_vect2_norm2(struct FloatVect2* v)
{
return v->x * v->x + v->y * v->y;
}
#define FLOAT_VECT2_NORMALIZE(_v) { \
const float n = sqrtf(FLOAT_VECT2_NORM2(_v)); \
VECT2_SMUL(_v, _v, 1./n); \
static inline float float_vect2_norm(struct FloatVect2* v)
{
return sqrtf(float_vect2_norm2(v));
}
/** normalize 2D vector in place */
static inline void float_vect2_normalize(struct FloatVect2* v)
{
const float n = float_vect2_norm(v);
if (n > 0) {
v->x /= n;
v->y /= n;
}
}
#define FLOAT_VECT2_NORMALIZE(_v) float_vect2_normalize(&(_v))
/*
@@ -127,14 +141,33 @@ struct FloatRates {
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.)
#define FLOAT_VECT3_NORM2(_v) ((_v).x*(_v).x + (_v).y*(_v).y + (_v).z*(_v).z)
/* macros also usable if _v is not a FloatVect3, but a different struct with x,y,z members */
#define FLOAT_VECT3_NORM(_v) sqrtf(VECT3_NORM2(_v))
#define FLOAT_VECT3_NORM(_v) (sqrtf(FLOAT_VECT3_NORM2(_v)))
static inline float float_vect3_norm2(struct FloatVect3* v)
{
return v->x * v->x + v->y * v->y + v->z * v->z;
}
#define FLOAT_VECT3_NORMALIZE(_v) { \
const float n = FLOAT_VECT3_NORM(_v); \
VECT3_SMUL(_v, _v, 1./n); \
static inline float float_vect3_norm(struct FloatVect3* v)
{
return sqrtf(float_vect3_norm2(v));
}
/** normalize 3D vector in place */
static inline void float_vect3_normalize(struct FloatVect3* v)
{
const float n = float_vect3_norm(v);
if (n > 0) {
v->x /= n;
v->y /= n;
v->z /= n;
}
}
#define FLOAT_VECT3_NORMALIZE(_v) float_vect3_norm(&(_v))
#define FLOAT_RATES_ZERO(_r) { \
RATES_ASSIGN(_r, 0., 0., 0.); \
sw/airborne/math/pprz_algebra_int.h
+23 -12
View File
@@ -231,16 +231,30 @@ extern int32_t int32_sqrt(int32_t in);
#define INT_VECT2_ZERO(_v) VECT2_ASSIGN(_v, 0, 0)
#define INT32_VECT2_NORM(n, v) { \
int32_t n2 = (v).x*(v).x + (v).y*(v).y; \
n = int32_sqrt(n2); \
}
/* macros also usable if _v is not a Int32Vect2, but a different struct with x,y members */
#define INT32_VECT2_NORM(_v) int32_sqrt(VECT2_NORM2(_v))
#define INT32_VECT2_NORMALIZE(_v,_frac) { \
int32_t n; \
INT32_VECT2_NORM(n, _v); \
INT32_VECT2_SCALE_2(_v, _v, BFP_OF_REAL((1.),_frac) , n); \
static inline uint32_t int32_vect2_norm2(struct Int32Vect2* v)
{
return v->x * v->x + v->y * v->y;
}
static inline uint32_t int32_vect2_norm(struct Int32Vect2* v)
{
return int32_sqrt(int32_vect2_norm2(v));
}
static inline void int32_vect2_normalize(struct Int32Vect2* v, uint8_t frac)
{
const uint32_t f = BFP_OF_REAL((1.), frac);
const uint32_t n = int32_vect2_norm(v);
if (n > 0) {
v->x = v->x * f / n;
v->y = v->y * f / n;
}
}
#define INT32_VECT2_NORMALIZE(_v,_frac) int32_vect2_normalize(&(_v), _frac)
#define INT32_VECT2_RSHIFT(_o, _i, _r) { \
@@ -271,10 +285,7 @@ extern int32_t int32_sqrt(int32_t in);
(_a).z = ((_b).z * (_num)) / (_den); \
}
#define INT32_VECT3_NORM(n, v) { \
int32_t n2 = (v).x*(v).x + (v).y*(v).y + (v).z*(v).z; \
n = int32_sqrt(n2); \
}
#define INT32_VECT3_NORM(_v) int32_sqrt(VECT3_NORM2(_v))
#define INT32_VECT3_RSHIFT(_o, _i, _r) { \
(_o).x = ((_i).x >> (_r)); \
sw/airborne/modules/digital_cam/hackhd.c
+1 -1
View File
@@ -229,7 +229,7 @@ void hackhd_autoshoot(void) {
struct FloatVect2 d_pos;
d_pos.x = pos.x - hackhd.last_shot_pos.x;
d_pos.y = pos.y - hackhd.last_shot_pos.y;
if (FLOAT_VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST*HACKHD_AUTOSHOOT_DIST)
if (VECT2_NORM2(d_pos) > (HACKHD_AUTOSHOOT_DIST*HACKHD_AUTOSHOOT_DIST)
|| hackhd.status == HACKHD_AUTOSHOOT_START) {
#endif
// take a picture
sw/airborne/modules/mission/mission_rotorcraft_nav.c
+1 -2
View File
@@ -56,8 +56,7 @@ bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
VECT2_DIFF(vect_from_home, tmp_enu_point, home);
//Saturate the mission wp not to overflow max_dist_from_home
//including a buffer zone before limits
float dist_to_home;
FLOAT_VECT2_NORM(dist_to_home, vect_from_home);
float dist_to_home = float_vect2_norm(&vect_from_home);
dist_to_home += BUFFER_ZONE_DIST;
if (dist_to_home > MAX_DIST_FROM_HOME) {
VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
sw/airborne/modules/nav/nav_spiral.c
+2 -2
View File
@@ -61,7 +61,7 @@ bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start,
struct FloatVect2 edge;
VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center);
FLOAT_VECT2_NORM(nav_spiral.radius, edge);
nav_spiral.radius = float_vect2_norm(&edge);
// get a copy of the current position
struct EnuCoor_f pos_enu;
@@ -132,7 +132,7 @@ bool_t nav_spiral_run(void)
//DistanceStartEstim = |Starting position angular - current positon|
struct FloatVect2 pos_diff;
VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu);
FLOAT_VECT2_NORM(DistanceStartEstim, pos_diff);
DistanceStartEstim = float_vect2_norm(&pos_diff);
CircleAlpha = (2.0 * asin (DistanceStartEstim / (2 * nav_spiral.radius_start)));
if (CircleAlpha >= nav_spiral.alpha_limit) {
VECT2_COPY(nav_spiral.last_circle, pos_enu);
sw/airborne/state.c
+7 -7
View File
@@ -767,7 +767,7 @@ void stateCalcHorizontalSpeedNorm_i(void) {
INT32_SQRT(state.h_speed_norm_i, n2);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
@@ -777,7 +777,7 @@ void stateCalcHorizontalSpeedNorm_i(void) {
INT32_SQRT(state.h_speed_norm_i, n2);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
@@ -792,7 +792,7 @@ void stateCalcHorizontalSpeedNorm_i(void) {
else if (bit_is_set(state.speed_status, SPEED_ECEF_F)) {
ned_of_ecef_vect_f(&state.ned_speed_f, &state.ned_origin_f, &state.ecef_speed_f);
SetBit(state.speed_status, SPEED_NED_F);
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
SetBit(state.speed_status, SPEED_HNORM_F);
state.h_speed_norm_i = SPEED_BFP_OF_REAL(state.h_speed_norm_f);
}
@@ -982,20 +982,20 @@ void stateCalcHorizontalSpeedNorm_f(void) {
state.h_speed_norm_f = SPEED_FLOAT_OF_BFP(state.h_speed_norm_i);
}
else if (bit_is_set(state.speed_status, SPEED_NED_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_F)) {
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_NED_I)) {
SPEEDS_FLOAT_OF_BFP(state.ned_speed_f, state.ned_speed_i);
SetBit(state.speed_status, SPEED_NED_F);
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.ned_speed_f);
state.h_speed_norm_f = FLOAT_VECT2_NORM(state.ned_speed_f);
}
else if (bit_is_set(state.speed_status, SPEED_ENU_I)) {
SPEEDS_FLOAT_OF_BFP(state.enu_speed_f, state.enu_speed_i);
SetBit(state.speed_status, SPEED_ENU_F);
FLOAT_VECT2_NORM(state.h_speed_norm_f, state.enu_speed_f);
state.h_speed_norm_f = FLOAT_VECT2_NORM(state.enu_speed_f);
}
/* set bit to indicate this representation is computed */
SetBit(state.speed_status, SPEED_HNORM_F);
sw/airborne/test/pprz_algebra_print.h
+1 -2
View File
@@ -65,8 +65,7 @@
#define DISPLAY_INT32_VECT3(text, _v) { \
int32_t norm; \
INT32_VECT3_NORM(norm, _v); \
int32_t norm = INT32_VECT3_NORM(_v); \
printf("%s %d %d %d (%d)\n",text, (_v).x, (_v).y, (_v).z, norm); \
}