zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

fixed code style

sbg
Andreas Antener 9 years ago
parent
f11a619bf8
commit
48e2053f98
1 changed files with 38 additions and 34 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 72
      src/modules/mc_pos_control/mc_pos_control_main.cpp

72
src/modules/mc_pos_control/mc_pos_control_main.cpp
Unescape View File

@ -918,6 +918,7 @@ void MulticopterPositionControl::control_auto(float dt)
_reset_pos_sp = true; _reset_pos_sp = true;
_reset_alt_sp = true; _reset_alt_sp = true;
} }
reset_pos_sp(); reset_pos_sp();
reset_alt_sp(); reset_alt_sp();
/* set current velocity as last target velocity to smooth out transition */ /* set current velocity as last target velocity to smooth out transition */
@ -933,8 +934,8 @@ void MulticopterPositionControl::control_auto(float dt)
//Make sure that the position setpoint is valid //Make sure that the position setpoint is valid
if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) ||
!PX4_ISFINITE(_pos_sp_triplet.current.lon) || !PX4_ISFINITE(_pos_sp_triplet.current.lon) ||
!PX4_ISFINITE(_pos_sp_triplet.current.alt)) { !PX4_ISFINITE(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false; _pos_sp_triplet.current.valid = false;
} }
} }
@ -954,8 +955,8 @@ void MulticopterPositionControl::control_auto(float dt)
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
if (PX4_ISFINITE(curr_sp(0)) && if (PX4_ISFINITE(curr_sp(0)) &&
PX4_ISFINITE(curr_sp(1)) && PX4_ISFINITE(curr_sp(1)) &&
PX4_ISFINITE(curr_sp(2))) { PX4_ISFINITE(curr_sp(2))) {
current_setpoint_valid = true; current_setpoint_valid = true;
} }
} }
@ -967,8 +968,8 @@ void MulticopterPositionControl::control_auto(float dt)
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if (PX4_ISFINITE(prev_sp(0)) && if (PX4_ISFINITE(prev_sp(0)) &&
PX4_ISFINITE(prev_sp(1)) && PX4_ISFINITE(prev_sp(1)) &&
PX4_ISFINITE(prev_sp(2))) { PX4_ISFINITE(prev_sp(2))) {
previous_setpoint_valid = true; previous_setpoint_valid = true;
} }
} }
@ -1078,7 +1079,7 @@ void MulticopterPositionControl::control_auto(float dt)
_att_sp.yaw_body = _pos_sp_triplet.current.yaw; _att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} }
/* /*
* if we're already near the current takeoff setpoint don't reset in case we switch back to posctl. * if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
* this makes the takeoff finish smoothly. * this makes the takeoff finish smoothly.
*/ */
@ -1090,7 +1091,8 @@ void MulticopterPositionControl::control_auto(float dt)
_reset_pos_sp = false; _reset_pos_sp = false;
_reset_alt_sp = false; _reset_alt_sp = false;
/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */ /* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
} else { } else {
_reset_pos_sp = true; _reset_pos_sp = true;
_reset_alt_sp = true; _reset_alt_sp = true;
@ -1205,8 +1207,8 @@ MulticopterPositionControl::task_main()
if (_local_pos.timestamp > 0) { if (_local_pos.timestamp > 0) {
if (PX4_ISFINITE(_local_pos.x) && if (PX4_ISFINITE(_local_pos.x) &&
PX4_ISFINITE(_local_pos.y) && PX4_ISFINITE(_local_pos.y) &&
PX4_ISFINITE(_local_pos.z)) { PX4_ISFINITE(_local_pos.z)) {
_pos(0) = _local_pos.x; _pos(0) = _local_pos.x;
_pos(1) = _local_pos.y; _pos(1) = _local_pos.y;
@ -1214,8 +1216,8 @@ MulticopterPositionControl::task_main()
} }
if (PX4_ISFINITE(_local_pos.vx) && if (PX4_ISFINITE(_local_pos.vx) &&
PX4_ISFINITE(_local_pos.vy) && PX4_ISFINITE(_local_pos.vy) &&
PX4_ISFINITE(_local_pos.vz)) { PX4_ISFINITE(_local_pos.vz)) {
_vel(0) = _local_pos.vx; _vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy; _vel(1) = _local_pos.vy;
@ -1228,9 +1230,9 @@ MulticopterPositionControl::task_main()
} }
if (_control_mode.flag_control_altitude_enabled || if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled || _control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) { _control_mode.flag_control_velocity_enabled) {
_vel_ff.zero(); _vel_ff.zero();
@ -1266,15 +1268,16 @@ MulticopterPositionControl::task_main()
} }
/* weather-vane mode for vtol: disable yaw control */ /* weather-vane mode for vtol: disable yaw control */
if(!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) { if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.disable_mc_yaw_control == true) {
_att_sp.disable_mc_yaw_control = true; _att_sp.disable_mc_yaw_control = true;
} else { } else {
/* reset in case of setpoint updates */ /* reset in case of setpoint updates */
_att_sp.disable_mc_yaw_control = false; _att_sp.disable_mc_yaw_control = false;
} }
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */ /* idle state, don't run controller and set zero thrust */
R.identity(); R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
@ -1318,6 +1321,7 @@ MulticopterPositionControl::task_main()
/* publish attitude setpoint */ /* publish attitude setpoint */
if (_att_sp_pub != nullptr) { if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) { } else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp); _att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
} }
@ -1371,18 +1375,18 @@ MulticopterPositionControl::task_main()
/* use constant descend rate when landing, ignore altitude setpoint */ /* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed; _vel_sp(2) = _params.land_speed;
} }
/* special thrust setpoint generation for takeoff from ground */ /* special thrust setpoint generation for takeoff from ground */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
&& _control_mode.flag_armed) { && _control_mode.flag_armed) {
// check if we are not already in air. // check if we are not already in air.
// if yes then we don't need a jumped takeoff anymore // if yes then we don't need a jumped takeoff anymore
if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON ) { if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON) {
_takeoff_jumped = true; _takeoff_jumped = true;
} }
@ -1392,6 +1396,7 @@ MulticopterPositionControl::task_main()
_takeoff_thrust_sp += 0.5f * dt; _takeoff_thrust_sp += 0.5f * dt;
_vel_sp.zero(); _vel_sp.zero();
_vel_prev.zero(); _vel_prev.zero();
} else { } else {
// copter has reached our takeoff speed. split the thrust setpoint up // copter has reached our takeoff speed. split the thrust setpoint up
// into an integral part and into a P part // into an integral part and into a P part
@ -1530,7 +1535,7 @@ MulticopterPositionControl::task_main()
/* adjust limits for landing mode */ /* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */ /* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land; tilt_max = _params.tilt_max_land;
@ -1540,15 +1545,14 @@ MulticopterPositionControl::task_main()
/* descend stabilized, we're landing */ /* descend stabilized, we're landing */
if (!_in_landing && !_lnd_reached_ground if (!_in_landing && !_lnd_reached_ground
&& (float)fabs(_acc_z_lp) < 0.1f && (float)fabs(_acc_z_lp) < 0.1f
&& _vel_z_lp > 0.5f * _params.land_speed) { && _vel_z_lp > 0.5f * _params.land_speed) {
_in_landing = true; _in_landing = true;
} }
/* assume ground, cut thrust */ /* assume ground, cut thrust */
if (_in_landing if (_in_landing
&& _vel_z_lp < 0.1f && _vel_z_lp < 0.1f) {
) {
thr_max = 0.0f; thr_max = 0.0f;
_in_landing = false; _in_landing = false;
_lnd_reached_ground = true; _lnd_reached_ground = true;
@ -1565,8 +1569,7 @@ MulticopterPositionControl::task_main()
if (_lnd_reached_ground if (_lnd_reached_ground
/* XXX: magic value, assuming free fall above 4m/s2 acceleration */ /* XXX: magic value, assuming free fall above 4m/s2 acceleration */
&& (_acc_z_lp > 4.0f && (_acc_z_lp > 4.0f
|| _vel_z_lp > 2.0f * _params.land_speed) || _vel_z_lp > 2.0f * _params.land_speed)) {
) {
thr_max = _params.thr_max; thr_max = _params.thr_max;
_in_landing = false; _in_landing = false;
_lnd_reached_ground = false; _lnd_reached_ground = false;
@ -1808,10 +1811,11 @@ MulticopterPositionControl::task_main()
/* do not move yaw while sitting on the ground */ /* do not move yaw while sitting on the ground */
else if (!_vehicle_status.condition_landed && else if (!_vehicle_status.condition_landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
/* we want to know the real constraint, and global overrides manual */ /* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : _params.global_yaw_max; const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
_params.global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p; const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max; _att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
@ -1851,8 +1855,8 @@ MulticopterPositionControl::task_main()
/* reset the acceleration set point for all non-attitude flight modes */ /* reset the acceleration set point for all non-attitude flight modes */
if (!(_control_mode.flag_control_offboard_enabled && if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled || !(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) { _control_mode.flag_control_velocity_enabled))) {
_thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust); _thrust_sp_prev = R_sp * math::Vector<3>(0, 0, -_att_sp.thrust);
} }
@ -1877,8 +1881,8 @@ MulticopterPositionControl::task_main()
* attitude setpoints for the transition). * attitude setpoints for the transition).
*/ */
if (!(_control_mode.flag_control_offboard_enabled && if (!(_control_mode.flag_control_offboard_enabled &&
!(_control_mode.flag_control_position_enabled || !(_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled))) { _control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr) { if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp); orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

Write Preview
Loading…
Cancel
Save