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 28 Packages Projects Releases Wiki Activity

Merge branch 'state_no_pointers'

Browse Source 
* state_no_pointers:
  [state interface] set basic types by value
  [state interface] get functions: don't return pointers for single values
This commit is contained in:
Felix Ruess
2015-09-04 22:32:34 +02:00
parent f81f35dd86 4ebf37e922
commit 99f5ce039e
37 changed files with 110 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/fixedwing/autopilot.c
+3 -2
View File
@@ -152,13 +152,14 @@ static void send_desired(struct transport_tx *trans, struct link_device *dev)
static void send_airspeed(struct transport_tx *trans __attribute__((unused)),
struct link_device *dev __attribute__((unused)))
{
float airspeed = stateGetAirspeed_f();
#if USE_AIRSPEED
pprz_msg_send_AIRSPEED(trans, dev, AC_ID,
stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint,
&airspeed, &v_ctl_auto_airspeed_setpoint,
&v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint);
#else
float zero = 0;
pprz_msg_send_AIRSPEED(trans, dev, AC_ID, stateGetAirspeed_f(), &zero, &zero, &zero);
pprz_msg_send_AIRSPEED(trans, dev, AC_ID, &airspeed, &zero, &zero, &zero);
#endif
}
#endif /* PERIODIC_TELEMETRY */
sw/airborne/firmwares/fixedwing/datalink.c
+3 -3
View File
@@ -166,11 +166,11 @@ void dl_parse_msg(void)
wind.y = DL_WIND_INFO_east(dl_buffer);
stateSetHorizontalWindspeed_f(&wind);
#if !USE_AIRSPEED
float airspeed = DL_WIND_INFO_airspeed(dl_buffer);
stateSetAirspeed_f(&airspeed);
stateSetAirspeed_f(DL_WIND_INFO_airspeed(dl_buffer));
#endif
#ifdef WIND_INFO_RET
DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f());
float airspeed = stateGetAirspeed_f();
DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, &airspeed);
#endif
}
break;
sw/airborne/firmwares/fixedwing/guidance/energy_ctrl.c
+2 -2
View File
@@ -314,7 +314,7 @@ void v_ctl_climb_loop(void)
#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - (*stateGetHorizontalSpeedNorm_f()));
float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - stateGetHorizontalSpeedNorm_f());
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) *
@@ -332,7 +332,7 @@ void v_ctl_climb_loop(void)
#endif
// Airspeed outerloop: positive means we need to accelerate
float speed_error = v_ctl_auto_airspeed_controlled - (*stateGetAirspeed_f());
float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f();
// Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
+2 -2
View File
@@ -344,7 +344,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void)
(err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f());
float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - stateGetHorizontalSpeedNorm_f());
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) *
@@ -358,7 +358,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void)
}
// Airspeed control loop (input: airspeed controlled, output: throttle controlled)
float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f());
float err_airspeed = (v_ctl_auto_airspeed_controlled - stateGetAirspeed_f());
v_ctl_auto_airspeed_sum_err += err_airspeed;
BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) *
sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
+2 -2
View File
@@ -292,7 +292,7 @@ static inline void v_ctl_set_airspeed(void)
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
}
float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f();
float err_airspeed = v_ctl_auto_airspeed_setpoint - stateGetAirspeed_f();
float d_err_airspeed = (err_airspeed - last_err_as) * AIRSPEED_LOOP_PERIOD;
last_err_as = err_airspeed;
if (v_ctl_auto_airspeed_throttle_igain > 0.) {
@@ -340,7 +340,7 @@ static inline void v_ctl_set_airspeed(void)
static inline void v_ctl_set_groundspeed(void)
{
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
float err_groundspeed = v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f();
float err_groundspeed = v_ctl_auto_groundspeed_setpoint - stateGetHorizontalSpeedNorm_f();
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err *
sw/airborne/firmwares/fixedwing/main_ap.c
+1 -1
View File
@@ -671,7 +671,7 @@ void monitor_task(void)
kill_throttle |= launch && (dist2_to_home > Square(KILL_MODE_DISTANCE));
if (!autopilot_flight_time &&
*stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
stateGetHorizontalSpeedNorm_f() > MIN_SPEED_FOR_TAKEOFF) {
autopilot_flight_time = 1;
launch = TRUE; /* Not set in non auto launch */
#if DOWNLINK
sw/airborne/firmwares/fixedwing/nav.c
+7 -7
View File
@@ -136,7 +136,7 @@ void nav_circle_XY(float x, float y, float radius)
(dist2_center > Square(abs_radius + dist_carrot)
|| dist2_center < Square(abs_radius - dist_carrot)) ?
0 :
atanf((*stateGetHorizontalSpeedNorm_f()) * (*stateGetHorizontalSpeedNorm_f()) / (NAV_GRAVITY * radius));
atanf(stateGetHorizontalSpeedNorm_f() * stateGetHorizontalSpeedNorm_f() / (NAV_GRAVITY * radius));
float carrot_angle = dist_carrot / abs_radius;
carrot_angle = Min(carrot_angle, M_PI / 4);
@@ -160,7 +160,7 @@ void nav_circle_XY(float x, float y, float radius)
float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \
float alt = start_alt + nav_leg_progress * diff_alt; \
float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \
float pre_climb = stateGetHorizontalSpeedNorm_f() * diff_alt / nav_leg_length; \
NavVerticalAltitudeMode(alt, pre_climb); \
}
@@ -212,7 +212,7 @@ static void nav_ground_speed_loop(void)
{
if (MINIMUM_AIRSPEED < nav_ground_speed_setpoint
&& nav_ground_speed_setpoint < MAXIMUM_AIRSPEED) {
float err = nav_ground_speed_setpoint - (*stateGetHorizontalSpeedNorm_f());
float err = nav_ground_speed_setpoint - stateGetHorizontalSpeedNorm_f();
v_ctl_auto_throttle_cruise_throttle += nav_ground_speed_pgain * err;
Bound(v_ctl_auto_throttle_cruise_throttle, v_ctl_auto_throttle_min_cruise_throttle,
v_ctl_auto_throttle_max_cruise_throttle);
@@ -342,13 +342,13 @@ bool_t nav_approaching_xy(float x, float y, float from_x, float from_y, float ap
float leg_x = x - from_x;
float leg_y = y - from_y;
float leg = sqrtf(Max(leg_x * leg_x + leg_y * leg_y, 1.));
float exceed_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f()); // negative value
float exceed_dist = approaching_time * stateGetHorizontalSpeedNorm_f(); // negative value
float scal_prod = (leg_x * pw_x + leg_y * pw_y) / leg;
return (scal_prod < exceed_dist);
} else {
// fly close enough of the waypoint or cross it
dist2_to_wp = pw_x * pw_x + pw_y * pw_y;
float min_dist = approaching_time * (*stateGetHorizontalSpeedNorm_f());
float min_dist = approaching_time * stateGetHorizontalSpeedNorm_f();
if (dist2_to_wp < min_dist * min_dist) {
return TRUE;
}
@@ -374,11 +374,11 @@ void fly_to_xy(float x, float y)
}
lateral_mode = LATERAL_MODE_COURSE;
} else {
float diff = atan2f(x - pos->x, y - pos->y) - (*stateGetHorizontalSpeedDir_f());
float diff = atan2f(x - pos->x, y - pos->y) - stateGetHorizontalSpeedDir_f();
NormRadAngle(diff);
BoundAbs(diff, M_PI / 2.);
float s = sinf(diff);
float speed = *stateGetHorizontalSpeedNorm_f();
float speed = stateGetHorizontalSpeedNorm_f();
h_ctl_roll_setpoint = atanf(2 * speed * speed * s * h_ctl_course_pgain / (CARROT * NOMINAL_AIRSPEED * 9.81));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
sw/airborne/firmwares/fixedwing/nav.h
+1 -1
View File
@@ -133,7 +133,7 @@ extern void nav_circle_XY(float x, float y, float radius);
/** True if x (in degrees) is close to the current QDR (less than 10 degrees)*/
#define NavQdrCloseTo(x) CloseDegAngles(x, NavCircleQdr())
#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(*stateGetHorizontalSpeedDir_f()))
#define NavCourseCloseTo(x) CloseDegAngles(x, DegOfRad(stateGetHorizontalSpeedDir_f()))
/*********** Navigation along a line *************************************/
extern void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y);
sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
+5 -5
View File
@@ -376,14 +376,14 @@ void h_ctl_course_loop(void)
static float last_err;
// Ground path error
float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f());
float err = h_ctl_course_setpoint - stateGetHorizontalSpeedDir_f();
NormRadAngle(err);
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f()) / NOMINAL_AIRSPEED;
float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
@@ -399,7 +399,7 @@ static inline void compute_airspeed_ratio(void)
if (use_airspeed_ratio) {
// low pass airspeed
static float airspeed = 0.;
airspeed = (15 * airspeed + (*stateGetAirspeed_f())) / 16;
airspeed = (15 * airspeed + stateGetAirspeed_f()) / 16;
// compute ratio
float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
@@ -663,7 +663,7 @@ inline static void h_ctl_yaw_loop(void)
#endif
#ifdef USE_AIRSPEED
float Vo = *stateGetAirspeed_f();
float Vo = stateGetAirspeed_f();
Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED);
#else
float Vo = NOMINAL_AIRSPEED;
@@ -712,7 +712,7 @@ inline static void h_ctl_cl_loop(void)
#if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT
float corrected_airspeed = v_ctl_auto_airspeed_setpoint;
#else
float corrected_airspeed = *stateGetAirspeed_f();
float corrected_airspeed = stateGetAirspeed_f();
#endif
#if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR
corrected_airspeed /= sqrtf(nz);
sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
+5 -5
View File
@@ -203,17 +203,17 @@ void h_ctl_course_loop(void)
static float last_err;
// Ground path error
float err = *stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
float err = stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
NormRadAngle(err);
#ifdef STRONG_WIND
// Usefull path speed
const float reference_advance = (NOMINAL_AIRSPEED / 2.);
float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance;
float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;
if (
(advance < 1.) && // Path speed is small
((*stateGetHorizontalSpeedNorm_f()) < reference_advance) // Small path speed is due to wind (small groundspeed)
(stateGetHorizontalSpeedNorm_f() < reference_advance) // Small path speed is due to wind (small groundspeed)
) {
/*
// rough crabangle approximation
@@ -282,7 +282,7 @@ void h_ctl_course_loop(void)
}
#endif
float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f()) / NOMINAL_AIRSPEED;
float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
@@ -461,7 +461,7 @@ inline static void h_ctl_pitch_loop(void)
err = att->theta - h_ctl_pitch_loop_setpoint;
break;
case H_CTL_PITCH_MODE_AOA:
err = (*stateGetAngleOfAttack_f()) - h_ctl_pitch_loop_setpoint;
err = stateGetAngleOfAttack_f() - h_ctl_pitch_loop_setpoint;
break;
default:
err = att->theta - h_ctl_pitch_loop_setpoint;
sw/airborne/modules/air_data/air_data.c
+1 -1
View File
@@ -129,7 +129,7 @@ static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pr
air_data.airspeed = eas_from_dynamic_pressure(air_data.differential);
air_data.tas = tas_from_eas(air_data.airspeed);
#if USE_AIRSPEED_AIR_DATA
stateSetAirspeed_f(&air_data.airspeed);
stateSetAirspeed_f(air_data.airspeed);
#endif
}
}
sw/airborne/modules/airborne_ant_track/airborne_ant_track.c
+2 -2
View File
@@ -100,8 +100,8 @@ void airborne_ant_point_periodic(void)
vSubtractVectors(&Home_PositionForPlane, Home_Position, svPlanePosition);
/* yaw */
smRotation.fx1 = (float)(cos((*stateGetHorizontalSpeedDir_f())));
smRotation.fx2 = (float)(sin((*stateGetHorizontalSpeedDir_f())));
smRotation.fx1 = cosf(stateGetHorizontalSpeedDir_f());
smRotation.fx2 = sinf(stateGetHorizontalSpeedDir_f());
smRotation.fx3 = 0.;
smRotation.fy1 = -smRotation.fx2;
smRotation.fy2 = smRotation.fx1;
sw/airborne/modules/benchmark/flight_benchmark.c
+1 -1
View File
@@ -59,7 +59,7 @@ void flight_benchmark_periodic(void)
if (benchm_go) {
#if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED)
Err_airspeed = fabs(*stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint);
Err_airspeed = fabs(stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint);
if (Err_airspeed > ToleranceAispeed) {
Err_airspeed = Err_airspeed - ToleranceAispeed;
SquareSumErr_airspeed += (Err_airspeed * Err_airspeed);
sw/airborne/modules/cam_control/cam.c
+1 -1
View File
@@ -270,7 +270,7 @@ void cam_target(void)
struct EnuCoor_f *pos = stateGetPositionEnu_f();
struct FloatEulers *att = stateGetNedToBodyEulers_f();
vPoint(pos->x, pos->y, stateGetPositionUtm_f()->alt,
att->phi, att->theta, *stateGetHorizontalSpeedDir_f(),
att->phi, att->theta, stateGetHorizontalSpeedDir_f(),
cam_target_x, cam_target_y, cam_target_alt,
&cam_pan_c, &cam_tilt_c);
#endif
sw/airborne/modules/cartography/cartography.c
+4 -4
View File
@@ -599,9 +599,9 @@ bool_t nav_survey_losange_carto(void)
course_next_rail = atan2(pointC.x - pointB.x, pointC.y - pointB.y);
PRTDEB(f, course_next_rail)
PRTDEB(f, (*stateGetHorizontalSpeedDir_f()))
PRTDEB(f, stateGetHorizontalSpeedDir_f())
angle_between = (course_next_rail - (*stateGetHorizontalSpeedDir_f()));
angle_between = (course_next_rail - stateGetHorizontalSpeedDir_f());
while (angle_between > M_PI) { angle_between -= 2 * M_PI; }
while (angle_between < -M_PI) { angle_between += 2 * M_PI; }
@@ -643,9 +643,9 @@ bool_t nav_survey_losange_carto(void)
course_next_rail = atan2(pointC.x - pointB.x, pointC.y - pointB.y);
PRTDEB(f, course_next_rail)
PRTDEB(f, (*stateGetHorizontalSpeedDir_f()))
PRTDEB(f, stateGetHorizontalSpeedDir_f())
angle_between = (course_next_rail - (*stateGetHorizontalSpeedDir_f()));
angle_between = (course_next_rail - stateGetHorizontalSpeedDir_f());
while (angle_between > M_PI) { angle_between -= 2 * M_PI; }
while (angle_between < -M_PI) { angle_between += 2 * M_PI; }
sw/airborne/modules/com/generic_com.c
+1 -1
View File
@@ -78,7 +78,7 @@ void generic_com_periodic(void)
FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps.lla_pos.alt / 1000)); // altitude (meters)
FillBufWith16bit(com_trans.buf, 11, gps.gspeed); // ground speed (cm/s)
FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course / 1e4)); // course (1e3rad)
FillBufWith16bit(com_trans.buf, 15, (uint16_t)((*stateGetAirspeed_f()) * 100)); // TAS (cm/s)
FillBufWith16bit(com_trans.buf, 15, (uint16_t)(stateGetAirspeed_f() * 100)); // TAS (cm/s)
com_trans.buf[17] = electrical.vsupply; // decivolts
com_trans.buf[18] = (uint8_t)(energy / 100); // deciAh
com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE] * 100 / MAX_PPRZ);
sw/airborne/modules/digital_cam/dc.c
+2 -2
View File
@@ -100,9 +100,9 @@ void dc_send_shot_position(void)
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
// course in decideg
int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
uint16_t speed = stateGetHorizontalSpeedNorm_f() * 10;
int16_t photo_nr = -1;
if (dc_photo_nr < DC_IMAGE_BUFFER) {
sw/airborne/modules/digital_cam/hackhd.c
+2 -2
View File
@@ -95,9 +95,9 @@ static inline void hackhd_send_shot_position(void)
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
// course in decideg
int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
uint16_t speed = stateGetHorizontalSpeedNorm_f() * 10;
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
&hackhd.photo_nr,
sw/airborne/modules/digital_cam/uart_cam_ctrl.c
+2 -2
View File
@@ -157,8 +157,8 @@ void dc_send_command(uint8_t cmd)
dc_shot_msg.data.phi = stateGetNedToBodyEulers_i()->phi;
dc_shot_msg.data.theta = stateGetNedToBodyEulers_i()->theta;
dc_shot_msg.data.psi = stateGetNedToBodyEulers_i()->psi;
dc_shot_msg.data.vground = *stateGetHorizontalSpeedNorm_i();
dc_shot_msg.data.course = *stateGetHorizontalSpeedDir_i();
dc_shot_msg.data.vground = stateGetHorizontalSpeedNorm_i();
dc_shot_msg.data.course = stateGetHorizontalSpeedDir_i();
dc_shot_msg.data.groundalt = POS_BFP_OF_REAL(state.alt_agl_f);
MoraHeader(MORA_SHOOT, MORA_SHOOT_MSG_SIZE);
sw/airborne/modules/enose/chemotaxis.c
+2 -2
View File
@@ -36,8 +36,8 @@ bool_t nav_chemotaxis(uint8_t c, uint8_t plume)
waypoints[c].y = waypoints[plume].y + ALPHA * y;
// DownlinkSendWp(c);
/* Turn in the right direction */
float dir_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float dir_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float dir_x = cos(M_PI_2 - stateGetHorizontalSpeedDir_f());
float dir_y = sin(M_PI_2 - stateGetHorizontalSpeedDir_f());
float pvect = dir_x * y - dir_y * x;
sign = (pvect > 0 ? -1 : 1);
/* Reduce the radius */
sw/airborne/modules/hott/hott_eam.h
+1 -1
View File
@@ -148,7 +148,7 @@ static void hott_update_eam_msg(struct HOTT_EAM_MSG *hott_eam_msg)
hott_eam_msg->current = electrical.current / 100;
hott_eam_msg->main_voltage = electrical.vsupply;
hott_eam_msg->batt_cap = 0;
uint16_t speed_buf = (uint16_t)(*stateGetHorizontalSpeedNorm_i() * 36 / 10 / (1 << INT32_SPEED_FRAC));
uint16_t speed_buf = (uint16_t)(stateGetHorizontalSpeedNorm_i() * 36 / 10 / (1 << INT32_SPEED_FRAC));
hott_eam_msg->speed_L = speed_buf && 0xFF;
hott_eam_msg->speed_H = (speed_buf >> 8) && 0xFF;
sw/airborne/modules/ins/ins_arduimu_basic.c
+1 -1
View File
@@ -111,7 +111,7 @@ void ArduIMU_periodicGPS(void)
// Test for high acceleration:
// - low speed
// - high thrust
float speed = *stateGetHorizontalSpeedNorm_f();
float speed = stateGetHorizontalSpeedNorm_f();
if (speed < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
sw/airborne/modules/multi/formation.c
+5 -5
View File
@@ -130,13 +130,13 @@ int formation_flight(void)
static uint8_t _1Hz = 0;
uint8_t nb = 0, i;
float hspeed_dir = (*stateGetHorizontalSpeedDir_f());
float hspeed_dir = stateGetHorizontalSpeedDir_f();
float ch = cosf(hspeed_dir);
float sh = sinf(hspeed_dir);
form_n = 0.;
form_e = 0.;
form_a = 0.;
form_speed = (*stateGetHorizontalSpeedNorm_f());
form_speed = stateGetHorizontalSpeedNorm_f();
form_speed_n = form_speed * ch;
form_speed_e = form_speed * sh;
@@ -212,9 +212,9 @@ int formation_flight(void)
form_n /= _nb;
form_e /= _nb;
form_a /= _nb;
form_speed = form_speed / (nb + 1) - (*stateGetHorizontalSpeedNorm_f());
//form_speed_e = form_speed_e / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * sh;
//form_speed_n = form_speed_n / (nb+1) - (*stateGetHorizontalSpeedNorm_f()) * ch;
form_speed = form_speed / (nb + 1) - stateGetHorizontalSpeedNorm_f();
//form_speed_e = form_speed_e / (nb+1) - stateGetHorizontalSpeedNorm_f() * sh;
//form_speed_n = form_speed_n / (nb+1) - stateGetHorizontalSpeedNorm_f() * ch;
// set commands
NavVerticalAutoThrottleMode(0.);
sw/airborne/modules/multi/potential.c
+4 -4
View File
@@ -58,8 +58,8 @@ int potential_task(void)
uint8_t i;
float ch = cosf((*stateGetHorizontalSpeedDir_f()));
float sh = sinf((*stateGetHorizontalSpeedDir_f()));
float ch = cosf(stateGetHorizontalSpeedDir_f());
float sh = sinf(stateGetHorizontalSpeedDir_f());
potential_force.east = 0.;
potential_force.north = 0.;
potential_force.alt = 0.;
@@ -83,8 +83,8 @@ int potential_task(void)
if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; }
float dist = sqrtf(de * de + dn * dn + da * da);
if (dist == 0.) { continue; }
float dve = (*stateGetHorizontalSpeedNorm_f()) * sh - ac->gspeed * sha;
float dvn = (*stateGetHorizontalSpeedNorm_f()) * ch - ac->gspeed * cha;
float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha;
float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha;
float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float scal = dve * de + dvn * dn + dva * da;
if (scal < 0.) { continue; } // No risk of collision
sw/airborne/modules/multi/tcas.c
+2 -2
View File
@@ -111,8 +111,8 @@ void tcas_periodic_task_1Hz(void)
float tau_min = tcas_tau_ta;
uint8_t ac_id_close = AC_ID;
uint8_t i;
float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f()));
float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f()));
float vx = stateGetHorizontalSpeedNorm_f() * sinf(stateGetHorizontalSpeedDir_f();
float vy = stateGetHorizontalSpeedNorm_f() * cosf(stateGetHorizontalSpeedDir_f();
for (i = 2; i < NB_ACS; i++) {
if (the_acs[i].ac_id == 0) { continue; } // no AC data
uint32_t dt = gps.tow - the_acs[i].itow;
sw/airborne/modules/nav/nav_bungee_takeoff.c
+3 -3
View File
@@ -169,7 +169,7 @@ bool_t nav_bungee_takeoff_run(void)
switch (CTakeoffStatus) {
case Launch:
// Recalculate lines if below min speed
if ((*stateGetHorizontalSpeedNorm_f()) < BUNGEE_TAKEOFF_MIN_SPEED) {
if (stateGetHorizontalSpeedNorm_f() < BUNGEE_TAKEOFF_MIN_SPEED) {
compute_points_from_bungee();
}
@@ -186,7 +186,7 @@ bool_t nav_bungee_takeoff_run(void)
VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point
cross = VECT2_DOT_PRODUCT(pos, takeoff_dir);
if (cross > 0. && (*stateGetHorizontalSpeedNorm_f()) > BUNGEE_TAKEOFF_MIN_SPEED) {
if (cross > 0. && stateGetHorizontalSpeedNorm_f() > BUNGEE_TAKEOFF_MIN_SPEED) {
CTakeoffStatus = Throttle;
kill_throttle = 0;
nav_init_stage();
@@ -204,7 +204,7 @@ bool_t nav_bungee_takeoff_run(void)
if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT)
#if USE_AIRSPEED
&& ((*stateGetAirspeed_f()) > BUNGEE_TAKEOFF_AIRSPEED)
&& (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED)
#endif
) {
CTakeoffStatus = Finished;
sw/airborne/modules/nav/nav_drop.c
+4 -4
View File
@@ -121,8 +121,8 @@ unit_t nav_drop_update_release(uint8_t wp_target)
nav_drop_x = 0.;
nav_drop_y = 0.;
nav_drop_vx = (*stateGetHorizontalSpeedNorm_f()) * sin((*stateGetHorizontalSpeedDir_f()));
nav_drop_vy = (*stateGetHorizontalSpeedNorm_f()) * cos((*stateGetHorizontalSpeedDir_f()));
nav_drop_vx = stateGetHorizontalSpeedNorm_f() * sin(stateGetHorizontalSpeedDir_f();
nav_drop_vy = stateGetHorizontalSpeedNorm_f() * cos(stateGetHorizontalSpeedDir_f();
nav_drop_vz = 0.;
integrate(wp_target);
@@ -161,8 +161,8 @@ unit_t nav_drop_compute_approach(uint8_t wp_target, uint8_t wp_start, uint8_t wp
// wind in NED frame
if (stateIsAirspeedValid()) {
nav_drop_vx = x1 **stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y;
nav_drop_vy = y_1 **stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x;
nav_drop_vx = x1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y;
nav_drop_vy = y_1 * stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x;
} else {
// use approximate airspeed, initially set to AIRSPEED_AT_RELEASE
nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y;
sw/airborne/modules/nav/nav_gls.c
+1 -1
View File
@@ -174,7 +174,7 @@ bool_t gls_run(uint8_t _af, uint8_t _sd, uint8_t _tod, uint8_t _td)
float final2 = Max(final_x * final_x + final_y * final_y, 1.);
struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
float hspeed = *stateGetHorizontalSpeedNorm_f();
float hspeed = stateGetHorizontalSpeedNorm_f();
float nav_final_progress = ((pos_enu->x - WaypointX(_tod)) * final_x +
(pos_enu->y - WaypointY(_tod)) * final_y) / final2;
sw/airborne/modules/nav/nav_smooth.c
+3 -3
View File
@@ -55,8 +55,8 @@ bool_t snav_init(uint8_t a, float desired_course_rad, float radius)
float da_y = WaypointY(wp_a) - stateGetPositionEnu_f()->y;
/* D_CD orthogonal to current course, CD on the side of A */
float u_x = cos(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float u_y = sin(M_PI_2 - (*stateGetHorizontalSpeedDir_f()));
float u_x = cos(M_PI_2 - stateGetHorizontalSpeedDir_f());
float u_y = sin(M_PI_2 - stateGetHorizontalSpeedDir_f());
d_radius = - Sign(u_x * da_y - u_y * da_x) * radius;
wp_cd.x = stateGetPositionEnu_f()->x + d_radius * u_y;
wp_cd.y = stateGetPositionEnu_f()->y - d_radius * u_x;
@@ -185,7 +185,7 @@ bool_t snav_on_time(float nominal_radius)
float remaining_time = snav_desired_tow - gps.tow / 1000.;
/* Use the nominal airspeed if the estimated one is not realistic */
float airspeed = *stateGetAirspeed_f();
float airspeed = stateGetAirspeed_f();
if (airspeed < NOMINAL_AIRSPEED / 2. ||
airspeed > 2.* NOMINAL_AIRSPEED) {
airspeed = NOMINAL_AIRSPEED;
sw/airborne/modules/sensors/airspeed_adc.c
+1 -1
View File
@@ -86,6 +86,6 @@ void airspeed_adc_update(void)
#endif //SITL
#if USE_AIRSPEED_ADC
stateSetAirspeed_f(&airspeed_adc.airspeed);
stateSetAirspeed_f(airspeed_adc.airspeed);
#endif
}
sw/airborne/modules/sensors/airspeed_amsys.c
+3 -3
View File
@@ -118,12 +118,12 @@ void airspeed_amsys_read_periodic(void)
}
#if USE_AIRSPEED_AMSYS
stateSetAirspeed_f(&airspeed_amsys);
stateSetAirspeed_f(airspeed_amsys);
#endif
#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
stateSetAirspeed_f(sim_air_speed);
#endif //SITL
@@ -208,7 +208,7 @@ void airspeed_amsys_read_event(void)
//New value available
#if USE_AIRSPEED
stateSetAirspeed_f(&airspeed_amsys);
stateSetAirspeed_f(airspeed_amsys);
#endif
#ifdef AIRSPEED_AMSYS_SYNC_SEND
airspeed_amsys_downlink();
sw/airborne/modules/sensors/airspeed_ets.c
+2 -2
View File
@@ -137,7 +137,7 @@ void airspeed_ets_read_periodic(void)
}
#elif !defined USE_NPS
extern float sim_air_speed;
stateSetAirspeed_f(&sim_air_speed);
stateSetAirspeed_f(sim_air_speed);
#endif //SITL
}
@@ -212,7 +212,7 @@ void airspeed_ets_read_event(void)
}
airspeed_ets = airspeed_ets / (float)AIRSPEED_ETS_NBSAMPLES_AVRG;
#if USE_AIRSPEED_ETS
stateSetAirspeed_f(&airspeed_ets);
stateSetAirspeed_f(airspeed_ets);
#endif
#if AIRSPEED_ETS_SYNC_SEND
DOWNLINK_SEND_AIRSPEED_ETS(DefaultChannel, DefaultDevice, &airspeed_ets_raw, &airspeed_ets_offset, &airspeed_ets);
sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c
+1 -1
View File
@@ -210,7 +210,7 @@ void ms45xx_i2c_event(void)
// Compute airspeed
ms45xx.airspeed = sqrtf(Max(ms45xx.diff_pressure * ms45xx.airspeed_scale, 0));
#if USE_AIRSPEED_MS45XX
stateSetAirspeed_f(&ms45xx.airspeed);
stateSetAirspeed_f(ms45xx.airspeed);
#endif
if (ms45xx.sync_send) {
ms45xx_downlink(&(DefaultChannel).trans_tx, &(DefaultDevice).device);
sw/airborne/modules/sensors/aoa_adc.c
+1 -1
View File
@@ -84,7 +84,7 @@ void aoa_adc_update(void)
prev_aoa = aoa_adc.angle;
#ifdef USE_AOA
stateSetAngleOfAttack_f(&aoa_adc.angle);
stateSetAngleOfAttack_f(aoa_adc.angle);
#endif
RunOnceEvery(30, DOWNLINK_SEND_AOA_ADC(DefaultChannel, DefaultDevice, &aoa_adc.raw, &aoa_adc.angle));
sw/airborne/modules/sensors/pressure_board_navarro.c
+1 -1
View File
@@ -151,7 +151,7 @@ void pbn_read_event(void)
pbn.airspeed = (pbn.airspeed_filter * pbn.airspeed + tmp_airspeed) /
(pbn.airspeed_filter + 1.);
#if USE_AIRSPEED_PBN
stateSetAirspeed_f(&pbn.airspeed);
stateSetAirspeed_f(pbn.airspeed);
#endif
pbn.altitude = PBN_ALTITUDE_SCALE * (float)(pbn.altitude_adc - pbn.altitude_offset);
sw/airborne/state.h
+26 -26
View File
@@ -396,8 +396,8 @@ struct State {
struct Int32Vect2 h_windspeed_i;
/**
* Norm of horizontal ground speed.
* @details Unit: m/s in BFP with #INT32_SPEED_FRAC
* Norm of relative wind speed.
* Unit: m/s in BFP with #INT32_SPEED_FRAC
*/
int32_t airspeed_i;
@@ -864,21 +864,21 @@ static inline struct EcefCoor_i *stateGetSpeedEcef_i(void)
}
/// Get norm of horizontal ground speed (int).
static inline uint32_t *stateGetHorizontalSpeedNorm_i(void)
static inline uint32_t stateGetHorizontalSpeedNorm_i(void)
{
if (!bit_is_set(state.speed_status, SPEED_HNORM_I)) {
stateCalcHorizontalSpeedNorm_i();
}
return &state.h_speed_norm_i;
return state.h_speed_norm_i;
}
/// Get dir of horizontal ground speed (int).
static inline int32_t *stateGetHorizontalSpeedDir_i(void)
static inline int32_t stateGetHorizontalSpeedDir_i(void)
{
if (!bit_is_set(state.speed_status, SPEED_HDIR_I)) {
stateCalcHorizontalSpeedDir_i();
}
return &state.h_speed_dir_i;
return state.h_speed_dir_i;
}
/// Get ground speed in local NED coordinates (float).
@@ -909,21 +909,21 @@ static inline struct EcefCoor_f *stateGetSpeedEcef_f(void)
}
/// Get norm of horizontal ground speed (float).
static inline float *stateGetHorizontalSpeedNorm_f(void)
static inline float stateGetHorizontalSpeedNorm_f(void)
{
if (!bit_is_set(state.speed_status, SPEED_HNORM_F)) {
stateCalcHorizontalSpeedNorm_f();
}
return &state.h_speed_norm_f;
return state.h_speed_norm_f;
}
/// Get dir of horizontal ground speed (float).
static inline float *stateGetHorizontalSpeedDir_f(void)
static inline float stateGetHorizontalSpeedDir_f(void)
{
if (!bit_is_set(state.speed_status, SPEED_HDIR_F)) {
stateCalcHorizontalSpeedDir_f();
}
return &state.h_speed_dir_f;
return state.h_speed_dir_f;
}
/** @}*/
@@ -1233,9 +1233,9 @@ static inline void stateSetHorizontalWindspeed_i(struct Int32Vect2 *h_windspeed)
}
/// Set airspeed (int).
static inline void stateSetAirspeed_i(int32_t *airspeed)
static inline void stateSetAirspeed_i(int32_t airspeed)
{
state.airspeed_i = *airspeed;
state.airspeed_i = airspeed;
/* clear bits for all airspeed representations and only set the new one */
ClearBit(state.wind_air_status, AIRSPEED_F);
SetBit(state.wind_air_status, AIRSPEED_I);
@@ -1251,27 +1251,27 @@ static inline void stateSetHorizontalWindspeed_f(struct FloatVect2 *h_windspeed)
}
/// Set airspeed (float).
static inline void stateSetAirspeed_f(float *airspeed)
static inline void stateSetAirspeed_f(float airspeed)
{
state.airspeed_f = *airspeed;
state.airspeed_f = airspeed;
/* clear bits for all airspeed representations and only set the new one */
ClearBit(state.wind_air_status, AIRSPEED_I);
SetBit(state.wind_air_status, AIRSPEED_F);
}
/// Set angle of attack in radians (float).
static inline void stateSetAngleOfAttack_f(float *aoa)
static inline void stateSetAngleOfAttack_f(float aoa)
{
state.angle_of_attack_f = *aoa;
state.angle_of_attack_f = aoa;
/* clear bits for all AOA representations and only set the new one */
/// @todo no integer yet
SetBit(state.wind_air_status, AOA_F);
}
/// Set sideslip angle in radians (float).
static inline void stateSetSideslip_f(float *sideslip)
static inline void stateSetSideslip_f(float sideslip)
{
state.sideslip_f = *sideslip;
state.sideslip_f = sideslip;
/* clear bits for all sideslip representations and only set the new one */
/// @todo no integer yet
SetBit(state.wind_air_status, SIDESLIP_F);
@@ -1289,12 +1289,12 @@ static inline struct Int32Vect2 *stateGetHorizontalWindspeed_i(void)
}
/// Get airspeed (int).
static inline int32_t *stateGetAirspeed_i(void)
static inline int32_t stateGetAirspeed_i(void)
{
if (!bit_is_set(state.wind_air_status, AIRSPEED_I)) {
stateCalcAirspeed_i();
}
return &state.airspeed_i;
return state.airspeed_i;
}
/// Get horizontal windspeed (float).
@@ -1307,30 +1307,30 @@ static inline struct FloatVect2 *stateGetHorizontalWindspeed_f(void)
}
/// Get airspeed (float).
static inline float *stateGetAirspeed_f(void)
static inline float stateGetAirspeed_f(void)
{
if (!bit_is_set(state.wind_air_status, AIRSPEED_F)) {
stateCalcAirspeed_f();
}
return &state.airspeed_f;
return state.airspeed_f;
}
/// Get angle of attack (float).
static inline float *stateGetAngleOfAttack_f(void)
static inline float stateGetAngleOfAttack_f(void)
{
/// @todo only float for now
// if (!bit_is_set(state.wind_air_status, AOA_F))
// stateCalcAOA_f();
return &state.angle_of_attack_f;
return state.angle_of_attack_f;
}
/// Get sideslip (float).
static inline float *stateGetSideslip_f(void)
static inline float stateGetSideslip_f(void)
{
/// @todo only float for now
// if (!bit_is_set(state.wind_air_status, SIDESLIP_F))
// stateCalcSideslip_f();
return &state.sideslip_f;
return state.sideslip_f;
}
/** @}*/
sw/airborne/subsystems/navigation/nav_survey_rectangle.c
+2 -2
View File
@@ -62,7 +62,7 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie
survey_from.x = survey_to.x = Min(Max(stateGetPositionEnu_f()->x, nav_survey_west + grid / 2.),
nav_survey_east - grid / 2.);
if (stateGetPositionEnu_f()->y > nav_survey_north || (stateGetPositionEnu_f()->y > nav_survey_south
&& (*stateGetHorizontalSpeedDir_f()) > M_PI / 2. && (*stateGetHorizontalSpeedDir_f()) < 3 * M_PI / 2)) {
&& stateGetHorizontalSpeedDir_f() > M_PI / 2. && stateGetHorizontalSpeedDir_f() < 3 * M_PI / 2)) {
survey_to.y = nav_survey_south;
survey_from.y = nav_survey_north;
} else {
@@ -73,7 +73,7 @@ void nav_survey_rectangle_init(uint8_t wp1, uint8_t wp2, float grid, survey_orie
survey_from.y = survey_to.y = Min(Max(stateGetPositionEnu_f()->y, nav_survey_south + grid / 2.),
nav_survey_north - grid / 2.);
if (stateGetPositionEnu_f()->x > nav_survey_east || (stateGetPositionEnu_f()->x > nav_survey_west
&& (*stateGetHorizontalSpeedDir_f()) > M_PI)) {
&& stateGetHorizontalSpeedDir_f() > M_PI)) {
survey_to.x = nav_survey_west;
survey_from.x = nav_survey_east;
} else {