From 759d2a3dff05b56aacb616294a9253cdc77a5ed6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 8 Jun 2015 15:51:09 +0200 Subject: [PATCH 001/155] mc position control: - directly use commanded velocity for control - use position error to derive desired velocity when in position hold mode --- .../mc_pos_control/mc_pos_control_main.cpp | 61 +++++++++++++++++-- 1 file changed, 57 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 36d87b9383..ec833fc3c4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -90,6 +90,9 @@ #define SIGMA 0.000001f #define MIN_DIST 0.01f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f +#define VEL_XY_THRESH 0.5f // max xy velocity for which pos hold in xy is engaged +#define VEL_Z_THRESH 0.5f // max z velocity for which pos hold in z is engaged +#define VEL_CMD_THRESH 0.1f // min velocity (in xy/z) which is interpreted as velocity command /** * Multicopter position control app start / stop handling function @@ -206,6 +209,8 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; bool _mode_auto; + bool _pos_hold_engaged; + bool _alt_hold_engaged; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -325,7 +330,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_pos_sp(true), _reset_alt_sp(true), - _mode_auto(false) + _mode_auto(false), + _pos_hold_engaged(false), + _alt_hold_engaged(false) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_att, 0, sizeof(_att)); @@ -666,7 +673,6 @@ MulticopterPositionControl::control_manual(float dt) _vel_ff = _sp_move_rate.emult(_params.vel_ff); /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; @@ -1061,9 +1067,56 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, calculate velocity setpoint */ - math::Vector<3> pos_err = _pos_sp - _pos; - _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + // check if we can engage position hold, do xy and z separately + float xy_vel_sp = sqrtf(_sp_move_rate(0)*_sp_move_rate(0) + _sp_move_rate(1)*_sp_move_rate(1)); + float xy_vel = sqrtf(_vel(0)*_vel(0) + _vel(1)*_vel(1)); + + /* in manual mode: if user commands velocity in xy plane then use this directly + as velocity setpoint. If user commands zero velocity and vehicle has dropped it's speed + then lock to current position and use position error to calculate velocity setpoint. + Do this separately for horizontal / vertical movement. + TODO: Consider doing all three axes separately. + */ + + // horizontal movement + if (xy_vel_sp < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) { + if (xy_vel < VEL_XY_THRESH && !_pos_hold_engaged) { + _pos_hold_engaged = true; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + } else { + _pos_hold_engaged = false; + } + + // vertical movement + if (fabsf(_sp_move_rate(2)) < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) { + if (fabsf(_vel(2)) < VEL_Z_THRESH && !_alt_hold_engaged) { + _alt_hold_engaged = true; + _pos_sp(2) = _pos(2); + } + } else { + _alt_hold_engaged = false; + } + + math::Vector<3> pos_err; + if (_pos_hold_engaged || !_control_mode.flag_control_manual_enabled) { + pos_err(0) = _pos_sp(0) - _pos(0); + pos_err(1) = _pos_sp(1) - _pos(1); + _vel_sp(0) = pos_err(0) * _params.pos_p(0) + _vel_ff(0); + _vel_sp(1) = pos_err(1) * _params.pos_p(1) + _vel_ff(1); + } else { + _vel_sp(0) = _sp_move_rate(0); + _vel_sp(1) = _sp_move_rate(1); + } + + if (_alt_hold_engaged || !_control_mode.flag_control_manual_enabled) { + pos_err(2) = _pos_sp(2) - _pos(2); + _vel_sp(2) = pos_err(2) * _params.pos_p(2) + _vel_ff(2); + } else { + _vel_sp(2) = _sp_move_rate(2); + } /* make sure velocity setpoint is saturated in xy*/ float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + From 324c27b9414f1636622cdf5f4bf5989cd2a09f13 Mon Sep 17 00:00:00 2001 From: v01d Date: Mon, 28 Sep 2015 16:59:27 -0300 Subject: [PATCH 002/155] velocity control implemented: tested in SITL under manual mode --- .../mc_pos_control/mc_pos_control_main.cpp | 197 ++++++++---------- 1 file changed, 89 insertions(+), 108 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index ec833fc3c4..fbf876fa10 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -218,7 +218,6 @@ private: math::Vector<3> _vel_sp; math::Vector<3> _vel_prev; /**< velocity on previous step */ math::Vector<3> _vel_ff; - math::Vector<3> _sp_move_rate; /** * Update our local parameter cache. @@ -361,7 +360,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_sp.zero(); _vel_prev.zero(); _vel_ff.zero(); - _sp_move_rate.zero(); _params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_max = param_find("MPC_THR_MAX"); @@ -591,9 +589,9 @@ MulticopterPositionControl::reset_pos_sp() _reset_pos_sp = false; /* shift position setpoint to make attitude setpoint continuous */ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0) - - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); + - _params.vel_ff(0) * _vel_sp(0)) / _params.pos_p(0); _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1) - - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); + - _params.vel_ff(1) * _vel_sp(1)) / _params.pos_p(1); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1)); } } @@ -603,7 +601,7 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); + _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _vel_sp(2)) / _params.pos_p(2); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2)); } } @@ -634,31 +632,20 @@ MulticopterPositionControl::limit_pos_sp_offset() void MulticopterPositionControl::control_manual(float dt) { - _sp_move_rate.zero(); + math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range + req_vel_sp.zero(); if (_control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with throttle stick */ - _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); + /* set vertical velocity setpoint with throttle stick */ + req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); // D } if (_control_mode.flag_control_position_enabled) { - /* move position setpoint with roll/pitch stick */ - _sp_move_rate(0) = _manual.x; - _sp_move_rate(1) = _manual.y; + /* set horizontal velocity setpoint with roll/pitch stick */ + req_vel_sp(0) = _manual.x; + req_vel_sp(1) = _manual.y; } - /* limit setpoint move rate */ - float sp_move_norm = _sp_move_rate.length(); - - if (sp_move_norm > 1.0f) { - _sp_move_rate /= sp_move_norm; - } - - /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ - math::Matrix<3, 3> R_yaw_sp; - R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); - _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); - if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -669,29 +656,75 @@ MulticopterPositionControl::control_manual(float dt) reset_pos_sp(); } - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); + /* limit velocity setpoint */ + float req_vel_sp_norm = req_vel_sp.length(); - /* move position setpoint */ - - /* check if position setpoint is too far from actual position */ - math::Vector<3> pos_sp_offs; - pos_sp_offs.zero(); - - if (_control_mode.flag_control_position_enabled) { - pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); - pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + if (req_vel_sp_norm > 1.0f) { + req_vel_sp /= req_vel_sp_norm; } - if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + /* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */ + math::Matrix<3, 3> R_yaw_sp; + R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity + + /* + * assisted velocity mode: user controls velocity, but if velocity is small enough, position + * hold is activated for the corresponding axis + */ + + /* horizontal axes */ + if (_control_mode.flag_control_position_enabled) + { + /* check for pos. hold */ + if (fabsf(req_vel_sp(0)) < VEL_CMD_THRESH && fabsf(req_vel_sp(1)) < VEL_CMD_THRESH) + { + if (!_pos_hold_engaged && fabsf(_vel(0)) < VEL_XY_THRESH && fabsf(_vel(1)) < VEL_XY_THRESH) + { + _pos_hold_engaged = true; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); + } + } + else { + _pos_hold_engaged = false; + } + + /* compute velocity/position setpoint */ + if (_pos_hold_engaged) { + _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); + _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); + } + else { + _vel_sp(0) = req_vel_sp_scaled(0); + _vel_sp(1) = req_vel_sp_scaled(1); + } } - float pos_sp_offs_norm = pos_sp_offs.length(); + /* vertical axis */ + if (_control_mode.flag_control_altitude_enabled) + { + /* check for pos. hold */ + if (fabsf(req_vel_sp(2)) < VEL_CMD_THRESH) + { + if (!_alt_hold_engaged && fabsf(_vel(2)) < VEL_Z_THRESH) + { + _alt_hold_engaged = true; + _pos_hold_engaged = true; + _pos_sp(2) = _pos(2); + } + } + else { + _alt_hold_engaged = false; + } - if (pos_sp_offs_norm > 1.0f) { - pos_sp_offs /= pos_sp_offs_norm; - _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + /* compute velocity/position setpoint */ + if (_alt_hold_engaged) { + _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); + } + else { + _vel_sp(2) = req_vel_sp_scaled(2); + } } } @@ -716,8 +749,8 @@ MulticopterPositionControl::control_offboard(float dt) reset_pos_sp(); /* set position setpoint move rate */ - _sp_move_rate(0) = _pos_sp_triplet.current.vx; - _sp_move_rate(1) = _pos_sp_triplet.current.vy; + _vel_sp(0) = _pos_sp_triplet.current.vx; + _vel_sp(1) = _pos_sp_triplet.current.vy; } if (_pos_sp_triplet.current.yaw_valid) { @@ -734,15 +767,8 @@ MulticopterPositionControl::control_offboard(float dt) reset_alt_sp(); /* set altitude setpoint move rate */ - _sp_move_rate(2) = _pos_sp_triplet.current.vz; + _vel_sp(2) = _pos_sp_triplet.current.vz; } - - /* feed forward setpoint move rate with weight vel_ff */ - _vel_ff = _sp_move_rate.emult(_params.vel_ff); - - /* move position setpoint */ - _pos_sp += _sp_move_rate * dt; - } else { reset_pos_sp(); reset_alt_sp(); @@ -1026,7 +1052,6 @@ MulticopterPositionControl::task_main() _vel(2) = _local_pos.vz; _vel_ff.zero(); - _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { @@ -1066,57 +1091,7 @@ MulticopterPositionControl::task_main() } } else { - /* run position & altitude controllers, calculate velocity setpoint */ - - // check if we can engage position hold, do xy and z separately - float xy_vel_sp = sqrtf(_sp_move_rate(0)*_sp_move_rate(0) + _sp_move_rate(1)*_sp_move_rate(1)); - float xy_vel = sqrtf(_vel(0)*_vel(0) + _vel(1)*_vel(1)); - - /* in manual mode: if user commands velocity in xy plane then use this directly - as velocity setpoint. If user commands zero velocity and vehicle has dropped it's speed - then lock to current position and use position error to calculate velocity setpoint. - Do this separately for horizontal / vertical movement. - TODO: Consider doing all three axes separately. - */ - - // horizontal movement - if (xy_vel_sp < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) { - if (xy_vel < VEL_XY_THRESH && !_pos_hold_engaged) { - _pos_hold_engaged = true; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); - } - } else { - _pos_hold_engaged = false; - } - - // vertical movement - if (fabsf(_sp_move_rate(2)) < VEL_CMD_THRESH && _control_mode.flag_control_manual_enabled) { - if (fabsf(_vel(2)) < VEL_Z_THRESH && !_alt_hold_engaged) { - _alt_hold_engaged = true; - _pos_sp(2) = _pos(2); - } - } else { - _alt_hold_engaged = false; - } - - math::Vector<3> pos_err; - if (_pos_hold_engaged || !_control_mode.flag_control_manual_enabled) { - pos_err(0) = _pos_sp(0) - _pos(0); - pos_err(1) = _pos_sp(1) - _pos(1); - _vel_sp(0) = pos_err(0) * _params.pos_p(0) + _vel_ff(0); - _vel_sp(1) = pos_err(1) * _params.pos_p(1) + _vel_ff(1); - } else { - _vel_sp(0) = _sp_move_rate(0); - _vel_sp(1) = _sp_move_rate(1); - } - - if (_alt_hold_engaged || !_control_mode.flag_control_manual_enabled) { - pos_err(2) = _pos_sp(2) - _pos(2); - _vel_sp(2) = pos_err(2) * _params.pos_p(2) + _vel_ff(2); - } else { - _vel_sp(2) = _sp_move_rate(2); - } + /* run position & altitude controllers, position/velocity setpoint already computed in control_* functions */ /* make sure velocity setpoint is saturated in xy*/ float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + @@ -1133,17 +1108,23 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; } - if (!_control_mode.flag_control_altitude_enabled) { - _reset_alt_sp = true; - _vel_sp(2) = 0.0f; - } - if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; + } + + if (!_control_mode.flag_control_altitude_enabled) { + _reset_alt_sp = true; + } + + if (!_control_mode.flag_control_velocity_enabled) { _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } + if (!_control_mode.flag_control_climb_rate_enabled) { + _vel_sp(2) = 0.0f; + } + /* use constant descend rate when landing, ignore altitude setpoint */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; @@ -1201,7 +1182,7 @@ MulticopterPositionControl::task_main() math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; + math::Vector<3> vel_err_d = (_vel - _vel_prev) / dt; /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; From da3087e54cfb5a13ffda70794ee4e7a1f2bb2140 Mon Sep 17 00:00:00 2001 From: v01d Date: Tue, 29 Sep 2015 11:08:46 -0300 Subject: [PATCH 003/155] expose pos-hold parameters (also allows different behaviours) --- .../mc_pos_control/mc_pos_control_main.cpp | 26 ++++++++++++++--- .../mc_pos_control/mc_pos_control_params.c | 28 +++++++++++++++++++ 2 files changed, 50 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index fbf876fa10..de81716a1b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,9 @@ private: param_t man_pitch_max; param_t man_yaw_max; param_t mc_att_yaw_p; + param_t hold_dz; + param_t hold_max_xy; + param_t hold_max_z; } _params_handles; /**< handles for interesting parameters */ struct { @@ -192,6 +195,9 @@ private: float man_pitch_max; float man_yaw_max; float mc_att_yaw_p; + float hold_dz; + float hold_max_xy; + float hold_max_z; math::Vector<3> pos_p; math::Vector<3> vel_p; @@ -382,6 +388,10 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); + _params_handles.hold_dz = param_find("MPC_HOLD_DZ"); + _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); + _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); + /* fetch initial parameter values */ parameters_update(true); @@ -469,6 +479,13 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; + param_get(_params_handles.hold_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_dz = v; + param_get(_params_handles.hold_max_xy, &v); + _params.hold_max_xy = (v < 0.0f ? 0.0f : v); + param_get(_params_handles.hold_max_z, &v); + _params.hold_max_z = (v < 0.0f ? 0.0f : v); _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; @@ -677,9 +694,10 @@ MulticopterPositionControl::control_manual(float dt) if (_control_mode.flag_control_position_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(0)) < VEL_CMD_THRESH && fabsf(req_vel_sp(1)) < VEL_CMD_THRESH) + if (fabsf(req_vel_sp(0)) < _params.hold_dz && fabsf(req_vel_sp(1)) < _params.hold_dz) { - if (!_pos_hold_engaged && fabsf(_vel(0)) < VEL_XY_THRESH && fabsf(_vel(1)) < VEL_XY_THRESH) + if (!_pos_hold_engaged && (_params.hold_max_xy == 0.0f || + (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) { _pos_hold_engaged = true; _pos_sp(0) = _pos(0); @@ -705,9 +723,9 @@ MulticopterPositionControl::control_manual(float dt) if (_control_mode.flag_control_altitude_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(2)) < VEL_CMD_THRESH) + if (fabsf(req_vel_sp(2)) < _params.hold_dz) { - if (!_alt_hold_engaged && fabsf(_vel(2)) < VEL_Z_THRESH) + if (!_alt_hold_engaged && (_params.hold_max_z == 0.0f || fabsf(_vel(2)) < _params.hold_max_z)) { _alt_hold_engaged = true; _pos_hold_engaged = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index d29d11c65d..c99acfc89d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -265,3 +265,31 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f); */ PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f); +/** + * Deadzone of X,Y,Z where position hold is enabled + * + * @unit percent + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); + +/** + * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * @unit m/s + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); + From 42ccc654b58bec06945a6f7cf64013092e7ecb54 Mon Sep 17 00:00:00 2001 From: v01d Date: Tue, 29 Sep 2015 11:58:51 -0300 Subject: [PATCH 004/155] fix float comparison to 0.0f --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index de81716a1b..971eba063a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -696,7 +696,7 @@ MulticopterPositionControl::control_manual(float dt) /* check for pos. hold */ if (fabsf(req_vel_sp(0)) < _params.hold_dz && fabsf(req_vel_sp(1)) < _params.hold_dz) { - if (!_pos_hold_engaged && (_params.hold_max_xy == 0.0f || + if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) { _pos_hold_engaged = true; @@ -725,7 +725,7 @@ MulticopterPositionControl::control_manual(float dt) /* check for pos. hold */ if (fabsf(req_vel_sp(2)) < _params.hold_dz) { - if (!_alt_hold_engaged && (_params.hold_max_z == 0.0f || fabsf(_vel(2)) < _params.hold_max_z)) + if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z)) { _alt_hold_engaged = true; _pos_hold_engaged = true; From c010962861e9f2f654158a112edce30cefe5fc40 Mon Sep 17 00:00:00 2001 From: v01d Date: Tue, 29 Sep 2015 12:25:07 -0300 Subject: [PATCH 005/155] change unit of MPC_HOLD_DZ parameter to "%" --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index c99acfc89d..f68de3c645 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -268,7 +268,7 @@ PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f); /** * Deadzone of X,Y,Z where position hold is enabled * - * @unit percent + * @unit % * @min 0.0 * @max 1.0 * @group Multicopter Position Control From 9c49b30118ce489eae91422918a8064e4866ac46 Mon Sep 17 00:00:00 2001 From: v01d Date: Tue, 29 Sep 2015 15:39:36 -0300 Subject: [PATCH 006/155] run position controller in main_task, otherwise position setpoints will not be followed --- .../mc_pos_control/mc_pos_control_main.cpp | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 971eba063a..78e180c69a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -90,9 +90,6 @@ #define SIGMA 0.000001f #define MIN_DIST 0.01f #define MANUAL_THROTTLE_MAX_MULTICOPTER 0.9f -#define VEL_XY_THRESH 0.5f // max xy velocity for which pos hold in xy is engaged -#define VEL_Z_THRESH 0.5f // max z velocity for which pos hold in z is engaged -#define VEL_CMD_THRESH 0.1f // min velocity (in xy/z) which is interpreted as velocity command /** * Multicopter position control app start / stop handling function @@ -217,6 +214,8 @@ private: bool _mode_auto; bool _pos_hold_engaged; bool _alt_hold_engaged; + bool _run_pos_control; + bool _run_alt_control; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -337,7 +336,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _reset_alt_sp(true), _mode_auto(false), _pos_hold_engaged(false), - _alt_hold_engaged(false) + _alt_hold_engaged(false), + _run_pos_control(true), + _run_alt_control(true) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_att, 0, sizeof(_att)); @@ -708,12 +709,9 @@ MulticopterPositionControl::control_manual(float dt) _pos_hold_engaged = false; } - /* compute velocity/position setpoint */ - if (_pos_hold_engaged) { - _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); - _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); - } - else { + /* set requested velocity setpoint */ + if (!_pos_hold_engaged) { + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ _vel_sp(0) = req_vel_sp_scaled(0); _vel_sp(1) = req_vel_sp_scaled(1); } @@ -728,7 +726,6 @@ MulticopterPositionControl::control_manual(float dt) if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z)) { _alt_hold_engaged = true; - _pos_hold_engaged = true; _pos_sp(2) = _pos(2); } } @@ -736,11 +733,9 @@ MulticopterPositionControl::control_manual(float dt) _alt_hold_engaged = false; } - /* compute velocity/position setpoint */ - if (_alt_hold_engaged) { - _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); - } - else { + /* set requested velocity setpoint */ + if (!_alt_hold_engaged) { + _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ _vel_sp(2) = req_vel_sp_scaled(2); } } @@ -1071,6 +1066,11 @@ MulticopterPositionControl::task_main() _vel_ff.zero(); + /* by default, run position/altitude controller. the control_* functions + * can disable this and run velocity controllers directly in this cycle */ + _run_pos_control = true; + _run_alt_control = true; + /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ @@ -1109,7 +1109,15 @@ MulticopterPositionControl::task_main() } } else { - /* run position & altitude controllers, position/velocity setpoint already computed in control_* functions */ + /* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */ + if (_run_pos_control) { + _vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0); + _vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1); + } + + if (_run_alt_control) { + _vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2); + } /* make sure velocity setpoint is saturated in xy*/ float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + From 44bb50e9eae36713a37624e9686eca62221a0360 Mon Sep 17 00:00:00 2001 From: v01d Date: Tue, 29 Sep 2015 15:51:05 -0300 Subject: [PATCH 007/155] separate _DZ parameter to XY and Z. this allows disabling pos hold for XY and not Z --- .../mc_pos_control/mc_pos_control_main.cpp | 20 ++++++++++++------- .../mc_pos_control/mc_pos_control_params.c | 14 +++++++++++-- 2 files changed, 25 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 78e180c69a..adaf4a3fb6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -177,7 +177,8 @@ private: param_t man_pitch_max; param_t man_yaw_max; param_t mc_att_yaw_p; - param_t hold_dz; + param_t hold_xy_dz; + param_t hold_z_dz; param_t hold_max_xy; param_t hold_max_z; } _params_handles; /**< handles for interesting parameters */ @@ -192,7 +193,8 @@ private: float man_pitch_max; float man_yaw_max; float mc_att_yaw_p; - float hold_dz; + float hold_xy_dz; + float hold_z_dz; float hold_max_xy; float hold_max_z; @@ -389,7 +391,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); - _params_handles.hold_dz = param_find("MPC_HOLD_DZ"); + _params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ"); + _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); _params_handles.hold_max_xy = param_find("MPC_HOLD_MAX_XY"); _params_handles.hold_max_z = param_find("MPC_HOLD_MAX_Z"); @@ -480,9 +483,12 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.z_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; - param_get(_params_handles.hold_dz, &v); + param_get(_params_handles.hold_xy_dz, &v); v = math::constrain(v, 0.0f, 1.0f); - _params.hold_dz = v; + _params.hold_xy_dz = v; + param_get(_params_handles.hold_z_dz, &v); + v = math::constrain(v, 0.0f, 1.0f); + _params.hold_z_dz = v; param_get(_params_handles.hold_max_xy, &v); _params.hold_max_xy = (v < 0.0f ? 0.0f : v); param_get(_params_handles.hold_max_z, &v); @@ -695,7 +701,7 @@ MulticopterPositionControl::control_manual(float dt) if (_control_mode.flag_control_position_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(0)) < _params.hold_dz && fabsf(req_vel_sp(1)) < _params.hold_dz) + if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) @@ -721,7 +727,7 @@ MulticopterPositionControl::control_manual(float dt) if (_control_mode.flag_control_altitude_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(2)) < _params.hold_dz) + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) { if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z)) { diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index f68de3c645..09843de1f1 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -266,14 +266,24 @@ PARAM_DEFINE_FLOAT(MPC_MAN_P_MAX, 35.0f); PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 120.0f); /** - * Deadzone of X,Y,Z where position hold is enabled + * Deadzone of X,Y sticks where position hold is enabled * * @unit % * @min 0.0 * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); +PARAM_DEFINE_FLOAT(MPC_HOLD_XY_DZ, 0.1f); + +/** + * Deadzone of Z stick where altitude hold is enabled + * + * @unit % + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_Z_DZ, 0.1f); /** * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) From 8133f8d61626efe2679bdd21c6ebaee12d48022f Mon Sep 17 00:00:00 2001 From: v01d Date: Tue, 29 Sep 2015 16:05:34 -0300 Subject: [PATCH 008/155] disable pos/alt controllers when following velocity setpoints in offboard mode --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index adaf4a3fb6..299330eebf 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -770,6 +770,8 @@ MulticopterPositionControl::control_offboard(float dt) /* set position setpoint move rate */ _vel_sp(0) = _pos_sp_triplet.current.vx; _vel_sp(1) = _pos_sp_triplet.current.vy; + + _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } if (_pos_sp_triplet.current.yaw_valid) { @@ -787,6 +789,8 @@ MulticopterPositionControl::control_offboard(float dt) /* set altitude setpoint move rate */ _vel_sp(2) = _pos_sp_triplet.current.vz; + + _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } } else { reset_pos_sp(); From b3f912ecc1e9c8b410130ab689cf9a7a61c6b851 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 16 Oct 2015 12:38:53 +0200 Subject: [PATCH 009/155] reset position setpoint when in velocity control mode --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 299330eebf..60747854cd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -707,12 +707,12 @@ MulticopterPositionControl::control_manual(float dt) (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) { _pos_hold_engaged = true; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); } } else { _pos_hold_engaged = false; + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); } /* set requested velocity setpoint */ @@ -732,11 +732,11 @@ MulticopterPositionControl::control_manual(float dt) if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z)) { _alt_hold_engaged = true; - _pos_sp(2) = _pos(2); } } else { _alt_hold_engaged = false; + _pos_sp(2) = _pos(2); } /* set requested velocity setpoint */ From c9a3b7b9b0e2765accc56d59a15014748a0b2ecb Mon Sep 17 00:00:00 2001 From: tumbili Date: Sat, 17 Oct 2015 13:30:44 +0200 Subject: [PATCH 010/155] added config file SITL iris with gazebo --- Makefile | 3 ++ posix-configs/SITL/init/rcS_iris_gazebo | 59 +++++++++++++++++++++++++ 2 files changed, 62 insertions(+) create mode 100644 posix-configs/SITL/init/rcS_iris_gazebo diff --git a/Makefile b/Makefile index e2f1814f0c..bb5d915f7c 100644 --- a/Makefile +++ b/Makefile @@ -132,6 +132,9 @@ ros: ros_sitl_simple run_sitl_quad: posix Tools/sitl_run.sh posix-configs/SITL/init/rcS +run_sitl_iris: posix + Tools/sitl_run.sh posix-configs/SITL/init/rcS_iris_gazebo + run_sitl_plane: posix Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing diff --git a/posix-configs/SITL/init/rcS_iris_gazebo b/posix-configs/SITL/init/rcS_iris_gazebo new file mode 100644 index 0000000000..ca5415d110 --- /dev/null +++ b/posix-configs/SITL/init/rcS_iris_gazebo @@ -0,0 +1,59 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 +param set COM_RC_IN_MODE 2 +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a From 7eaef30c0b235ff8d3cf47dd10bfb49e9c6c20a5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Oct 2015 17:28:43 +0200 Subject: [PATCH 011/155] Add jMAVSim --- .gitmodules | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitmodules b/.gitmodules index 2820e68539..54f9f9f02a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -22,3 +22,6 @@ [submodule "src/lib/dspal"] path = src/lib/dspal url = https://github.com/mcharleb/dspal.git +[submodule "Tools/jMAVSim"] + path = Tools/jMAVSim + url = https://github.com/PX4/jMAVSim.git From 4d71e8c3302932202faea4419e831f5723fd4e9c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Oct 2015 17:30:58 +0200 Subject: [PATCH 012/155] Added jMAVSim as submodule --- Tools/jMAVSim | 1 + 1 file changed, 1 insertion(+) create mode 160000 Tools/jMAVSim diff --git a/Tools/jMAVSim b/Tools/jMAVSim new file mode 160000 index 0000000000..96029523d1 --- /dev/null +++ b/Tools/jMAVSim @@ -0,0 +1 @@ +Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9 From 08e37f79dc9dcb48f7e6bae5c8e1842cdbf89ad0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Oct 2015 17:33:27 +0200 Subject: [PATCH 013/155] Introduce jMAVSim as submodule --- CMakeLists.txt | 1 + src/platforms/posix/drivers/accelsim/CMakeLists.txt | 1 + 2 files changed, 2 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6aec7f2e0c..732496778f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -228,6 +228,7 @@ px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") +px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") add_custom_target(submodule_clean WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} diff --git a/src/platforms/posix/drivers/accelsim/CMakeLists.txt b/src/platforms/posix/drivers/accelsim/CMakeLists.txt index e5b1a92147..afa59e515f 100644 --- a/src/platforms/posix/drivers/accelsim/CMakeLists.txt +++ b/src/platforms/posix/drivers/accelsim/CMakeLists.txt @@ -37,5 +37,6 @@ px4_add_module( accelsim.cpp DEPENDS platforms__common + git_jmavsim ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From b61e95adfd901196f0cd40c90e26cecbc2635770 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Oct 2015 17:41:57 +0200 Subject: [PATCH 014/155] Automate SITL run --- Makefile | 6 +++--- Tools/sitl_run.sh | 8 ++++++++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index bb5d915f7c..635b0c3e9f 100644 --- a/Makefile +++ b/Makefile @@ -130,7 +130,7 @@ posix: posix_sitl_simple ros: ros_sitl_simple run_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS + Tools/sitl_run.sh posix-configs/SITL/init/rcS none jmavsim run_sitl_iris: posix Tools/sitl_run.sh posix-configs/SITL/init/rcS_iris_gazebo @@ -142,7 +142,7 @@ run_sitl_ros: posix Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS lldb + Tools/sitl_run.sh posix-configs/SITL/init/rcS lldb jmavsim lldb_sitl_plane: posix Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb @@ -151,7 +151,7 @@ lldb_sitl_ros: posix Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb gdb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS gdb + Tools/sitl_run.sh posix-configs/SITL/init/rcS gdb jmavsim gdb_sitl_plane: posix Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 1a7741d6fd..1d80464fad 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -1,10 +1,18 @@ #!/bin/bash cp Tools/posix_lldbinit build_posix_sitl_simple/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit build_posix_sitl_simple/src/firmware/posix/.gdbinit +if [ "$3" == "jmavsim" ] +then + cd Tools/jMAVSim + ant + java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & + cd ../.. +fi cd build_posix_sitl_simple/src/firmware/posix mkdir -p rootfs/fs/microsd mkdir -p rootfs/eeprom touch rootfs/eeprom/parameters +# Start Java simulator if [ "$2" == "lldb" ] then lldb -- mainapp ../../../../$1 From c4f9318e75d489cac4f5075d7d55bcbfeb28244b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Oct 2015 15:49:01 +0200 Subject: [PATCH 015/155] Q attitude estimator: Prevent division by zero --- src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index efe8a2ea99..a00aa4749b 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -398,7 +398,7 @@ void AttitudeEstimatorQ::task_main() // Time from previous iteration hrt_abstime now = hrt_absolute_time(); - float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.0f; + float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f; last_time = now; if (dt > _dt_max) { From 6b2ad2e2580afd097192b9cf71e24030ee88d330 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Oct 2015 16:10:33 +0200 Subject: [PATCH 016/155] Travis CI: Fail complete build on first fail --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 5806f9d232..40ee49690f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -4,6 +4,7 @@ language: cpp matrix: + fast_finish: true include: - os: linux sudo: false From 01588216709f25396f0e9bf5dd2703cf8dc265a1 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 18 Oct 2015 21:48:59 +0200 Subject: [PATCH 017/155] protect from zero division --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 766954c64a..52de4ef6f9 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -315,8 +315,12 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) // scale yaw if it violates limits. inform about yaw limit reached if (out < 0.0f) { - yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { + yaw = 0.0f; + } else { + yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + } if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; @@ -326,8 +330,12 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); thrust -= thrust_reduction; - yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { + yaw = 0.0f; + } else { + yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * + roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; + } if (status_reg != NULL) { (*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT; From 25dfa31374541e4a8e2e284dba8a7f281eaa05c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 09:37:01 +0200 Subject: [PATCH 018/155] Mixer: Fix code style --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 52de4ef6f9..38074f7ae5 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -317,9 +317,10 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) if (out < 0.0f) { if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { yaw = 0.0f; + } else { yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * - roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; + roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale; } if (status_reg != NULL) { @@ -330,8 +331,10 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg) // allow to reduce thrust to get some yaw response float thrust_reduction = fminf(0.15f, out - 1.0f); thrust -= thrust_reduction; + if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) { yaw = 0.0f; + } else { yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) * roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale; From b8a72bfe753384799bc757309a6335f82ad0f036 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 17 Oct 2015 00:47:16 -0400 Subject: [PATCH 019/155] travis-ci use ninja --- .travis.yml | 46 ++++++++++++---------------------------------- 1 file changed, 12 insertions(+), 34 deletions(-) diff --git a/.travis.yml b/.travis.yml index 40ee49690f..36f40a8d49 100644 --- a/.travis.yml +++ b/.travis.yml @@ -10,6 +10,7 @@ matrix: sudo: false - os: osx osx_image: beta-xcode6.3 + sudo: true cache: directories: @@ -23,13 +24,14 @@ addons: packages: - build-essential - ccache - - cmake - clang-3.5 + - cmake - g++-4.8 - gcc-4.8 - genromfs - libc6-i386 - libncurses5-dev + - ninja-build - python-argparse - python-empy - python-serial @@ -55,10 +57,11 @@ before_install: elif [ "${TRAVIS_OS_NAME}" = "osx" ]; then brew tap PX4/homebrew-px4 && brew update + && brew install astyle + && brew install gcc-arm-none-eabi && brew install genromfs && brew install kconfig-frontends - && brew install gcc-arm-none-eabi - && brew install astyle + && brew install ninja && sudo easy_install pip && sudo pip install pyserial empy ; @@ -81,6 +84,7 @@ before_script: env: global: + - NINJA_BUILD=1 # AWS KEY: $PX4_AWS_KEY - secure: "XknnZHWBbpHbN4f3fuAVwUztdLIu8ej4keC3aQSDofo3uw8AFEzojfsQsN9u77ShWSIV4iYJWh9C9ALkCx7TocJ+xYjiboo10YhM9lH/8u+EXjYWG6GHS8ua0wkir+cViSxoLNaMtmcb/rPTicJecAGANxLsIHyBAgTL3fkbLSA=" # AWS SECRET: $PX4_AWS_SECRET @@ -90,37 +94,11 @@ env: script: - make check_format - arm-none-eabi-gcc --version - - echo 'Building POSIX Firmware..' && echo -en 'travis_fold:start:script.1\\r' - - make posix_sitl_simple - - echo -en 'travis_fold:end:script.1\\r' - - echo 'Running Tests..' && echo -en 'travis_fold:start:script.2\\r' - - make posix_sitl_simple test - - cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h - - echo -en 'travis_fold:end:script.2\\r' - - echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.3\\r' - - make px4fmu-v1_default - - make px4fmu-v2_default - - echo -en 'travis_fold:end:script.3\\r' - - echo 'Running Tests..' && echo -en 'travis_fold:start:script.4\\r' - - make px4fmu-v2_default test - - echo -en 'travis_fold:end:script.4\\r' - #- make px4fmu-v2_default package - #- make posix -j4 - #- ccache -s - #- echo -en 'travis_fold:end:script.1\\r' - #- echo 'Running Tests..' && echo -en 'travis_fold:start:script.2\\r' - #- make tests - #- cat src/modules/systemlib/mixer/mixer_multirotor.generated.h - #- echo -en 'travis_fold:end:script.2\\r' - #- echo 'Building NuttX..' && echo -en 'travis_fold:start:script.3\\r' - #- make archives - #- ccache -s - #- echo -en 'travis_fold:end:script.3\\r' - #- echo 'Building NuttX Firmware..' && echo -en 'travis_fold:start:script.4\\r' - #- make -j4 - #- make size - #- ccache -s - #- echo -en 'travis_fold:end:script.4\\r' + - echo 'Building POSIX Firmware..' && make posix_sitl_simple + - echo 'Running Tests..' && make posix_sitl_simple test && cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h + - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default + - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default + - echo 'Running Tests..' && make px4fmu-v2_default test #- zip Firmware.zip Images/*.px4 #after_script: From d874b4bec6722cfa3f8e011bc27f7b47e0db6a8b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 17 Oct 2015 10:33:18 -0400 Subject: [PATCH 020/155] travis-ci s3 upload --- .travis.yml | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/.travis.yml b/.travis.yml index 36f40a8d49..9aeedc9fbe 100644 --- a/.travis.yml +++ b/.travis.yml @@ -99,22 +99,24 @@ script: - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test - #- zip Firmware.zip Images/*.px4 -#after_script: - #- git clone git://github.com/PX4/CI-Tools.git - #- ./CI-Tools/s3cmd-configure -## upload newest build for this branch with s3 index - #- ./CI-Tools/s3cmd-put Images/px4*.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ -## archive newest build by date with s3 index - #- ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ - #- ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ -## upload top level index.html and timestamp.html - #- ./CI-Tools/s3cmd-put CI-Tools/index.html index.html - #- ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html - #- echo "" - #- echo "Binaries have been posted to:" - #- echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip +after_success: + - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then + cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 + && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 + && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 + && git clone git://github.com/PX4/CI-Tools.git + && ./CI-Tools/s3cmd-configure + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ + && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ + && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ + && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html + && ./CI-Tools/s3cmd-put CI-Tools/timestamp.html timestamp.html + && echo "" + && echo "Binaries have been posted to:" + && echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip + ; + fi deploy: provider: releases From cbbb6a0e598c9a5c2d97b951ec5cf9bc1757fa1d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 18 Oct 2015 14:52:42 -0400 Subject: [PATCH 021/155] switch to official googletest repo --- .gitmodules | 6 +++--- unittests/googletest | 1 + unittests/gtest | 1 - 3 files changed, 4 insertions(+), 4 deletions(-) create mode 160000 unittests/googletest delete mode 160000 unittests/gtest diff --git a/.gitmodules b/.gitmodules index 54f9f9f02a..b05206ecfa 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,9 +13,6 @@ [submodule "Tools/gencpp"] path = Tools/gencpp url = https://github.com/ros/gencpp.git -[submodule "unittests/gtest"] - path = unittests/gtest - url = https://github.com/sjwilks/gtest.git [submodule "src/lib/eigen"] path = src/lib/eigen url = https://github.com/PX4/eigen.git @@ -25,3 +22,6 @@ [submodule "Tools/jMAVSim"] path = Tools/jMAVSim url = https://github.com/PX4/jMAVSim.git +[submodule "unittests/googletest"] + path = unittests/googletest + url = https://github.com/google/googletest.git diff --git a/unittests/googletest b/unittests/googletest new file mode 160000 index 0000000000..c99458533a --- /dev/null +++ b/unittests/googletest @@ -0,0 +1 @@ +Subproject commit c99458533a9b4c743ed51537e25989ea55944908 diff --git a/unittests/gtest b/unittests/gtest deleted file mode 160000 index cdef6e4ce0..0000000000 --- a/unittests/gtest +++ /dev/null @@ -1 +0,0 @@ -Subproject commit cdef6e4ce097f953445446e8da4cb8bb68478bc5 From 8e82ced5a4cb434bf20456c3af9296ac7f46cedf Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 16 Oct 2015 12:42:26 -0700 Subject: [PATCH 022/155] Added backtrace function Added PX4_BACKTRACE() to display stack trace for debugging Added backtrace for HRT reschedule with 0 timeout Signed-off-by: Mark Charlebois --- src/platforms/posix/px4_layer/drv_hrt.c | 1 + src/platforms/posix/px4_layer/px4_log.c | 21 +++++++++++++++++++++ src/platforms/posix/work_queue/hrt_thread.c | 1 + src/platforms/px4_log.h | 5 +++++ 4 files changed, 28 insertions(+) diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index 10792b3089..cae3c379da 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -367,6 +367,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte // Use this to debug busy CPU that keeps rescheduling with 0 period time if (interval < HRT_INTERVAL_MIN) { PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); + PX4_BACKTRACE(); } #endif diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c index 238b324194..b0cfb4c087 100644 --- a/src/platforms/posix/px4_layer/px4_log.c +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -1,5 +1,26 @@ +#include #include +#include __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; + +void px4_backtrace() +{ + void *buffer[10]; + char **callstack; + int bt_size; + int idx; + + bt_size = backtrace(buffer, 10); + callstack = backtrace_symbols(buffer, bt_size); + + PX4_INFO("Backtrace:", bt_size); + + for(idx=0; idx < bt_size; idx++) + PX4_INFO("%s", callstack[idx]); + + free(callstack); +} + diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index efe09ea860..46ca7538c1 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -165,6 +165,7 @@ static void hrt_work_process() if (!worker) { PX4_ERR("MESSED UP: worker = 0"); + PX4_BACKTRACE(); } else { worker(arg); diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 55bf3a55bc..3dcb29515b 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -64,6 +64,7 @@ static inline void do_nothing(int level, ...) #define PX4_WARN(...) ROS_WARN(__VA_ARGS__) #define PX4_INFO(...) ROS_WARN(__VA_ARGS__) #define PX4_DEBUG(...) +#define PX4_BACKTRACE() #elif defined(__PX4_QURT) #include "qurt_log.h" @@ -72,6 +73,7 @@ static inline void do_nothing(int level, ...) ****************************************************************************/ #define PX4_LOG(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) #define PX4_INFO(FMT, ...) qurt_log(_PX4_LOG_LEVEL_ALWAYS, __FILE__, __LINE__, FMT, ##__VA_ARGS__) +#define PX4_BACKTRACE() #if defined(TRACE_BUILD) /**************************************************************************** @@ -127,8 +129,11 @@ __EXPORT extern uint64_t hrt_absolute_time(void); __EXPORT extern const char *__px4_log_level_str[5]; __EXPORT extern int __px4_log_level_current; +__EXPORT extern void px4_backtrace(void); __END_DECLS +#define PX4_BACKTRACE() px4_backtrace() + // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME #define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_ERROR From eb6553d92362dbd1e96a721933b8835cd2a59a1e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 09:48:27 +0200 Subject: [PATCH 023/155] Fix POSIX backtrace compile --- src/platforms/posix/px4_layer/px4_log.c | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/px4_layer/px4_log.c b/src/platforms/posix/px4_layer/px4_log.c index b0cfb4c087..074ae8b466 100644 --- a/src/platforms/posix/px4_layer/px4_log.c +++ b/src/platforms/posix/px4_layer/px4_log.c @@ -1,13 +1,16 @@ #include #include +#ifdef __PX4_POSIX #include +#endif __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; -__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC+1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; +__EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "INFO", "DEBUG", "WARN", "ERROR", "PANIC" }; void px4_backtrace() { +#ifdef __PX4_POSIX void *buffer[10]; char **callstack; int bt_size; @@ -16,11 +19,12 @@ void px4_backtrace() bt_size = backtrace(buffer, 10); callstack = backtrace_symbols(buffer, bt_size); - PX4_INFO("Backtrace:", bt_size); + PX4_INFO("Backtrace: %d", bt_size); - for(idx=0; idx < bt_size; idx++) + for (idx = 0; idx < bt_size; idx++) { PX4_INFO("%s", callstack[idx]); + } free(callstack); +#endif } - From 9a33e6e2aa29815ef38bf4336d73d421f1084698 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 10:08:17 +0200 Subject: [PATCH 024/155] Fixed MPU9250 formatting --- src/drivers/mpu9250/mpu9250.cpp | 444 ++++++++++++++++++++------------ 1 file changed, 279 insertions(+), 165 deletions(-) diff --git a/src/drivers/mpu9250/mpu9250.cpp b/src/drivers/mpu9250/mpu9250.cpp index 248c4b038f..bff57865c8 100644 --- a/src/drivers/mpu9250/mpu9250.cpp +++ b/src/drivers/mpu9250/mpu9250.cpp @@ -337,7 +337,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed=MPU9250_LOW_BUS_SPEED); + uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -392,7 +392,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -406,7 +406,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set low pass filter frequency @@ -424,8 +424,8 @@ private: void check_registers(void); /* do not allow to copy this class due to pointer data members */ - MPU9250(const MPU9250&); - MPU9250 operator=(const MPU9250&); + MPU9250(const MPU9250 &); + MPU9250 operator=(const MPU9250 &); #pragma pack(push, 1) /** @@ -455,12 +455,13 @@ const uint8_t MPU9250::_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPU MPUREG_PWR_MGMT_2, MPUREG_USER_CTRL, MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_ACCEL_CONFIG2, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; + MPUREG_INT_PIN_CFG + }; @@ -490,8 +491,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - MPU9250_gyro(const MPU9250_gyro&); - MPU9250_gyro operator=(const MPU9250_gyro&); + MPU9250_gyro(const MPU9250_gyro &); + MPU9250_gyro operator=(const MPU9250_gyro &); }; /** driver 'main' command */ @@ -577,13 +578,17 @@ MPU9250::~MPU9250() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -612,15 +617,20 @@ MPU9250::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + + if (_accel_reports == nullptr) { goto out; + } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_gyro_reports == nullptr) - goto out; - if (reset() != OK) + if (_gyro_reports == nullptr) { goto out; + } + + if (reset() != OK) { + goto out; + } /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -640,6 +650,7 @@ MPU9250::init() /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -656,7 +667,7 @@ MPU9250::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -668,7 +679,7 @@ MPU9250::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); @@ -722,16 +733,20 @@ int MPU9250::reset() usleep(1000); uint8_t retries = 10; + while (retries--) { bool all_ok = true; - for (uint8_t i=0; i200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; } @@ -790,31 +808,40 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) if (frequency_hz == 0) { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; + } else if (frequency_hz <= 5) { _dlpf_freq = 5; filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { _dlpf_freq = 10; filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { _dlpf_freq = 20; filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 41) { _dlpf_freq = 41; filter = BITS_DLPF_CFG_41HZ; + } else if (frequency_hz <= 92) { _dlpf_freq = 92; filter = BITS_DLPF_CFG_92HZ; + } else if (frequency_hz <= 184) { _dlpf_freq = 184; filter = BITS_DLPF_CFG_184HZ; + } else if (frequency_hz <= 250) { _dlpf_freq = 250; filter = BITS_DLPF_CFG_250HZ; + } else { _dlpf_freq = 0; filter = BITS_DLPF_CFG_3600HZ; } + write_checked_reg(MPUREG_CONFIG, filter); } @@ -824,8 +851,9 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -834,17 +862,21 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -867,24 +899,34 @@ MPU9250::self_test() int MPU9250::accel_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -892,8 +934,9 @@ MPU9250::accel_self_test() int MPU9250::gyro_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -909,27 +952,35 @@ MPU9250::gyro_self_test() const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ - if (fabsf(_gyro_scale.x_offset) > max_offset) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* check if all scales are zero */ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && - (fabsf(_gyro_scale.y_offset) < 0.000001f) && - (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { /* if all are zero, this device is not calibrated */ return 1; } @@ -960,8 +1011,9 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -970,17 +1022,21 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -1000,27 +1056,27 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU9250_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -1029,12 +1085,13 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -1051,17 +1108,18 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - /* - set call interval faster then the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu9250 clock - */ - _call.period = _call_interval - MPU9250_TIMER_REDUCTION; + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu9250 clock + */ + _call.period = _call_interval - MPU9250_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1069,25 +1127,29 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -1109,14 +1171,15 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -1131,18 +1194,20 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/MPU9250_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / MPU9250_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); #ifdef ACCELIOCSHWLOWPASS + case ACCELIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef ACCELIOCGHWLOWPASS + case ACCELIOCGHWLOWPASS: return _dlpf_freq; #endif @@ -1159,26 +1224,29 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_gyro_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -1216,6 +1284,7 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -1223,12 +1292,14 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return gyro_self_test(); #ifdef GYROIOCSHWLOWPASS + case GYROIOCSHWLOWPASS: _set_dlpf_filter(arg); return OK; #endif #ifdef GYROIOCGHWLOWPASS + case GYROIOCGHWLOWPASS: return _dlpf_freq; #endif @@ -1244,8 +1315,8 @@ MPU9250::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - // general register transfer at low clock speed - set_frequency(speed); + // general register transfer at low clock speed + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1257,8 +1328,8 @@ MPU9250::read_reg16(unsigned reg) { uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -1273,8 +1344,8 @@ MPU9250::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed - set_frequency(MPU9250_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU9250_LOW_BUS_SPEED); transfer(cmd, nullptr, sizeof(cmd)); } @@ -1294,7 +1365,8 @@ void MPU9250::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1346,9 +1421,9 @@ MPU9250::start() /* start polling at the specified rate */ hrt_call_every(&_call, - 1000, - _call_interval-MPU9250_TIMER_REDUCTION, - (hrt_callout)&MPU9250::measure_trampoline, this); + 1000, + _call_interval - MPU9250_TIMER_REDUCTION, + (hrt_callout)&MPU9250::measure_trampoline, this); } void @@ -1379,7 +1454,8 @@ MPU9250::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != + + if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { _checked_bad[_checked_next] = v; @@ -1408,7 +1484,8 @@ MPU9250::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; + _checked_next = 0; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); // waiting 3ms between register writes seems @@ -1416,9 +1493,11 @@ MPU9250::check_registers(void) // recovering considerably _reset_wait = hrt_absolute_time() + 3000; } + _register_wait = 20; } - _checked_next = (_checked_next+1) % MPU9250_NUM_CHECKED_REGISTERS; + + _checked_next = (_checked_next + 1) % MPU9250_NUM_CHECKED_REGISTERS; } void @@ -1430,6 +1509,7 @@ MPU9250::measure() } struct MPUReport mpu_report; + struct Report { int16_t accel_x; int16_t accel_y; @@ -1448,13 +1528,14 @@ MPU9250::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - set_frequency(MPU9250_HIGH_BUS_SPEED); + // sensor transfer at high clock speed + set_frequency(MPU9250_HIGH_BUS_SPEED); - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; + } - check_registers(); + check_registers(); /* see if this is duplicate accelerometer data. Note that we @@ -1464,13 +1545,14 @@ MPU9250::measure() sampled at 8kHz, so we would incorrectly think we have new data when we are in fact getting duplicate accelerometer data. */ - if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { // it isn't new data - wait for next timer perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; return; } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); _got_duplicate = false; @@ -1498,10 +1580,10 @@ MPU9250::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } @@ -1548,7 +1630,7 @@ MPU9250::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1657,22 +1739,26 @@ MPU9250::print_info() perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1778,48 +1869,55 @@ void start(bool external_bus, enum Rotation rotation) { int fd; - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ + { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT *g_dev_ptr = new MPU9250(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif + } else { *g_dev_ptr = new MPU9250(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -1828,14 +1926,17 @@ fail: void stop(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ warnx("already stopped."); } + exit(0); } @@ -1847,8 +1948,8 @@ stop(bool external_bus) void test(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; @@ -1863,12 +1964,14 @@ test(bool external_bus) /* get the driver */ int fd_gyro = open(path_gyro, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", path_gyro); + } /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); @@ -1922,19 +2025,22 @@ test(bool external_bus) void reset(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; int fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } - close(fd); + close(fd); exit(0); } @@ -1945,9 +2051,11 @@ reset(bool external_bus) void info(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_info(); @@ -1961,9 +2069,11 @@ info(bool external_bus) void regdump(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_registers(); @@ -1977,9 +2087,11 @@ regdump(bool external_bus) void testerror(bool external_bus) { - MPU9250 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU9250 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } (*g_dev_ptr)->test_error(); @@ -2010,9 +2122,11 @@ mpu9250_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + default: mpu9250::usage(); exit(0); From c326189ce89c121f2ceb846f2ef8b002e60afe7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:05:27 +0200 Subject: [PATCH 025/155] NuttX build flags: Remove trivial warnings --- nuttx-configs/px4fmu-v1/nsh/Make.defs | 3 --- nuttx-configs/px4fmu-v2/nsh/Make.defs | 3 --- nuttx-configs/px4io-v1/nsh/Make.defs | 3 --- nuttx-configs/px4io-v2/nsh/Make.defs | 3 --- 4 files changed, 12 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs index 4e08d28a29..a1439476aa 100644 --- a/nuttx-configs/px4fmu-v1/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs @@ -124,11 +124,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -140,7 +138,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) \ -Wno-psabi diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs index 5a1d5af2c4..a09e6794e3 100644 --- a/nuttx-configs/px4fmu-v2/nsh/Make.defs +++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs @@ -124,11 +124,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -140,7 +138,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) \ -Wno-psabi diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs index 74f38c0cb4..df1110fc61 100644 --- a/nuttx-configs/px4io-v1/nsh/Make.defs +++ b/nuttx-configs/px4io-v1/nsh/Make.defs @@ -117,11 +117,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -133,7 +131,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) ARCHDEFINES = diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs index 287466db60..23ebca902a 100644 --- a/nuttx-configs/px4io-v2/nsh/Make.defs +++ b/nuttx-configs/px4io-v2/nsh/Make.defs @@ -117,11 +117,9 @@ ARCHWARNINGS = -Wall \ -Wextra \ -Wdouble-promotion \ -Wshadow \ - -Wfloat-equal \ -Wframe-larger-than=1024 \ -Wpointer-arith \ -Wlogical-op \ - -Wmissing-declarations \ -Wpacked \ -Wno-unused-parameter # -Wcast-qual - generates spurious noreturn attribute warnings, try again later @@ -133,7 +131,6 @@ ARCHCWARNINGS = $(ARCHWARNINGS) \ -Wstrict-prototypes \ -Wold-style-declaration \ -Wmissing-parameter-type \ - -Wmissing-prototypes \ -Wnested-externs ARCHWARNINGSXX = $(ARCHWARNINGS) ARCHDEFINES = From 236a423edd11a0148d906830e0d4527afbc93e42 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:12:32 +0200 Subject: [PATCH 026/155] Fixed MPU6K code style --- src/drivers/mpu6000/mpu6000.cpp | 514 ++++++++++++++++++++------------ 1 file changed, 317 insertions(+), 197 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 4af1bec586..ab2eede349 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -348,7 +348,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED); + uint8_t read_reg(unsigned reg, uint32_t speed = MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -403,7 +403,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -417,7 +417,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set low pass filter frequency @@ -435,8 +435,8 @@ private: void check_registers(void); /* do not allow to copy this class due to pointer data members */ - MPU6000(const MPU6000&); - MPU6000 operator=(const MPU6000&); + MPU6000(const MPU6000 &); + MPU6000 operator=(const MPU6000 &); #pragma pack(push, 1) /** @@ -465,11 +465,12 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_PWR_MGMT_1, MPUREG_USER_CTRL, MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG }; + MPUREG_INT_PIN_CFG + }; @@ -499,8 +500,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - MPU6000_gyro(const MPU6000_gyro&); - MPU6000_gyro operator=(const MPU6000_gyro&); + MPU6000_gyro(const MPU6000_gyro &); + MPU6000_gyro operator=(const MPU6000_gyro &); }; /** driver 'main' command */ @@ -588,13 +589,17 @@ MPU6000::~MPU6000() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -623,15 +628,20 @@ MPU6000::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + + if (_accel_reports == nullptr) { goto out; + } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_gyro_reports == nullptr) - goto out; - if (reset() != OK) + if (_gyro_reports == nullptr) { goto out; + } + + if (reset() != OK) { + goto out; + } /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -651,6 +661,7 @@ MPU6000::init() /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -667,7 +678,7 @@ MPU6000::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic == nullptr) { warnx("ADVERT FAIL"); @@ -679,7 +690,7 @@ MPU6000::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { warnx("ADVERT FAIL"); @@ -696,6 +707,7 @@ int MPU6000::reset() // frequenctly comes up in a bad state where all transfers // come as zero uint8_t tries = 5; + while (--tries != 0) { irqstate_t state; state = irqsave(); @@ -716,9 +728,11 @@ int MPU6000::reset() if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { break; } + perf_count(_reset_retries); usleep(2000); } + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { return -EIO; } @@ -768,6 +782,7 @@ MPU6000::probe() { uint8_t whoami; whoami = read_reg(MPUREG_WHOAMI); + if (whoami != MPU_WHOAMI_6000) { DEVICE_DEBUG("unexpected WHOAMI 0x%02x", whoami); return -EIO; @@ -807,15 +822,18 @@ void MPU6000::_set_sample_rate(unsigned desired_sample_rate_hz) { if (desired_sample_rate_hz == 0 || - desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = MPU6000_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; - write_checked_reg(MPUREG_SMPLRT_DIV, div-1); + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } + + write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; } @@ -830,25 +848,34 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) /* choose next highest filter frequency available */ - if (frequency_hz == 0) { + if (frequency_hz == 0) { filter = BITS_DLPF_CFG_2100HZ_NOLPF; - } else if (frequency_hz <= 5) { + + } else if (frequency_hz <= 5) { filter = BITS_DLPF_CFG_5HZ; + } else if (frequency_hz <= 10) { filter = BITS_DLPF_CFG_10HZ; + } else if (frequency_hz <= 20) { filter = BITS_DLPF_CFG_20HZ; + } else if (frequency_hz <= 42) { filter = BITS_DLPF_CFG_42HZ; + } else if (frequency_hz <= 98) { filter = BITS_DLPF_CFG_98HZ; + } else if (frequency_hz <= 188) { filter = BITS_DLPF_CFG_188HZ; + } else if (frequency_hz <= 256) { filter = BITS_DLPF_CFG_256HZ_NOLPF2; + } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } + write_checked_reg(MPUREG_CONFIG, filter); } @@ -858,8 +885,9 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -868,17 +896,21 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -901,24 +933,34 @@ MPU6000::self_test() int MPU6000::accel_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -926,8 +968,9 @@ MPU6000::accel_self_test() int MPU6000::gyro_self_test() { - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -947,27 +990,35 @@ MPU6000::gyro_self_test() const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ - if (fabsf(_gyro_scale.x_offset) > max_offset) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* check if all scales are zero */ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && - (fabsf(_gyro_scale.y_offset) < 0.000001f) && - (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { /* if all are zero, this device is not calibrated */ return 1; } @@ -1003,14 +1054,14 @@ MPU6000::factory_self_test() float gyro_ftrim[3]; // get baseline values without self-test enabled - set_frequency(MPU6000_HIGH_BUS_SPEED); + set_frequency(MPU6000_HIGH_BUS_SPEED); memset(accel_baseline, 0, sizeof(accel_baseline)); memset(gyro_baseline, 0, sizeof(gyro_baseline)); memset(accel, 0, sizeof(accel)); memset(gyro, 0, sizeof(gyro)); - for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); - atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); - atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + atrim[0] = ((trims[0] >> 3) & 0x1C) | ((trims[3] >> 4) & 0x03); + atrim[1] = ((trims[1] >> 3) & 0x1C) | ((trims[3] >> 2) & 0x03); + atrim[2] = ((trims[2] >> 3) & 0x1C) | ((trims[3] >> 0) & 0x03); gtrim[0] = trims[0] & 0x1F; gtrim[1] = trims[1] & 0x1F; gtrim[2] = trims[2] & 0x1F; // convert factory trims to right units - for (uint8_t i=0; i<3; i++) { - accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); - gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + for (uint8_t i = 0; i < 3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f / 0.34f, (atrim[i] - 1) / 30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i] - 1); } + // Y gyro trim is negative gyro_ftrim[1] *= -1; - for (uint8_t i=0; i<3; i++) { - float diff = accel[i]-accel_baseline[i]; - float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + for (uint8_t i = 0; i < 3; i++) { + float diff = accel[i] - accel_baseline[i]; + float err = 100 * (diff - accel_ftrim[i]) / accel_ftrim[i]; ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", (unsigned)i, - (int)(1000*accel_baseline[i]), - (int)(1000*accel[i]), - (int)(1000*diff), - (int)(1000*accel_ftrim[i]), + (int)(1000 * accel_baseline[i]), + (int)(1000 * accel[i]), + (int)(1000 * diff), + (int)(1000 * accel_ftrim[i]), (int)err); + if (fabsf(err) > 14) { ::printf("FAIL\n"); ret = -EIO; } } - for (uint8_t i=0; i<3; i++) { - float diff = gyro[i]-gyro_baseline[i]; - float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + + for (uint8_t i = 0; i < 3; i++) { + float diff = gyro[i] - gyro_baseline[i]; + float err = 100 * (diff - gyro_ftrim[i]) / gyro_ftrim[i]; ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", (unsigned)i, - (int)(1000*gyro_baseline[i]), - (int)(1000*gyro[i]), - (int)(1000*(gyro[i]-gyro_baseline[i])), - (int)(1000*gyro_ftrim[i]), + (int)(1000 * gyro_baseline[i]), + (int)(1000 * gyro[i]), + (int)(1000 * (gyro[i] - gyro_baseline[i])), + (int)(1000 * gyro_ftrim[i]), (int)err); + if (fabsf(err) > 14) { ::printf("FAIL\n"); ret = -EIO; @@ -1118,6 +1173,7 @@ MPU6000::factory_self_test() write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); _in_factory_test = false; + if (ret == OK) { ::printf("PASSED\n"); } @@ -1149,8 +1205,9 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -1159,17 +1216,21 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -1189,27 +1250,27 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -1218,12 +1279,13 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); @@ -1240,17 +1302,18 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - /* - set call interval faster then the sample time. We - then detect when we have duplicate samples and reject - them. This prevents aliasing due to a beat between the - stm32 clock and the mpu6000 clock - */ - _call.period = _call_interval - MPU6000_TIMER_REDUCTION; + /* + set call interval faster then the sample time. We + then detect when we have duplicate samples and reject + them. This prevents aliasing due to a beat between the + stm32 clock and the mpu6000 clock + */ + _call.period = _call_interval - MPU6000_TIMER_REDUCTION; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1258,25 +1321,29 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -1300,14 +1367,15 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -1322,7 +1390,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / MPU6000_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -1338,26 +1406,29 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_gyro_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_gyro_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -1371,6 +1442,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); + case GYROIOCSLOWPASS: // set hardware filtering _set_dlpf_filter(arg); @@ -1395,6 +1467,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -1412,8 +1485,8 @@ MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - // general register transfer at low clock speed - set_frequency(speed); + // general register transfer at low clock speed + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1425,8 +1498,8 @@ MPU6000::read_reg16(unsigned reg) { uint8_t cmd[3] = { (uint8_t)(reg | DIR_READ), 0, 0 }; - // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, cmd, sizeof(cmd)); @@ -1441,8 +1514,8 @@ MPU6000::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + // general register transfer at low clock speed + set_frequency(MPU6000_LOW_BUS_SPEED); transfer(cmd, nullptr, sizeof(cmd)); } @@ -1462,7 +1535,8 @@ void MPU6000::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1525,9 +1602,9 @@ MPU6000::start() /* start polling at the specified rate */ hrt_call_every(&_call, - 1000, - _call_interval-MPU6000_TIMER_REDUCTION, - (hrt_callout)&MPU6000::measure_trampoline, this); + 1000, + _call_interval - MPU6000_TIMER_REDUCTION, + (hrt_callout)&MPU6000::measure_trampoline, this); } void @@ -1565,7 +1642,8 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + + if ((v = read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* if we get the wrong value then we know the SPI bus @@ -1591,7 +1669,8 @@ MPU6000::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; + _checked_next = 0; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); // waiting 3ms between register writes seems @@ -1599,9 +1678,11 @@ MPU6000::check_registers(void) // recovering considerably _reset_wait = hrt_absolute_time() + 3000; } + _register_wait = 20; } - _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; + + _checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS; } void @@ -1618,6 +1699,7 @@ MPU6000::measure() } struct MPUReport mpu_report; + struct Report { int16_t accel_x; int16_t accel_y; @@ -1636,13 +1718,14 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - set_frequency(MPU6000_HIGH_BUS_SPEED); + // sensor transfer at high clock speed + set_frequency(MPU6000_HIGH_BUS_SPEED); - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) + if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; + } - check_registers(); + check_registers(); /* see if this is duplicate accelerometer data. Note that we @@ -1652,13 +1735,14 @@ MPU6000::measure() sampled at 8kHz, so we would incorrectly think we have new data when we are in fact getting duplicate accelerometer data. */ - if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { + if (!_got_duplicate && memcmp(&mpu_report.accel_x[0], &_last_accel[0], 6) == 0) { // it isn't new data - wait for next timer perf_end(_sample_perf); perf_count(_duplicates); _got_duplicate = true; return; } + memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6); _got_duplicate = false; @@ -1686,10 +1770,10 @@ MPU6000::measure() // all zero data - probably a SPI bus error perf_count(_bad_transfers); perf_end(_sample_perf); - // note that we don't call reset() here as a reset() - // costs 20ms with interrupts disabled. That means if - // the mpu6k does go bad it would cause a FMU failure, - // regardless of whether another sensor is available, + // note that we don't call reset() here as a reset() + // costs 20ms with interrupts disabled. That means if + // the mpu6k does go bad it would cause a FMU failure, + // regardless of whether another sensor is available, return; } @@ -1736,7 +1820,7 @@ MPU6000::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1866,16 +1950,19 @@ MPU6000::print_info() perf_print_counter(_duplicates); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1982,51 +2074,59 @@ void start(bool external_bus, enum Rotation rotation, int range) { int fd; - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; if (*g_dev_ptr != nullptr) /* if already started, the still command succeeded */ + { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT *g_dev_ptr = new MPU6000(PX4_SPI_BUS_EXT, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_EXT_MPU, rotation); #else errx(0, "External SPI not available"); #endif + } else { *g_dev_ptr = new MPU6000(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, (spi_dev_e)PX4_SPIDEV_MPU, rotation); } - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - if (ioctl(fd, ACCELIOCSRANGE, range) < 0) + if (ioctl(fd, ACCELIOCSRANGE, range) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } errx(1, "driver start failed"); @@ -2035,14 +2135,17 @@ fail: void stop(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ warnx("already stopped."); } + exit(0); } @@ -2054,8 +2157,8 @@ stop(bool external_bus) void test(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = external_bus?MPU_DEVICE_PATH_GYRO_EXT:MPU_DEVICE_PATH_GYRO; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; + const char *path_gyro = external_bus ? MPU_DEVICE_PATH_GYRO_EXT : MPU_DEVICE_PATH_GYRO; accel_report a_report; gyro_report g_report; ssize_t sz; @@ -2070,12 +2173,14 @@ test(bool external_bus) /* get the driver */ int fd_gyro = open(path_gyro, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", path_gyro); + } /* reset to manual polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd, &a_report, sizeof(a_report)); @@ -2117,8 +2222,9 @@ test(bool external_bus) warnx("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); /* reset to default polling */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); + } close(fd); close(fd_gyro); @@ -2135,19 +2241,22 @@ test(bool external_bus) void reset(bool external_bus) { - const char *path_accel = external_bus?MPU_DEVICE_PATH_ACCEL_EXT:MPU_DEVICE_PATH_ACCEL; + const char *path_accel = external_bus ? MPU_DEVICE_PATH_ACCEL_EXT : MPU_DEVICE_PATH_ACCEL; int fd = open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } - close(fd); + close(fd); exit(0); } @@ -2158,9 +2267,11 @@ reset(bool external_bus) void info(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_info(); @@ -2174,9 +2285,11 @@ info(bool external_bus) void regdump(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", *g_dev_ptr); (*g_dev_ptr)->print_registers(); @@ -2190,9 +2303,11 @@ regdump(bool external_bus) void testerror(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } (*g_dev_ptr)->test_error(); @@ -2205,9 +2320,11 @@ testerror(bool external_bus) void factorytest(bool external_bus) { - MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; - if (*g_dev_ptr == nullptr) + MPU6000 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int; + + if (*g_dev_ptr == nullptr) { errx(1, "driver not running"); + } (*g_dev_ptr)->factory_self_test(); @@ -2240,12 +2357,15 @@ mpu6000_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': accel_range = atoi(optarg); break; + default: mpu6000::usage(); exit(0); From ca4982a6e8557fe0af0e72ecbd9c3872f9902ec2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:13:43 +0200 Subject: [PATCH 027/155] HoTT: Fixed code style --- src/drivers/hott/comms.cpp | 7 ++- src/drivers/hott/messages.cpp | 36 ++++++------ src/drivers/hott/messages.h | 104 +++++++++++++++++----------------- 3 files changed, 76 insertions(+), 71 deletions(-) diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index 60a49b559c..378b5b75d6 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -57,10 +57,11 @@ open_uart(const char *device) if (uart < 0) { err(1, "ERR: opening %s", device); } - - /* Back up the original uart configuration to restore it after exit */ + + /* Back up the original uart configuration to restore it after exit */ int termios_state; struct termios uart_config_original; + if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); err(1, "ERR: %s: %d", device, termios_state); @@ -77,7 +78,7 @@ open_uart(const char *device) if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { close(uart); err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)", - device, termios_state); + device, termios_state); } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 61daa302fb..db225a2b98 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -69,7 +69,7 @@ static bool _home_position_set = false; static double _home_lat = 0.0d; static double _home_lon = 0.0d; -void +void init_sub_messages(void) { _battery_sub = orb_subscribe(ORB_ID(battery_status)); @@ -80,7 +80,7 @@ init_sub_messages(void) _esc_sub = orb_subscribe(ORB_ID(esc_status)); } -void +void init_pub_messages(void) { } @@ -122,12 +122,13 @@ publish_gam_message(const uint8_t *buffer) /* announce the esc if needed, just publish else */ if (_esc_pub != nullptr) { orb_publish(ORB_ID(esc_status), _esc_pub, &esc); + } else { _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); } } -void +void build_eam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ @@ -147,7 +148,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius[0] + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; @@ -170,7 +171,7 @@ build_eam_response(uint8_t *buffer, size_t *size) memcpy(buffer, &msg, *size); } -void +void build_gam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the ESC Status values */ @@ -185,7 +186,7 @@ build_gam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.gam_sensor_id = GAM_SENSOR_ID; msg.sensor_text_id = GAM_SENSOR_TEXT_ID; - + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20.0F); msg.temperature2 = 20; // 0 deg. C. @@ -205,7 +206,7 @@ build_gam_response(uint8_t *buffer, size_t *size) memcpy(buffer, &msg, *size); } -void +void build_gps_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ @@ -213,7 +214,7 @@ build_gps_response(uint8_t *buffer, size_t *size) memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); - /* get a local copy of the battery data */ + /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); @@ -241,12 +242,13 @@ build_gps_response(uint8_t *buffer, size_t *size) uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f); msg.gps_speed_L = (uint8_t)speed & 0xff; msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; - - /* Get latitude in degrees, minutes and seconds */ - double lat = ((double)(gps.lat))*1e-7d; - /* Set the N or S specifier */ + /* Get latitude in degrees, minutes and seconds */ + double lat = ((double)(gps.lat)) * 1e-7d; + + /* Set the N or S specifier */ msg.latitude_ns = 0; + if (lat < 0) { msg.latitude_ns = 1; lat = abs(lat); @@ -265,32 +267,34 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; /* Get longitude in degrees, minutes and seconds */ - double lon = ((double)(gps.lon))*1e-7d; + double lon = ((double)(gps.lon)) * 1e-7d; /* Set the E or W specifier */ msg.longitude_ew = 0; + if (lon < 0) { msg.longitude_ew = 1; lon = abs(lon); } convert_to_degrees_minutes_seconds(lon, °, &min, &sec); - + uint16_t lon_min = (uint16_t)(deg * 100 + min); msg.longitude_min_L = (uint8_t)lon_min & 0xff; msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; uint16_t lon_sec = (uint16_t)(sec); msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; - + /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f); + uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; /* Get any (and probably only ever one) _home_sub postion report */ bool updated; orb_check(_home_sub, &updated); + if (updated) { /* get a local copy of the home position data */ struct home_position_s home; diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index a116a50dd6..504c9f19ec 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -125,63 +125,63 @@ struct eam_module_msg { #define GAM_SENSOR_TEXT_ID 0xd0 struct gam_module_msg { - uint8_t start; /**< Start byte */ - uint8_t gam_sensor_id; /**< GAM sensor id */ - uint8_t warning_beeps; - uint8_t sensor_text_id; - uint8_t alarm_invers1; - uint8_t alarm_invers2; - uint8_t cell1; /**< Lipo cell voltages. Not supported. */ - uint8_t cell2; - uint8_t cell3; - uint8_t cell4; - uint8_t cell5; - uint8_t cell6; - uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */ - uint8_t batt1_H; - uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */ - uint8_t batt2_H; - uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */ - uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */ - uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */ - /**< Graphical display ranges: 0 25% 50% 75% 100% */ - uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */ - uint8_t fuel_ml_H; - uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ - uint8_t rpm_H; - uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */ - uint8_t altitude_H; - uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */ - uint8_t climbrate_H; - uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */ - uint8_t current_L; /**< Current in 0.1A steps */ - uint8_t current_H; - uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */ - uint8_t main_voltage_H; - uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */ - uint8_t batt_cap_H; - uint8_t speed_L; /**< Speed in km/h */ - uint8_t speed_H; - uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */ - uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */ - uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ - uint8_t rpm2_H; - uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */ - uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */ - uint8_t version; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed */ + uint8_t start; /**< Start byte */ + uint8_t gam_sensor_id; /**< GAM sensor id */ + uint8_t warning_beeps; + uint8_t sensor_text_id; + uint8_t alarm_invers1; + uint8_t alarm_invers2; + uint8_t cell1; /**< Lipo cell voltages. Not supported. */ + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uint8_t batt1_L; /**< Battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt1_H; + uint8_t batt2_L; /**< Battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V */ + uint8_t batt2_H; + uint8_t temperature1; /**< Temperature 1. offset of 20. a value of 20 = 0°C */ + uint8_t temperature2; /**< Temperature 2. offset of 20. a value of 20 = 0°C */ + uint8_t fuel_procent; /**< Fuel capacity in %. Values 0 - 100 */ + /**< Graphical display ranges: 0 25% 50% 75% 100% */ + uint8_t fuel_ml_L; /**< Fuel in ml scale. Full = 65535 */ + uint8_t fuel_ml_H; + uint8_t rpm_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm_H; + uint8_t altitude_L; /**< Altitude in meters. offset of 500, 500 = 0m */ + uint8_t altitude_H; + uint8_t climbrate_L; /**< Climb rate in 0.01m/s. Value of 30000 = 0.00 m/s */ + uint8_t climbrate_H; + uint8_t climbrate3s; /**< Climb rate in m/3sec. Value of 120 = 0m/3sec */ + uint8_t current_L; /**< Current in 0.1A steps */ + uint8_t current_H; + uint8_t main_voltage_L; /**< Main power voltage using 0.1V steps */ + uint8_t main_voltage_H; + uint8_t batt_cap_L; /**< Used battery capacity in 10mAh steps */ + uint8_t batt_cap_H; + uint8_t speed_L; /**< Speed in km/h */ + uint8_t speed_H; + uint8_t min_cell_volt; /**< Minimum cell voltage in 2mV steps. 124 = 2,48V */ + uint8_t min_cell_volt_num; /**< Number of the cell with the lowest voltage */ + uint8_t rpm2_L; /**< RPM in 10 RPM steps. 300 = 3000rpm */ + uint8_t rpm2_H; + uint8_t general_error_number; /**< Voice error == 12. TODO: more docu */ + uint8_t pressure; /**< Pressure up to 16bar. 0,1bar scale. 20 = 2bar */ + uint8_t version; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed */ }; /* GPS sensor constants. */ #define GPS_SENSOR_ID 0x8a #define GPS_SENSOR_TEXT_ID 0xa0 -/** +/** * The GPS sensor message - * Struct based on: https://code.google.com/p/diy-hott-gps/downloads + * Struct based on: https://code.google.com/p/diy-hott-gps/downloads */ -struct gps_module_msg { +struct gps_module_msg { uint8_t start; /**< Start byte */ uint8_t sensor_id; /**< GPS sensor ID*/ uint8_t warning; /**< 0…= warning beeps */ @@ -191,19 +191,19 @@ struct gps_module_msg { uint8_t flight_direction; /**< 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ uint8_t gps_speed_L; /**< 8 = /GPS speed low byte 8km/h */ uint8_t gps_speed_H; /**< 0 = /GPS speed high byte */ - + uint8_t latitude_ns; /**< 000 = N = 48°39’988 */ uint8_t latitude_min_L; /**< 231 0xE7 = 0x12E7 = 4839 */ uint8_t latitude_min_H; /**< 018 18 = 0x12 */ uint8_t latitude_sec_L; /**< 171 220 = 0xDC = 0x03DC =0988 */ uint8_t latitude_sec_H; /**< 016 3 = 0x03 */ - + uint8_t longitude_ew; /**< 000 = E= 9° 25’9360 */ uint8_t longitude_min_L; /**< 150 157 = 0x9D = 0x039D = 0925 */ uint8_t longitude_min_H; /**< 003 3 = 0x03 */ uint8_t longitude_sec_L; /**< 056 144 = 0x90 0x2490 = 9360 */ uint8_t longitude_sec_H; /**< 004 36 = 0x24 */ - + uint8_t distance_L; /**< 027 123 = /distance low byte 6 = 6 m */ uint8_t distance_H; /**< 036 35 = /distance high byte */ uint8_t altitude_L; /**< 243 244 = /Altitude low byte 500 = 0m */ From 9d670dd877176eaf1fd86d95a2cf54ad5d8c92b5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:14:00 +0200 Subject: [PATCH 028/155] LL40LS: Fixed code style --- src/drivers/ll40ls/LidarLiteI2C.cpp | 14 ++++++++++---- src/drivers/ll40ls/LidarLiteI2C.h | 6 +++--- src/drivers/ll40ls/LidarLitePWM.cpp | 2 +- src/drivers/ll40ls/ll40ls.cpp | 27 ++++++++++++++++++--------- 4 files changed, 32 insertions(+), 17 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 9a05890cd0..150465ec5b 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -132,7 +132,7 @@ int LidarLiteI2C::init() measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); @@ -165,12 +165,16 @@ int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t { if (send != NULL && send_len > 0) { int ret = transfer(send, send_len, NULL, 0); - if (ret != OK) + + if (ret != OK) { return ret; + } } + if (recv != NULL && recv_len > 0) { return transfer(NULL, 0, recv, recv_len); } + return OK; } @@ -345,8 +349,10 @@ int LidarLiteI2C::measure() int LidarLiteI2C::reset_sensor() { int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); - if (ret != OK) + + if (ret != OK) { return ret; + } // wait for sensor reset to complete usleep(1000); @@ -447,7 +453,7 @@ int LidarLiteI2C::collect() } _last_distance = distance_cm; - + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 51cca0862f..1c6de3c9b1 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -51,7 +51,7 @@ #include #include - + /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION @@ -117,8 +117,8 @@ private: uint16_t _zero_counter; uint64_t _acquire_time_usec; volatile bool _pause_measurements; - uint8_t _hw_version; - uint8_t _sw_version; + uint8_t _hw_version; + uint8_t _sw_version; /**< the bus the device is connected to */ int _bus; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 3d17d5b36b..e2a993ad14 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -114,7 +114,7 @@ int LidarLitePWM::init() measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a4c8720dbc..8591c4dee3 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -80,7 +80,7 @@ LidarLiteI2C *g_dev_int; LidarLiteI2C *g_dev_ext; LidarLitePWM *g_dev_pwm; -LidarLite * get_dev(const bool use_i2c, const int bus); +LidarLite *get_dev(const bool use_i2c, const int bus); void start(const bool use_i2c, const int bus); void stop(const bool use_i2c, const int bus); void test(const bool use_i2c, const int bus); @@ -92,19 +92,25 @@ void usage(); /** * Get the correct device pointer */ -LidarLite * get_dev(const bool use_i2c, const int bus) { - LidarLite * g_dev = nullptr; +LidarLite *get_dev(const bool use_i2c, const int bus) +{ + LidarLite *g_dev = nullptr; + if (use_i2c) { - g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "i2c driver not running"); } + } else { - g_dev = static_cast(g_dev_pwm); + g_dev = static_cast(g_dev_pwm); + if (g_dev == nullptr) { errx(1, "pwm driver not running"); } } + return g_dev; }; @@ -114,7 +120,7 @@ LidarLite * get_dev(const bool use_i2c, const int bus) { void start(const bool use_i2c, const int bus) { if (g_dev_int != nullptr || g_dev_ext != nullptr || g_dev_pwm != nullptr) { - errx(1,"driver already started"); + errx(1, "driver already started"); } if (use_i2c) { @@ -254,18 +260,21 @@ void stop(const bool use_i2c, const int bus) delete g_dev_int; g_dev_int = nullptr; } + } else { if (g_dev_ext != nullptr) { delete g_dev_ext; g_dev_ext = nullptr; } } + } else { if (g_dev_pwm != nullptr) { delete g_dev_pwm; g_dev_pwm = nullptr; } } + exit(0); } @@ -334,7 +343,7 @@ test(const bool use_i2c, const int bus) warnx("periodic read %u", i); warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); } @@ -386,7 +395,7 @@ reset(const bool use_i2c, const int bus) void info(const bool use_i2c, const int bus) { - LidarLite * g_dev = get_dev(use_i2c, bus); + LidarLite *g_dev = get_dev(use_i2c, bus); printf("state @ %p\n", g_dev); g_dev->print_info(); exit(0); @@ -398,7 +407,7 @@ info(const bool use_i2c, const int bus) void regdump(const bool use_i2c, const int bus) { - LidarLite * g_dev = get_dev(use_i2c, bus); + LidarLite *g_dev = get_dev(use_i2c, bus); printf("regdump @ %p\n", g_dev); g_dev->print_registers(); exit(0); From 3c2252fa0ddd79805ca660f4d50ff40fa8f80e84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:14:12 +0200 Subject: [PATCH 029/155] mb12xx: Fixed code style --- src/drivers/mb12xx/mb12xx.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index ecd2ea04a6..f3cfa0e267 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -143,7 +143,8 @@ private: int _cycling_rate; /* */ uint8_t _index_counter; /* temporary sonar i2c address */ std::vector addr_ind; /* temp sonar i2c address vector */ - std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ /** @@ -274,7 +275,7 @@ MB12XX::init() struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); @@ -862,7 +863,7 @@ test() warnx("periodic read %u", i); warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f", (double)report.current_distance); warnx("time: %llu", report.timestamp); } From c88c2c7276082acd8ef4da230aad9e34d68bf4aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:14:25 +0200 Subject: [PATCH 030/155] RGB led: Fixed code style --- src/drivers/rgbled/rgbled.cpp | 36 ++++++++++++++++++++++------------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index d379b20b60..3cf33e8eca 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -135,9 +135,9 @@ extern "C" __EXPORT int rgbled_main(int argc, char *argv[]); RGBLED::RGBLED(int bus, int rgbled) : I2C("rgbled", RGBLED0_DEVICE_PATH, bus, rgbled #ifdef __PX4_NUTTX - ,100000 /* maximum speed supported */ + , 100000 /* maximum speed supported */ #endif - ), + ), _mode(RGBLED_MODE_OFF), _r(0), _g(0), @@ -195,9 +195,9 @@ RGBLED::probe() unsigned prevretries = _retries; _retries = 4; - if ((ret=get(on, powersave, r, g, b)) != OK || - (ret=send_led_enable(false) != OK) || - (ret=send_led_enable(false) != OK)) { + if ((ret = get(on, powersave, r, g, b)) != OK || + (ret = send_led_enable(false) != OK) || + (ret = send_led_enable(false) != OK)) { return ret; } @@ -297,6 +297,7 @@ RGBLED::led() if (_param_sub >= 0) { bool updated = false; orb_check(_param_sub, &updated); + if (updated) { parameter_update_s pupdate; orb_copy(ORB_ID(parameter_update), _param_sub, &pupdate); @@ -310,8 +311,9 @@ RGBLED::led() case RGBLED_MODE_BLINK_SLOW: case RGBLED_MODE_BLINK_NORMAL: case RGBLED_MODE_BLINK_FAST: - if (_counter >= 2) + if (_counter >= 2) { _counter = 0; + } send_led_enable(_counter == 0); @@ -319,8 +321,9 @@ RGBLED::led() case RGBLED_MODE_BREATHE: - if (_counter >= 62) + if (_counter >= 62) { _counter = 0; + } int n; @@ -338,8 +341,9 @@ RGBLED::led() case RGBLED_MODE_PATTERN: /* don't run out of the pattern array and stop if the next frame is 0 */ - if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) + if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) { _counter = 0; + } set_color(_pattern.color[_counter]); send_led_rgb(); @@ -549,8 +553,9 @@ RGBLED::send_led_enable(bool enable) { uint8_t settings_byte = 0; - if (enable) + if (enable) { settings_byte |= SETTING_ENABLE; + } settings_byte |= SETTING_NOT_POWERSAVE; @@ -606,6 +611,7 @@ RGBLED::update_params() if (maxbrt == 1) { maxbrt = 2; } + _max_brightness = maxbrt / 15.0f; } @@ -629,6 +635,7 @@ rgbled_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "a:b:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'a': @@ -645,10 +652,10 @@ rgbled_main(int argc, char *argv[]) } } - if (myoptind >= argc) { - rgbled_usage(); - return 1; - } + if (myoptind >= argc) { + rgbled_usage(); + return 1; + } const char *verb = argv[myoptind]; @@ -677,6 +684,7 @@ rgbled_main(int argc, char *argv[]) warnx("no RGB led on bus #%d", i2cdevice); return 1; } + i2cdevice = PX4_I2C_BUS_LED; } } @@ -741,12 +749,14 @@ rgbled_main(int argc, char *argv[]) ret = px4_ioctl(fd, RGBLED_SET_MODE, (unsigned long)RGBLED_MODE_OFF); px4_close(fd); + /* delete the rgbled object if stop was requested, in addition to turning off the LED. */ if (!strcmp(verb, "stop")) { delete g_rgbled; g_rgbled = nullptr; return 0; } + return ret; } From 01ee8c9b58ad2b691ca51c20cd9ca1c36564e5d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:14:40 +0200 Subject: [PATCH 031/155] roboclaw: Fixed code style --- src/drivers/roboclaw/RoboClaw.cpp | 121 +++++++++++++++++-------- src/drivers/roboclaw/roboclaw_main.cpp | 25 ++--- 2 files changed, 97 insertions(+), 49 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 5b945e452d..a3d2f96a20 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -61,7 +61,7 @@ uint8_t RoboClaw::checksum_mask = 0x7f; RoboClaw::RoboClaw(const char *deviceName, uint16_t address, - uint16_t pulsesPerRev): + uint16_t pulsesPerRev): _address(address), _pulsesPerRev(pulsesPerRev), _uart(0), @@ -80,18 +80,27 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); - if (_uart < 0) err(1, "could not open %s", deviceName); + + if (_uart < 0) { err(1, "could not open %s", deviceName); } + int ret = 0; struct termios uart_config; ret = tcgetattr(_uart, &uart_config); - if (ret < 0) err (1, "failed to get attr"); + + if (ret < 0) { err(1, "failed to get attr"); } + uart_config.c_oflag &= ~ONLCR; // no CR for every LF ret = cfsetispeed(&uart_config, B38400); - if (ret < 0) err (1, "failed to set input speed"); + + if (ret < 0) { err(1, "failed to set input speed"); } + ret = cfsetospeed(&uart_config, B38400); - if (ret < 0) err (1, "failed to set output speed"); + + if (ret < 0) { err(1, "failed to set output speed"); } + ret = tcsetattr(_uart, TCSANOW, &uart_config); - if (ret < 0) err (1, "failed to set attr"); + + if (ret < 0) { err(1, "failed to set attr"); } // setup default settings, reset encoders resetEncoders(); @@ -107,25 +116,30 @@ RoboClaw::~RoboClaw() int RoboClaw::readEncoder(e_motor motor) { uint16_t sum = 0; + if (motor == MOTOR_1) { _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); } + uint8_t rbuf[50]; usleep(5000); int nread = read(_uart, rbuf, 50); + if (nread < 6) { printf("failed to read\n"); return -1; } + //printf("received: \n"); //for (int i=0;i 1) value = 1; - if (value < -1) value = -1; - uint8_t speed = fabs(value)*127; + if (value > 1) { value = 1; } + + if (value < -1) { value = -1; } + + uint8_t speed = fabs(value) * 127; + // send command if (motor == MOTOR_1) { if (value > 0) { return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); } + } else if (motor == MOTOR_2) { if (value > 0) { return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); } } + return -1; } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) { uint16_t sum = 0; + // bound - if (value > 1) value = 1; - if (value < -1) value = -1; - int16_t duty = 1500*value; + if (value > 1) { value = 1; } + + if (value < -1) { value = -1; } + + int16_t duty = 1500 * value; + // send command if (motor == MOTOR_1) { return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, - (uint8_t *)(&duty), 2, sum); + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, - (uint8_t *)(&duty), 2, sum); + (uint8_t *)(&duty), 2, sum); } + return -1; } @@ -247,7 +285,7 @@ int RoboClaw::resetEncoders() { uint16_t sum = 0; return _sendCommand(CMD_RESET_ENCODERS, - nullptr, 0, sum); + nullptr, 0, sum); } int RoboClaw::update() @@ -256,52 +294,57 @@ int RoboClaw::update() // check for exit condition every second // note "::poll" is required to distinguish global // poll from member function for driver - if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error + if (::poll(&_controlPoll, 1, 1000) < 0) { return -1; } // poll error // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); - setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1, _actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2, _actuators.control[CH_VOLTAGE_RIGHT]); } + return 0; } -uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +uint16_t RoboClaw::_sumBytes(uint8_t *buf, size_t n) { uint16_t sum = 0; + //printf("sum\n"); - for (size_t i=0;i Date: Mon, 19 Oct 2015 13:14:54 +0200 Subject: [PATCH 032/155] SF0X: Fixed code style --- src/drivers/sf0x/sf0x.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index ec5795025c..dbabef34fd 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -227,6 +227,7 @@ SF0X::SF0X(const char *port) : /* clear ONLCR flag (which appends a CR for every LF) */ uart_config.c_oflag &= ~ONLCR; + /* no parity, one stop bit */ uart_config.c_cflag &= ~(CSTOPB | PARENB); @@ -281,10 +282,12 @@ SF0X::init() /* do regular cdev init */ ret = CDev::init(); - if (ret != OK) break; + + if (ret != OK) { break; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); + if (_reports == nullptr) { warnx("mem err"); ret = -1; @@ -298,14 +301,14 @@ SF0X::init() struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } - } while(0); + } while (0); /* close the fd */ ::close(_fd); @@ -570,6 +573,7 @@ SF0X::collect() } else { return -EAGAIN; } + } else if (ret == 0) { return -EAGAIN; } @@ -691,11 +695,13 @@ SF0X::cycle() if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { DEVICE_LOG("collection error #%u", _consecutive_fail_count); } + _consecutive_fail_count++; /* restart the measurement state machine */ start(); return; + } else { /* apparently success */ _consecutive_fail_count = 0; @@ -886,7 +892,7 @@ test() warnx("read #%u", i); warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %llu", report.timestamp); } From 2154f0ca1d1d0c013ea27c8ffc445858a0e290a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:15:04 +0200 Subject: [PATCH 033/155] trone: fixed code style --- src/drivers/trone/trone.cpp | 47 +++++++++++++++++++------------------ 1 file changed, 24 insertions(+), 23 deletions(-) diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 83834b7f84..e3b77fb19c 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -212,7 +212,8 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; - static uint8_t crc8(uint8_t *p, uint8_t len) { +static uint8_t crc8(uint8_t *p, uint8_t len) +{ uint16_t i; uint16_t crc = 0x0; @@ -301,7 +302,7 @@ TRONE::init() _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); @@ -318,23 +319,23 @@ out: int TRONE::probe() { - uint8_t who_am_i=0; + uint8_t who_am_i = 0; const uint8_t cmd = TRONE_WHO_AM_I_REG; - - // set the I2C bus address - set_address(TRONE_BASEADDR); - - // can't use a single transfer as TROne need a bit of time for internal processing + + // set the I2C bus address + set_address(TRONE_BASEADDR); + + // can't use a single transfer as TROne need a bit of time for internal processing if (transfer(&cmd, 1, nullptr, 0) == OK) { - if ( transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL){ - return measure(); - } - } + if (transfer(nullptr, 0, &who_am_i, 1) == OK && who_am_i == TRONE_WHO_AM_I_REG_VAL) { + return measure(); + } + } DEVICE_DEBUG("WHO_AM_I byte mismatch 0x%02x should be 0x%02x\n", - (unsigned)who_am_i, - TRONE_WHO_AM_I_REG_VAL); + (unsigned)who_am_i, + TRONE_WHO_AM_I_REG_VAL); // not found on any address return -EIO; @@ -596,7 +597,7 @@ TRONE::collect() // This validation check can be used later _valid = crc8(val, 2) == val[2] && (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0; + && (float)report.current_distance < report.max_distance ? 1 : 0; /* publish it, if we are the primary */ if (_distance_sensor_topic != nullptr) { @@ -680,10 +681,10 @@ TRONE::cycle() if (_measure_ticks > USEC2TICK(TRONE_CONVERSION_INTERVAL)) { /* schedule a fresh cycle call when we are ready to measure again */ work_queue(HPWORK, - &_work, - (worker_t)&TRONE::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(TRONE_CONVERSION_INTERVAL)); return; } @@ -699,10 +700,10 @@ TRONE::cycle() /* schedule a fresh cycle call when the measurement is done */ work_queue(HPWORK, - &_work, - (worker_t)&TRONE::cycle_trampoline, - this, - USEC2TICK(TRONE_CONVERSION_INTERVAL)); + &_work, + (worker_t)&TRONE::cycle_trampoline, + this, + USEC2TICK(TRONE_CONVERSION_INTERVAL)); } void From fe216b5c5faec1f43f91a751837a902537eaca23 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:15:22 +0200 Subject: [PATCH 034/155] Device: Fixed code style --- src/drivers/device/cdev.cpp | 44 ++- src/drivers/device/device_nuttx.cpp | 15 +- src/drivers/device/device_nuttx.h | 45 +-- src/drivers/device/device_posix.cpp | 17 +- src/drivers/device/i2c_nuttx.cpp | 27 +- src/drivers/device/i2c_nuttx.h | 3 +- src/drivers/device/i2c_posix.cpp | 68 ++-- src/drivers/device/i2c_posix.h | 4 +- src/drivers/device/integrator.cpp | 3 +- src/drivers/device/integrator.h | 7 +- src/drivers/device/ringbuffer.cpp | 35 ++- src/drivers/device/ringbuffer.h | 7 +- src/drivers/device/sim.cpp | 4 +- src/drivers/device/sim.h | 10 +- src/drivers/device/spi.cpp | 16 +- src/drivers/device/spi.h | 8 +- src/drivers/device/vdev.cpp | 111 ++++--- src/drivers/device/vdev.h | 63 ++-- src/drivers/device/vdev_posix.cpp | 461 ++++++++++++++-------------- src/drivers/device/vfile.cpp | 2 +- src/drivers/device/vfile.h | 3 +- 21 files changed, 540 insertions(+), 413 deletions(-) diff --git a/src/drivers/device/cdev.cpp b/src/drivers/device/cdev.cpp index 048357ebc8..c43ab1353e 100644 --- a/src/drivers/device/cdev.cpp +++ b/src/drivers/device/cdev.cpp @@ -100,14 +100,16 @@ CDev::CDev(const char *name, _registered(false), _open_count(0) { - for (unsigned i = 0; i < _max_pollwaiters; i++) + for (unsigned i = 0; i < _max_pollwaiters; i++) { _pollset[i] = nullptr; + } } CDev::~CDev() { - if (_registered) + if (_registered) { unregister_driver(_devname); + } } int @@ -124,13 +126,16 @@ CDev::register_class_devname(const char *class_devname) char name[32]; snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, &fops, 0666, (void *)this); - if (ret == OK) break; + + if (ret == OK) { break; } + class_instance++; } if (class_instance == 4) { return ret; } + return class_instance; } @@ -148,15 +153,17 @@ CDev::init() // base class init first int ret = Device::init(); - if (ret != OK) + if (ret != OK) { goto out; + } // now register the driver if (_devname != nullptr) { ret = register_driver(_devname, &fops, 0666, (void *)this); - if (ret != OK) + if (ret != OK) { goto out; + } _registered = true; } @@ -182,8 +189,9 @@ CDev::open(file_t *filp) /* first-open callback may decline the open */ ret = open_first(filp); - if (ret != OK) + if (ret != OK) { _open_count--; + } } unlock(); @@ -209,8 +217,9 @@ CDev::close(file_t *filp) _open_count--; /* callback cannot decline the close */ - if (_open_count == 0) + if (_open_count == 0) { ret = close_last(filp); + } } else { ret = -EBADF; @@ -250,26 +259,30 @@ CDev::ioctl(file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - /* fetch a pointer to the driver's private data */ + /* fetch a pointer to the driver's private data */ case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; return OK; break; + case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); return OK; break; + case DEVIOCGPUBBLOCK: return _pub_blocked; break; } /* try the superclass. The different ioctl() function form - * means we need to copy arg */ - unsigned arg2 = arg; + * means we need to copy arg */ + unsigned arg2 = arg; int ret = Device::ioctl(cmd, arg2); - if (ret != -ENODEV) + + if (ret != -ENODEV) { return ret; + } return -ENOTTY; } @@ -305,8 +318,9 @@ CDev::poll(file_t *filp, struct pollfd *fds, bool setup) fds->revents |= fds->events & poll_state(filp); /* yes? post the notification */ - if (fds->revents != 0) + if (fds->revents != 0) { px4_sem_post(fds->sem); + } } } else { @@ -328,8 +342,9 @@ CDev::poll_notify(pollevent_t events) irqstate_t state = irqsave(); for (unsigned i = 0; i < _max_pollwaiters; i++) - if (nullptr != _pollset[i]) + if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); + } irqrestore(state); } @@ -342,8 +357,9 @@ CDev::poll_notify_one(struct pollfd *fds, pollevent_t events) /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ - if ((fds->revents != 0) && (fds->sem->semcount <= 0)) + if ((fds->revents != 0) && (fds->sem->semcount <= 0)) { px4_sem_post(fds->sem); + } } pollevent_t diff --git a/src/drivers/device/device_nuttx.cpp b/src/drivers/device/device_nuttx.cpp index 9146acb5da..12d42d5bfe 100644 --- a/src/drivers/device/device_nuttx.cpp +++ b/src/drivers/device/device_nuttx.cpp @@ -95,7 +95,7 @@ Device::Device(const char *name, _irq_attached(false) { sem_init(&_lock, 0, 1); - + /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ _device_id.devid = 0; @@ -109,8 +109,9 @@ Device::~Device() { sem_destroy(&_lock); - if (_irq_attached) + if (_irq_attached) { unregister_interrupt(_irq); + } } int @@ -126,8 +127,9 @@ Device::init() /* register */ ret = register_interrupt(_irq, this); - if (ret != OK) + if (ret != OK) { goto out; + } _irq_attached = true; } @@ -139,15 +141,17 @@ out: void Device::interrupt_enable() { - if (_irq_attached) + if (_irq_attached) { up_enable_irq(_irq); + } } void Device::interrupt_disable() { - if (_irq_attached) + if (_irq_attached) { up_disable_irq(_irq); + } } void @@ -224,6 +228,7 @@ Device::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return (int)_device_id.devid; } + return -ENODEV; } diff --git a/src/drivers/device/device_nuttx.h b/src/drivers/device/device_nuttx.h index 974d94ee45..c8f732fab1 100644 --- a/src/drivers/device/device_nuttx.h +++ b/src/drivers/device/device_nuttx.h @@ -118,7 +118,7 @@ public: * @param data The buffer from which values should be read. * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. - */ + */ virtual int write(unsigned address, void *data, unsigned count); /** @@ -147,13 +147,13 @@ public: parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; + enum DeviceBusType bus_type : 3; + uint8_t bus: 5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; - union DeviceId { + union DeviceId { struct DeviceStructure devid_s; uint32_t devid; }; @@ -189,14 +189,16 @@ protected: * * Note that we must loop as the wait may be interrupted by a signal. */ - void lock() { + void lock() + { do {} while (sem_wait(&_lock) != 0); } /** * Release the driver lock. */ - void unlock() { + void unlock() + { sem_post(&_lock); } @@ -416,7 +418,7 @@ protected: */ virtual int close_last(file_t *filp); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -425,7 +427,7 @@ protected: */ virtual int register_class_devname(const char *class_devname); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -440,7 +442,7 @@ protected: * * @return the file system string of the device handle */ - const char* get_devname() { return _devname; } + const char *get_devname() { return _devname; } bool _pub_blocked; /**< true if publishing should be blocked */ @@ -470,8 +472,8 @@ private: int remove_poll_waiter(struct pollfd *fds); /* do not allow copying this class */ - CDev(const CDev&); - CDev operator=(const CDev&); + CDev(const CDev &); + CDev operator=(const CDev &); }; /** @@ -503,7 +505,8 @@ protected: * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) + { return *(volatile uint32_t *)(_base + offset); } @@ -513,7 +516,8 @@ protected: * @param offset Register offset in bytes from the base address. * @param value Value to write. */ - void reg(uint32_t offset, uint32_t value) { + void reg(uint32_t offset, uint32_t value) + { *(volatile uint32_t *)(_base + offset) = value; } @@ -527,7 +531,8 @@ protected: * @param clearbits Bits to clear in the register * @param setbits Bits to set in the register */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) + { uint32_t val = reg(offset); val &= ~clearbits; val |= setbits; @@ -542,9 +547,9 @@ private: // class instance for primary driver of each class enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 + CLASS_DEVICE_PRIMARY = 0, + CLASS_DEVICE_SECONDARY = 1, + CLASS_DEVICE_TERTIARY = 2 }; #endif /* _DEVICE_DEVICE_H */ diff --git a/src/drivers/device/device_posix.cpp b/src/drivers/device/device_posix.cpp index bed97ff56d..088d7ccdf8 100644 --- a/src/drivers/device/device_posix.cpp +++ b/src/drivers/device/device_posix.cpp @@ -60,7 +60,7 @@ Device::Device(const char *name) : if (ret != 0) { PX4_WARN("SEM INIT FAIL: ret %d, %s", ret, strerror(errno)); } - + /* setup a default device ID. When bus_type is UNKNOWN the other fields are invalid */ _device_id.devid = 0; @@ -86,23 +86,24 @@ Device::init() int Device::dev_read(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int Device::dev_write(unsigned offset, void *data, unsigned count) { - return -ENODEV; + return -ENODEV; } int Device::dev_ioctl(unsigned operation, unsigned &arg) { - switch (operation) { - case DEVIOCGDEVICEID: - return (int)_device_id.devid; - } - return -ENODEV; + switch (operation) { + case DEVIOCGDEVICEID: + return (int)_device_id.devid; + } + + return -ENODEV; } } // namespace device diff --git a/src/drivers/device/i2c_nuttx.cpp b/src/drivers/device/i2c_nuttx.cpp index ad74f60c47..5efd2eac23 100644 --- a/src/drivers/device/i2c_nuttx.cpp +++ b/src/drivers/device/i2c_nuttx.cpp @@ -69,7 +69,7 @@ I2C::I2C(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -92,6 +92,7 @@ I2C::set_bus_clock(unsigned bus, unsigned clock_hz) if (_bus_clocks[index] > 0) { // DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz); } + _bus_clocks[index] = clock_hz; return OK; @@ -122,7 +123,7 @@ I2C::init() (void)up_i2cuninitialize(_dev); _dev = nullptr; DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %u KHz)", - _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); + _bus, _bus_clocks[bus_index] / 1000, _frequency / 1000); ret = -EINVAL; goto out; } @@ -162,13 +163,15 @@ I2C::init() // tell the world where we are DEVICE_LOG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %u KHz)", - _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); + _bus, _address, _bus_clocks[bus_index] / 1000, _frequency / 1000); out: + if ((ret != OK) && (_dev != nullptr)) { up_i2cuninitialize(_dev); _dev = nullptr; } + return ret; } @@ -208,18 +211,21 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs++; } - if (msgs == 0) + if (msgs == 0) { return -EINVAL; + } ret = I2C_TRANSFER(_dev, &msgv[0], msgs); /* success */ - if (ret == OK) + if (ret == OK) { break; + } /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) + if ((retry_count >= 1) || (retry_count >= _retries)) { up_i2creset(_dev); + } } while (retry_count++ < _retries); @@ -234,20 +240,23 @@ I2C::transfer(i2c_msg_s *msgv, unsigned msgs) unsigned retry_count = 0; /* force the device address into the message vector */ - for (unsigned i = 0; i < msgs; i++) + for (unsigned i = 0; i < msgs; i++) { msgv[i].addr = _address; + } do { ret = I2C_TRANSFER(_dev, msgv, msgs); /* success */ - if (ret == OK) + if (ret == OK) { break; + } /* if we have already retried once, or we are going to give up, then reset the bus */ - if ((retry_count >= 1) || (retry_count >= _retries)) + if ((retry_count >= 1) || (retry_count >= _retries)) { up_i2creset(_dev); + } } while (retry_count++ < _retries); diff --git a/src/drivers/device/i2c_nuttx.h b/src/drivers/device/i2c_nuttx.h index 97ab25672c..f4aa608857 100644 --- a/src/drivers/device/i2c_nuttx.h +++ b/src/drivers/device/i2c_nuttx.h @@ -134,7 +134,8 @@ protected: * * @param address The new bus address to set. */ - void set_address(uint16_t address) { + void set_address(uint16_t address) + { _address = address; _device_id.devid_s.address = _address; } diff --git a/src/drivers/device/i2c_posix.cpp b/src/drivers/device/i2c_posix.cpp index acd35c4cce..48bb3529e0 100644 --- a/src/drivers/device/i2c_posix.cpp +++ b/src/drivers/device/i2c_posix.cpp @@ -75,7 +75,7 @@ I2C::I2C(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } I2C::~I2C() @@ -105,6 +105,7 @@ I2C::init() } _fd = px4_open(get_devname(), PX4_F_RDONLY | PX4_F_WRONLY); + if (_fd < 0) { DEVICE_DEBUG("px4_open failed of device %s", get_devname()); return PX4_ERROR; @@ -116,16 +117,18 @@ I2C::init() if (simulate) { _fd = 10000; - } - else { + + } else { #ifndef __PX4_QURT // Open the actual I2C device and map to the virtual dev name _fd = ::open(get_devname(), O_RDWR); + if (_fd < 0) { warnx("could not open %s", get_devname()); px4_errno = errno; return PX4_ERROR; } + #endif } @@ -135,9 +138,9 @@ I2C::init() int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { - #ifndef __PX4_LINUX +#ifndef __PX4_LINUX return 1; - #else +#else struct i2c_msg msgv[2]; unsigned msgs; struct i2c_rdwr_ioctl_data packets; @@ -145,7 +148,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re unsigned retry_count = 0; if (_fd < 0) { - warnx("I2C device not opened"); + warnx("I2C device not opened"); return 1; } @@ -169,8 +172,9 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re msgs++; } - if (msgs == 0) + if (msgs == 0) { return -EINVAL; + } packets.msgs = msgv; packets.nmsgs = msgs; @@ -178,9 +182,10 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re if (simulate) { //warnx("I2C SIM: transfer_4 on %s", get_devname()); ret = PX4_OK; - } - else { + + } else { ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); + if (ret < 0) { warnx("I2C transfer failed"); return 1; @@ -188,28 +193,30 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re } /* success */ - if (ret == PX4_OK) + if (ret == PX4_OK) { break; + } } while (retry_count++ < _retries); return ret; - #endif +#endif } int I2C::transfer(struct i2c_msg *msgv, unsigned msgs) { - #ifndef __PX4_LINUX +#ifndef __PX4_LINUX return 1; - #else +#else struct i2c_rdwr_ioctl_data packets; int ret; unsigned retry_count = 0; /* force the device address into the message vector */ - for (unsigned i = 0; i < msgs; i++) + for (unsigned i = 0; i < msgs; i++) { msgv[i].addr = _address; + } do { packets.msgs = msgv; @@ -218,34 +225,38 @@ I2C::transfer(struct i2c_msg *msgv, unsigned msgs) if (simulate) { warnx("I2C SIM: transfer_2 on %s", get_devname()); ret = PX4_OK; - } - else { + + } else { ret = ::ioctl(_fd, I2C_RDWR, (unsigned long)&packets); } + if (ret < 0) { - warnx("I2C transfer failed"); - return 1; - } + warnx("I2C transfer failed"); + return 1; + } /* success */ - if (ret == PX4_OK) + if (ret == PX4_OK) { break; + } } while (retry_count++ < _retries); return ret; - #endif +#endif } int I2C::ioctl(device::file_t *filp, int cmd, unsigned long arg) { //struct i2c_rdwr_ioctl_data *packets = (i2c_rdwr_ioctl_data *)(void *)arg; switch (cmd) { - #ifdef __PX4_LINUX +#ifdef __PX4_LINUX + case I2C_RDWR: - warnx("Use I2C::transfer, not ioctl"); + warnx("Use I2C::transfer, not ioctl"); return 0; - #endif +#endif + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); @@ -256,27 +267,28 @@ ssize_t I2C::read(file_t *filp, char *buffer, size_t buflen) { if (simulate) { // FIXME no idea what this should be - warnx ("2C SIM I2C::read"); + warnx("2C SIM I2C::read"); return 0; } + #ifndef __PX4_QURT return ::read(_fd, buffer, buflen); #else - return 0; + return 0; #endif } ssize_t I2C::write(file_t *filp, const char *buffer, size_t buflen) { if (simulate) { - warnx ("2C SIM I2C::write"); + warnx("2C SIM I2C::write"); return buflen; } #ifndef __PX4_QURT return ::write(_fd, buffer, buflen); #else - return buflen; + return buflen; #endif } diff --git a/src/drivers/device/i2c_posix.h b/src/drivers/device/i2c_posix.h index c05100ae43..bee8d6a38a 100644 --- a/src/drivers/device/i2c_posix.h +++ b/src/drivers/device/i2c_posix.h @@ -127,8 +127,8 @@ private: int _fd; std::string _dname; - I2C(const device::I2C&); - I2C operator=(const device::I2C&); + I2C(const device::I2C &); + I2C operator=(const device::I2C &); }; } // namespace device diff --git a/src/drivers/device/integrator.cpp b/src/drivers/device/integrator.cpp index bc2baacc85..d6bdfbf5a6 100644 --- a/src/drivers/device/integrator.cpp +++ b/src/drivers/device/integrator.cpp @@ -82,7 +82,7 @@ Integrator::put(uint64_t timestamp, math::Vector<3> &val, math::Vector<3> &integ // Coning compensation derived by Paul Riseborough and Jonathan Challinger, // following: // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation - // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf + // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf i += ((_integral_auto + _last_delta * (1.0f / 6.0f)) % i) * 0.5f; } @@ -117,6 +117,7 @@ math::Vector<3> Integrator::read(bool auto_reset) { math::Vector<3> val = _integral_read; + if (auto_reset) { _integral_read(0) = 0.0f; _integral_read(1) = 0.0f; diff --git a/src/drivers/device/integrator.h b/src/drivers/device/integrator.h index 6de5c942d9..ace96af2e2 100644 --- a/src/drivers/device/integrator.h +++ b/src/drivers/device/integrator.h @@ -43,7 +43,8 @@ #include -class Integrator { +class Integrator +{ public: Integrator(uint64_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false); virtual ~Integrator(); @@ -91,6 +92,6 @@ private: bool _coning_comp_on; /**< coning compensation */ /* we don't want this class to be copied */ - Integrator(const Integrator&); - Integrator operator=(const Integrator&); + Integrator(const Integrator &); + Integrator operator=(const Integrator &); }; diff --git a/src/drivers/device/ringbuffer.cpp b/src/drivers/device/ringbuffer.cpp index c30e0a54df..5843312915 100644 --- a/src/drivers/device/ringbuffer.cpp +++ b/src/drivers/device/ringbuffer.cpp @@ -46,15 +46,16 @@ namespace ringbuffer RingBuffer::RingBuffer(unsigned num_items, size_t item_size) : _num_items(num_items), _item_size(item_size), - _buf(new char[(_num_items+1) * item_size]), - _head(_num_items), - _tail(_num_items) + _buf(new char[(_num_items + 1) * item_size]), + _head(_num_items), + _tail(_num_items) {} RingBuffer::~RingBuffer() { - if (_buf != nullptr) + if (_buf != nullptr) { delete[] _buf; + } } unsigned @@ -84,20 +85,25 @@ RingBuffer::size() void RingBuffer::flush() { - while (!empty()) + while (!empty()) { get(NULL); + } } bool RingBuffer::put(const void *val, size_t val_size) { unsigned next = _next(_head); + if (next != _tail) { - if ((val_size == 0) || (val_size > _item_size)) + if ((val_size == 0) || (val_size > _item_size)) { val_size = _item_size; + } + memcpy(&_buf[_head * _item_size], val, val_size); _head = next; return true; + } else { return false; } @@ -169,11 +175,14 @@ RingBuffer::force(const void *val, size_t val_size) bool overwrote = false; for (;;) { - if (put(val, val_size)) + if (put(val, val_size)) { break; + } + get(NULL); overwrote = true; } + return overwrote; } @@ -246,6 +255,7 @@ static inline bool my_sync_bool_compare_and_swap(volatile unsigned *a, unsigned *a = c; return true; } + return false; } @@ -259,8 +269,9 @@ RingBuffer::get(void *val, size_t val_size) unsigned candidate; unsigned next; - if ((val_size == 0) || (val_size > _item_size)) + if ((val_size == 0) || (val_size > _item_size)) { val_size = _item_size; + } do { /* decide which element we think we're going to read */ @@ -270,13 +281,15 @@ RingBuffer::get(void *val, size_t val_size) next = _next(candidate); /* go ahead and read from this index */ - if (val != NULL) + if (val != NULL) { memcpy(val, &_buf[candidate * _item_size], val_size); + } /* if the tail pointer didn't change, we got our item */ } while (!__PX4_SBCAP(&_tail, candidate, next)); return true; + } else { return false; } @@ -377,10 +390,12 @@ bool RingBuffer::resize(unsigned new_size) { char *old_buffer; - char *new_buffer = new char [(new_size+1) * _item_size]; + char *new_buffer = new char [(new_size + 1) * _item_size]; + if (new_buffer == nullptr) { return false; } + old_buffer = _buf; _buf = new_buffer; _num_items = new_size; diff --git a/src/drivers/device/ringbuffer.h b/src/drivers/device/ringbuffer.h index 4fcdaf47fa..32077edb64 100644 --- a/src/drivers/device/ringbuffer.h +++ b/src/drivers/device/ringbuffer.h @@ -46,7 +46,8 @@ namespace ringbuffer __EXPORT { -class RingBuffer { +class RingBuffer +{ public: RingBuffer(unsigned ring_size, size_t entry_size); virtual ~RingBuffer(); @@ -171,8 +172,8 @@ private: unsigned _next(unsigned index); /* we don't want this class to be copied */ - RingBuffer(const RingBuffer&); - RingBuffer operator=(const RingBuffer&); + RingBuffer(const RingBuffer &); + RingBuffer operator=(const RingBuffer &); }; } // namespace ringbuffer diff --git a/src/drivers/device/sim.cpp b/src/drivers/device/sim.cpp index 787a3826c9..5b089af935 100644 --- a/src/drivers/device/sim.cpp +++ b/src/drivers/device/sim.cpp @@ -69,7 +69,7 @@ SIM::SIM(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = address; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } SIM::~SIM() @@ -104,7 +104,7 @@ SIM::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re if (recv_len > 0) { PX4_DEBUG("SIM: receiving %d bytes", recv_len); - + // TODO - write data to recv; } diff --git a/src/drivers/device/sim.h b/src/drivers/device/sim.h index 139967f6e8..1e84af3863 100644 --- a/src/drivers/device/sim.h +++ b/src/drivers/device/sim.h @@ -37,7 +37,7 @@ * Base class for devices on simulation bus. */ -#pragma once +#pragma once #include "vdev.h" @@ -98,14 +98,14 @@ protected: * otherwise. */ virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + uint8_t *recv, unsigned recv_len); private: uint16_t _address; - const char * _devname; + const char *_devname; - SIM(const device::SIM&); - SIM operator=(const device::SIM&); + SIM(const device::SIM &); + SIM operator=(const device::SIM &); }; } // namespace device diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 698fa63996..ce81878102 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -80,7 +80,7 @@ SPI::SPI(const char *name, _device_id.devid_s.bus = bus; _device_id.devid_s.address = (uint8_t)device; // devtype needs to be filled in by the driver - _device_id.devid_s.devtype = 0; + _device_id.devid_s.devtype = 0; } SPI::~SPI() @@ -94,8 +94,9 @@ SPI::init() int ret = OK; /* attach to the spi bus */ - if (_dev == nullptr) + if (_dev == nullptr) { _dev = up_spiinitialize(_bus); + } if (_dev == nullptr) { DEVICE_DEBUG("failed to init SPI"); @@ -141,34 +142,37 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { int result; - if ((send == nullptr) && (recv == nullptr)) + if ((send == nullptr) && (recv == nullptr)) { return -EINVAL; + } LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; /* lock the bus as required */ switch (mode) { default: - case LOCK_PREEMPTION: - { + case LOCK_PREEMPTION: { irqstate_t state = irqsave(); result = _transfer(send, recv, len); irqrestore(state); } break; + case LOCK_THREADS: SPI_LOCK(_dev, true); result = _transfer(send, recv, len); SPI_LOCK(_dev, false); break; + case LOCK_NONE: result = _transfer(send, recv, len); break; } + return result; } -void +void SPI::set_frequency(uint32_t frequency) { _frequency = frequency; diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 2f44f3cafa..9c3bf36f25 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -109,12 +109,12 @@ protected: * Set the SPI bus frequency * This is used to change frequency on the fly. Some sensors * (such as the MPU6000) need a lower frequency for setup - * registers and can handle higher frequency for sensor + * registers and can handle higher frequency for sensor * value registers * * @param frequency Frequency to set (Hz) */ - void set_frequency(uint32_t frequency); + void set_frequency(uint32_t frequency); /** * Locking modes supported by the driver. @@ -134,8 +134,8 @@ private: struct spi_dev_s *_dev; /* this class does not allow copying */ - SPI(const SPI&); - SPI operator=(const SPI&); + SPI(const SPI &); + SPI operator=(const SPI &); protected: int _bus; diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 1900f19fbe..60761ed511 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -54,8 +54,9 @@ struct px4_dev_t { char *name; void *cdev; - px4_dev_t(const char *n, void *c) : cdev(c) { - name = strdup(n); + px4_dev_t(const char *n, void *c) : cdev(c) + { + name = strdup(n); } ~px4_dev_t() { free(name); } @@ -85,21 +86,26 @@ VDev::VDev(const char *name, _open_count(0) { PX4_DEBUG("VDev::VDev"); - for (unsigned i = 0; i < _max_pollwaiters; i++) + + for (unsigned i = 0; i < _max_pollwaiters; i++) { _pollset[i] = nullptr; + } } VDev::~VDev() { PX4_DEBUG("VDev::~VDev"); - if (_registered) + + if (_registered) { unregister_driver(_devname); + } } int VDev::register_class_devname(const char *class_devname) { PX4_DEBUG("VDev::register_class_devname %s", class_devname); + if (class_devname == nullptr) { return -EINVAL; } @@ -111,13 +117,16 @@ VDev::register_class_devname(const char *class_devname) char name[32]; snprintf(name, sizeof(name), "%s%d", class_devname, class_instance); ret = register_driver(name, (void *)this); - if (ret == OK) break; + + if (ret == OK) { break; } + class_instance++; } if (class_instance == 4) { return ret; } + return class_instance; } @@ -127,17 +136,19 @@ VDev::register_driver(const char *name, void *data) PX4_DEBUG("VDev::register_driver %s", name); int ret = -ENOSPC; - if (name == NULL || data == NULL) + if (name == NULL || data == NULL) { return -EINVAL; + } // Make sure the device does not already exist // FIXME - convert this to a map for efficiency - for (int i=0;iname,name) == 0)) { + for (int i = 0; i < PX4_MAX_DEV; ++i) { + if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) { return -EEXIST; } } - for (int i=0;iname) == 0)) { delete devmap[i]; devmap[i] = NULL; @@ -169,6 +183,7 @@ VDev::unregister_driver(const char *name) break; } } + return ret; } @@ -178,14 +193,16 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc PX4_DEBUG("VDev::unregister_class_devname"); char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); - for (int i=0;iname,name) == 0) { + + for (int i = 0; i < PX4_MAX_DEV; ++i) { + if (devmap[i] && strcmp(devmap[i]->name, name) == 0) { delete devmap[i]; PX4_DEBUG("Unregistered class DEV %s", name); devmap[i] = NULL; return PX4_OK; } } + return -EINVAL; } @@ -197,15 +214,17 @@ VDev::init() // base class init first int ret = Device::init(); - if (ret != PX4_OK) + if (ret != PX4_OK) { goto out; + } // now register the driver if (_devname != nullptr) { ret = register_driver(_devname, (void *)this); - if (ret != PX4_OK) + if (ret != PX4_OK) { goto out; + } _registered = true; } @@ -232,8 +251,9 @@ VDev::open(file_t *filep) /* first-open callback may decline the open */ ret = open_first(filep); - if (ret != PX4_OK) + if (ret != PX4_OK) { _open_count--; + } } unlock(); @@ -261,8 +281,9 @@ VDev::close(file_t *filep) _open_count--; /* callback cannot decline the close */ - if (_open_count == 0) + if (_open_count == 0) { ret = close_last(filep); + } } else { ret = -EBADF; @@ -309,22 +330,26 @@ VDev::ioctl(file_t *filep, int cmd, unsigned long arg) switch (cmd) { - /* fetch a pointer to the driver's private data */ + /* fetch a pointer to the driver's private data */ case DIOC_GETPRIV: *(void **)(uintptr_t)arg = (void *)this; ret = PX4_OK; break; + case DEVIOCSPUBBLOCK: _pub_blocked = (arg != 0); ret = PX4_OK; break; + case DEVIOCGPUBBLOCK: ret = _pub_blocked; break; - case DEVIOCGDEVICEID: - ret = (int)_device_id.devid; + + case DEVIOCGDEVICEID: + ret = (int)_device_id.devid; PX4_INFO("IOCTL DEVIOCGDEVICEID %d", ret); break; + default: break; } @@ -365,8 +390,10 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) fds->revents |= fds->events & poll_state(filep); /* yes? post the notification */ - if (fds->revents != 0) + if (fds->revents != 0) { px4_sem_post(fds->sem); + } + } else { PX4_WARN("Store Poll Waiter error."); } @@ -392,8 +419,9 @@ VDev::poll_notify(pollevent_t events) lock(); for (unsigned i = 0; i < _max_pollwaiters; i++) - if (nullptr != _pollset[i]) + if (nullptr != _pollset[i]) { poll_notify_one(_pollset[i], events); + } unlock(); } @@ -408,7 +436,7 @@ VDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) /* update the reported event set */ fds->revents |= fds->events & events; - PX4_DEBUG(" Events fds=%p %0x %0x %0x %d",fds, fds->revents, fds->events, events, value); + PX4_DEBUG(" Events fds=%p %0x %0x %0x %d", fds, fds->revents, fds->events, events, value); /* if the state is now interesting, wake the waiter if it's still asleep */ /* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */ @@ -432,6 +460,7 @@ VDev::store_poll_waiter(px4_pollfd_struct_t *fds) * Look for a free slot. */ PX4_DEBUG("VDev::store_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { if (nullptr == _pollset[i]) { @@ -449,6 +478,7 @@ int VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) { PX4_DEBUG("VDev::remove_poll_waiter"); + for (unsigned i = 0; i < _max_pollwaiters; i++) { if (fds == _pollset[i]) { @@ -465,8 +495,9 @@ VDev::remove_poll_waiter(px4_pollfd_struct_t *fds) VDev *VDev::getDev(const char *path) { PX4_DEBUG("VDev::getDev"); - int i=0; - for (; iname, path); //} @@ -474,14 +505,16 @@ VDev *VDev::getDev(const char *path) return (VDev *)(devmap[i]->cdev); } } + return NULL; } void VDev::showDevices() { - int i=0; + int i = 0; PX4_INFO("Devices:"); - for (; iname, "/dev/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } @@ -490,9 +523,10 @@ void VDev::showDevices() void VDev::showTopics() { - int i=0; + int i = 0; PX4_INFO("Devices:"); - for (; iname, "/obj/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } @@ -501,11 +535,12 @@ void VDev::showTopics() void VDev::showFiles() { - int i=0; + int i = 0; PX4_INFO("Files:"); - for (; iname, "/obj/", 5) != 0 && - strncmp(devmap[i]->name, "/dev/", 5) != 0) { + strncmp(devmap[i]->name, "/dev/", 5) != 0) { PX4_INFO(" %s", devmap[i]->name); } } @@ -513,17 +548,21 @@ void VDev::showFiles() const char *VDev::topicList(unsigned int *next) { - for (;*nextname, "/obj/", 5) == 0) + for (; *next < PX4_MAX_DEV; (*next)++) + if (devmap[*next] && strncmp(devmap[(*next)]->name, "/obj/", 5) == 0) { return devmap[(*next)++]->name; + } + return NULL; } const char *VDev::devList(unsigned int *next) { - for (;*nextname, "/dev/", 5) == 0) + for (; *next < PX4_MAX_DEV; (*next)++) + if (devmap[*next] && strncmp(devmap[(*next)]->name, "/dev/", 5) == 0) { return devmap[(*next)++]->name; + } + return NULL; } diff --git a/src/drivers/device/vdev.h b/src/drivers/device/vdev.h index 89e06358f5..d1fb8ff8e1 100644 --- a/src/drivers/device/vdev.h +++ b/src/drivers/device/vdev.h @@ -37,7 +37,7 @@ * Definitions for the generic base classes in the virtual device framework. */ -#pragma once +#pragma once /* * Includes here should only cover the needs of the framework definitions. @@ -118,17 +118,17 @@ public: * @param data The buffer from which values should be read. * @param count The number of items to write. * @return The number of items written on success, negative errno otherwise. - */ + */ virtual int dev_write(unsigned address, void *data, unsigned count); - /** - * Perform a device-specific operation. - * - * @param operation The operation to perform. - * @param arg An argument to the operation. - * @return Negative errno on error, OK or positive value on success. - */ - virtual int dev_ioctl(unsigned operation, unsigned &arg); + /** + * Perform a device-specific operation. + * + * @param operation The operation to perform. + * @param arg An argument to the operation. + * @return Negative errno on error, OK or positive value on success. + */ + virtual int dev_ioctl(unsigned operation, unsigned &arg); /* device bus types for DEVID @@ -148,13 +148,13 @@ public: parameter protocol without loss of information. */ struct DeviceStructure { - enum DeviceBusType bus_type:3; - uint8_t bus:5; // which instance of the bus type - uint8_t address; // address on the bus (eg. I2C address) - uint8_t devtype; // device class specific device type - }; + enum DeviceBusType bus_type : 3; + uint8_t bus: 5; // which instance of the bus type + uint8_t address; // address on the bus (eg. I2C address) + uint8_t devtype; // device class specific device type + }; - union DeviceId { + union DeviceId { struct DeviceStructure devid_s; uint32_t devid; }; @@ -179,7 +179,8 @@ protected: * * Note that we must loop as the wait may be interrupted by a signal. */ - void lock() { + void lock() + { DEVICE_DEBUG("lock"); do {} while (px4_sem_wait(&_lock) != 0); } @@ -187,7 +188,8 @@ protected: /** * Release the driver lock. */ - void unlock() { + void unlock() + { DEVICE_DEBUG("unlock"); px4_sem_post(&_lock); } @@ -330,7 +332,7 @@ public: * * @return the file system string of the device handle */ - const char* get_devname() { return _devname; } + const char *get_devname() { return _devname; } protected: @@ -395,7 +397,7 @@ protected: */ virtual int close_last(file_t *filep); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -404,7 +406,7 @@ protected: */ virtual int register_class_devname(const char *class_devname); - /** + /** * Register a class device name, automatically adding device * class instance suffix if need be. * @@ -442,7 +444,7 @@ private: int remove_poll_waiter(px4_pollfd_struct_t *fds); /* do not allow copying this class */ - VDev(const VDev&); + VDev(const VDev &); //VDev operator=(const VDev&); }; @@ -464,7 +466,7 @@ public: PIO(const char *name, const char *devname, unsigned long base - ); + ); virtual ~PIO(); virtual int init(); @@ -476,7 +478,8 @@ protected: * * @param offset Register offset in bytes from the base address. */ - uint32_t reg(uint32_t offset) { + uint32_t reg(uint32_t offset) + { return *(volatile uint32_t *)(_base + offset); } @@ -486,7 +489,8 @@ protected: * @param offset Register offset in bytes from the base address. * @param value Value to write. */ - void reg(uint32_t offset, uint32_t value) { + void reg(uint32_t offset, uint32_t value) + { *(volatile uint32_t *)(_base + offset) = value; } @@ -500,7 +504,8 @@ protected: * @param clearbits Bits to clear in the register * @param setbits Bits to set in the register */ - void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) { + void modify(uint32_t offset, uint32_t clearbits, uint32_t setbits) + { uint32_t val = reg(offset); val &= ~clearbits; val |= setbits; @@ -516,8 +521,8 @@ private: // class instance for primary driver of each class enum CLASS_DEVICE { - CLASS_DEVICE_PRIMARY=0, - CLASS_DEVICE_SECONDARY=1, - CLASS_DEVICE_TERTIARY=2 + CLASS_DEVICE_PRIMARY = 0, + CLASS_DEVICE_SECONDARY = 1, + CLASS_DEVICE_TERTIARY = 2 }; diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index 5647bad892..a30bc8d06b 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -54,254 +54,265 @@ using namespace device; extern "C" { -static void timer_cb(void *data) -{ - px4_sem_t *p_sem = (px4_sem_t *)data; - px4_sem_post(p_sem); - PX4_DEBUG("timer_handler: Timer expired"); -} + static void timer_cb(void *data) + { + px4_sem_t *p_sem = (px4_sem_t *)data; + px4_sem_post(p_sem); + PX4_DEBUG("timer_handler: Timer expired"); + } #define PX4_MAX_FD 200 -static device::file_t *filemap[PX4_MAX_FD] = {}; + static device::file_t *filemap[PX4_MAX_FD] = {}; -int px4_errno; + int px4_errno; -inline bool valid_fd(int fd) -{ - return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); -} - -int px4_open(const char *path, int flags, ...) -{ - PX4_DEBUG("px4_open"); - VDev *dev = VDev::getDev(path); - int ret = 0; - int i; - mode_t mode; - - if (!dev && (flags & (PX4_F_WRONLY|PX4_F_CREAT)) != 0 && - strncmp(path, "/obj/", 5) != 0 && - strncmp(path, "/dev/", 5) != 0) + inline bool valid_fd(int fd) { - va_list p; - va_start(p, flags); - mode = va_arg(p, mode_t); - va_end(p); + return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + } - // Create the file - PX4_DEBUG("Creating virtual file %s", path); - dev = VFile::createFile(path, mode); - } - if (dev) { - for (i=0; iopen(filemap[i]); - } - else { - PX4_WARN("exceeded maximum number of file descriptors!"); - ret = -ENOENT; - } - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - return -1; - } - PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); - return filemap[i]->fd; -} - -int px4_close(int fd) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_close fd = %d", fd); - ret = dev->close(filemap[fd]); - filemap[fd] = NULL; - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} - -ssize_t px4_read(int fd, void *buffer, size_t buflen) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_read fd = %d", fd); - ret = dev->read(filemap[fd], (char *)buffer, buflen); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} - -ssize_t px4_write(int fd, const void *buffer, size_t buflen) -{ - int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_write fd = %d", fd); - ret = dev->write(filemap[fd], (const char *)buffer, buflen); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - ret = PX4_ERROR; - } - return ret; -} - -int px4_ioctl(int fd, int cmd, unsigned long arg) -{ - PX4_DEBUG("px4_ioctl fd = %d", fd); - int ret = 0; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - ret = dev->ioctl(filemap[fd], cmd, arg); - } - else { - ret = -EINVAL; - } - if (ret < 0) { - px4_errno = -ret; - } - - return ret; -} - -int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) -{ - px4_sem_t sem; - int count = 0; - int ret = -1; - unsigned int i; - - PX4_DEBUG("Called px4_poll timeout = %d", timeout); - px4_sem_init(&sem, 0, 0); - - // For each fd - for (i=0; ivdev);; - PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); - ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + if (!dev && (flags & (PX4_F_WRONLY | PX4_F_CREAT)) != 0 && + strncmp(path, "/obj/", 5) != 0 && + strncmp(path, "/dev/", 5) != 0) { + va_list p; + va_start(p, flags); + mode = va_arg(p, mode_t); + va_end(p); - if (ret < 0) - break; - } - } - - if (ret >= 0) - { - if (timeout > 0) - { - // Use a work queue task - work_s _hpwork; - - hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000*timeout); - px4_sem_wait(&sem); - - // Make sure timer thread is killed before sem goes - // out of scope - hrt_work_cancel(&_hpwork); - } - else if (timeout < 0) - { - px4_sem_wait(&sem); + // Create the file + PX4_DEBUG("Creating virtual file %s", path); + dev = VFile::createFile(path, mode); } - // For each fd - for (i=0; ivdev);; - PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); - ret = dev->poll(filemap[fds[i].fd], &fds[i], false); - - if (ret < 0) + if (dev) { + for (i = 0; i < PX4_MAX_FD; ++i) { + if (filemap[i] == 0) { + filemap[i] = new device::file_t(flags, dev, i); break; + } + } - if (fds[i].revents) - count += 1; + if (i < PX4_MAX_FD) { + ret = dev->open(filemap[i]); + + } else { + PX4_WARN("exceeded maximum number of file descriptors!"); + ret = -ENOENT; + } + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + return -1; + } + + PX4_DEBUG("px4_open fd = %d", filemap[i]->fd); + return filemap[i]->fd; + } + + int px4_close(int fd) + { + int ret; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_close fd = %d", fd); + ret = dev->close(filemap[fd]); + filemap[fd] = NULL; + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + + return ret; + } + + ssize_t px4_read(int fd, void *buffer, size_t buflen) + { + int ret; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_read fd = %d", fd); + ret = dev->read(filemap[fd], (char *)buffer, buflen); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + + return ret; + } + + ssize_t px4_write(int fd, const void *buffer, size_t buflen) + { + int ret; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + PX4_DEBUG("px4_write fd = %d", fd); + ret = dev->write(filemap[fd], (const char *)buffer, buflen); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + ret = PX4_ERROR; + } + + return ret; + } + + int px4_ioctl(int fd, int cmd, unsigned long arg) + { + PX4_DEBUG("px4_ioctl fd = %d", fd); + int ret = 0; + + if (valid_fd(fd)) { + VDev *dev = (VDev *)(filemap[fd]->vdev); + ret = dev->ioctl(filemap[fd], cmd, arg); + + } else { + ret = -EINVAL; + } + + if (ret < 0) { + px4_errno = -ret; + } + + return ret; + } + + int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) + { + px4_sem_t sem; + int count = 0; + int ret = -1; + unsigned int i; + + PX4_DEBUG("Called px4_poll timeout = %d", timeout); + px4_sem_init(&sem, 0, 0); + + // For each fd + for (i = 0; i < nfds; ++i) { + fds[i].sem = &sem; + fds[i].revents = 0; + fds[i].priv = NULL; + + // If fd is valid + if (valid_fd(fds[i].fd)) { + VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], true); + + if (ret < 0) { + break; + } } } + + if (ret >= 0) { + if (timeout > 0) { + // Use a work queue task + work_s _hpwork; + + hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)&sem, 1000 * timeout); + px4_sem_wait(&sem); + + // Make sure timer thread is killed before sem goes + // out of scope + hrt_work_cancel(&_hpwork); + + } else if (timeout < 0) { + px4_sem_wait(&sem); + } + + // For each fd + for (i = 0; i < nfds; ++i) { + // If fd is valid + if (valid_fd(fds[i].fd)) { + VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); + ret = dev->poll(filemap[fds[i].fd], &fds[i], false); + + if (ret < 0) { + break; + } + + if (fds[i].revents) { + count += 1; + } + } + } + } + + px4_sem_destroy(&sem); + + return count; } - px4_sem_destroy(&sem); - - return count; -} - -int px4_fsync(int fd) -{ - return 0; -} - -int px4_access(const char *pathname, int mode) -{ - if (mode != F_OK) { - errno = EINVAL; - return -1; + int px4_fsync(int fd) + { + return 0; } - VDev *dev = VDev::getDev(pathname); - return (dev != nullptr) ? 0 : -1; -} -void px4_show_devices() -{ - VDev::showDevices(); -} + int px4_access(const char *pathname, int mode) + { + if (mode != F_OK) { + errno = EINVAL; + return -1; + } -void px4_show_topics() -{ - VDev::showTopics(); -} + VDev *dev = VDev::getDev(pathname); + return (dev != nullptr) ? 0 : -1; + } -void px4_show_files() -{ - VDev::showFiles(); -} + void px4_show_devices() + { + VDev::showDevices(); + } -const char * px4_get_device_names(unsigned int *handle) -{ - return VDev::devList(handle); -} + void px4_show_topics() + { + VDev::showTopics(); + } -const char * px4_get_topic_names(unsigned int *handle) -{ - return VDev::topicList(handle); -} + void px4_show_files() + { + VDev::showFiles(); + } + + const char *px4_get_device_names(unsigned int *handle) + { + return VDev::devList(handle); + } + + const char *px4_get_topic_names(unsigned int *handle) + { + return VDev::topicList(handle); + } } diff --git a/src/drivers/device/vfile.cpp b/src/drivers/device/vfile.cpp index 2d2d81c816..e142f626d1 100644 --- a/src/drivers/device/vfile.cpp +++ b/src/drivers/device/vfile.cpp @@ -48,7 +48,7 @@ VFile::VFile(const char *fname, mode_t mode) : { } -VFile * VFile::createFile(const char *fname, mode_t mode) +VFile *VFile::createFile(const char *fname, mode_t mode) { VFile *me = new VFile(fname, mode); me->register_driver(fname, me); diff --git a/src/drivers/device/vfile.h b/src/drivers/device/vfile.h index d7c5e15d7f..6bea62d1f1 100644 --- a/src/drivers/device/vfile.h +++ b/src/drivers/device/vfile.h @@ -44,7 +44,8 @@ #include #include -class VFile : public device::VDev { +class VFile : public device::VDev +{ public: static VFile *createFile(const char *fname, mode_t mode); From e2e83cd578e51572202d9e1b218e592438e90b70 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:15:35 +0200 Subject: [PATCH 035/155] Airspeed: Fixed code style --- src/drivers/airspeed/airspeed.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 0b6227a1be..df7f8c633a 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -133,6 +133,7 @@ Airspeed::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); + if (_reports == nullptr) { goto out; } From ab854f99e845a2d34a51ae80f9df3a2a27e3323f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:15:49 +0200 Subject: [PATCH 036/155] AR Drone interface: Fixed code style --- .../ardrone_interface/ardrone_interface.c | 51 ++++++++++-------- .../ardrone_interface/ardrone_motor_control.c | 54 +++++++++++-------- 2 files changed, 62 insertions(+), 43 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 1777e7f27c..8ad0ee7168 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -92,8 +92,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -122,11 +124,11 @@ int ardrone_interface_main(int argc, char *argv[]) thread_should_exit = false; ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 1100, - ardrone_interface_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 1100, + ardrone_interface_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } @@ -138,9 +140,11 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("not started"); } + exit(0); } @@ -212,19 +216,23 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { - if (i+1 < argc) { - int motor = atoi(argv[i+1]); + if (i + 1 < argc) { + int motor = atoi(argv[i + 1]); + if (motor > 0 && motor < 5) { test_motor = motor; + } else { thread_running = false; errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage); } + } else { thread_running = false; errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); } } + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set if (argc > i + 1) { device = argv[i + 1]; @@ -314,6 +322,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int motors[4] = {0, 0, 0, 0}; motors[test_motor - 1] = 10; ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]); + } else { ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); } @@ -338,33 +347,33 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (counter % 24 == 0) { - if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); + if (led_counter == 0) { ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); + if (led_counter == 1) { ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); + if (led_counter == 2) { ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0); } - if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); + if (led_counter == 3) { ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0); } - if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); + if (led_counter == 4) { ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0); } - if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); + if (led_counter == 5) { ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0); } - if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); + if (led_counter == 6) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0); } - if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); + if (led_counter == 7) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0); } - if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); + if (led_counter == 8) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0); } - if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); + if (led_counter == 9) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1); } - if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); + if (led_counter == 10) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1); } - if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); + if (led_counter == 11) { ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0); } led_counter++; - if (led_counter == 12) led_counter = 0; + if (led_counter == 12) { led_counter = 0; } } /* run at approximately 200 Hz */ diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 46e1f86511..4991389129 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -108,7 +108,7 @@ void ar_enable_broadcast(int fd) int ar_multiplexing_init() { int fd; - + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { @@ -176,7 +176,7 @@ int ar_select_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_CLEAR, motor_gpios); } else { - /* select reqested motor */ + /* select reqested motor */ ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); } @@ -199,7 +199,7 @@ int ar_deselect_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_SET, motor_gpios); } else { - /* deselect reqested motor */ + /* deselect reqested motor */ ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]); } @@ -235,7 +235,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[0]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * write 0x91 - request checksum @@ -243,7 +243,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[1]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*120); + usleep(UART_TRANSFER_TIME_BYTE_US * 120); /* * write 0xA1 - set status OK @@ -252,7 +252,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[2]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * set as motor i, where i = 1..4 @@ -268,7 +268,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[4]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*11); + usleep(UART_TRANSFER_TIME_BYTE_US * 11); ar_deselect_motor(gpios, i); /* sleep 200 ms */ @@ -304,13 +304,15 @@ int ar_init_motors(int ardrone_uart, int gpios) warnx("Failed %d times", -errcounter); fflush(stdout); } + return errcounter; } /** * Sets the leds on the motor controllers, 1 turns led on, 0 off. */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, + uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) { /* * 2 bytes are sent. The first 3 bits describe the command: 011 means led control @@ -322,12 +324,15 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t * 011 rrrr 0000 gggg 0 */ uint8_t leds[2]; - leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); - leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | (( + led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | (( + led1_green & 0x01) << 1); write(ardrone_uart, leds, 2); } -int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ const unsigned int min_motor_interval = 4900; static uint64_t last_motor_time = 0; @@ -338,6 +343,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[2] = motor3; outputs.output[3] = motor4; static orb_advert_t pub = 0; + if (pub == 0) { /* advertise to channel 0 / primary */ pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); @@ -355,15 +361,18 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor if (ret == sizeof(buf)) { return OK; + } else { return ret; } + } else { return -ERROR; } } -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) { +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) +{ float roll_control = actuators->control[0]; float pitch_control = actuators->control[1]; @@ -385,7 +394,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { - output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 + output_band = motor_thrust / startpoint_full_control; // linear from 0 to 1 + } else { output_band = 1.0f; } @@ -407,7 +417,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); /* if one motor is saturated, reduce throttle */ - float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; + float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]), fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; if (saturation > 0.0f) { @@ -428,16 +438,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* set the motor values */ /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[0] = (uint16_t)(motor_calc[0] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[1] = (uint16_t)(motor_calc[1] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[2] = (uint16_t)(motor_calc[2] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[3] = (uint16_t)(motor_calc[3] * ((float)max_gas - min_gas) + min_gas); /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[0] = (uint16_t)(motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[1] = (uint16_t)(motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[2] = (uint16_t)(motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[3] = (uint16_t)(motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); /* Failsafe logic for min values - should never be necessary */ motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas; From 25434055c655b499c22a23a2927cc89629d0636c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:16:02 +0200 Subject: [PATCH 037/155] Camera trigger: Fixed code style --- src/drivers/camera_trigger/camera_trigger.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index a75516b716..d07ec24684 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -195,6 +195,7 @@ CameraTrigger::CameraTrigger() : // Convert number to individual channels unsigned i = 0; int single_pin; + while ((single_pin = pin_list % 10)) { _pins[i] = single_pin - 1; @@ -226,18 +227,19 @@ CameraTrigger::control(bool on) if (on) { // schedule trigger on and off calls hrt_call_every(&_engagecall, 500, (_interval * 1000), - (hrt_callout)&CameraTrigger::engage, this); + (hrt_callout)&CameraTrigger::engage, this); // schedule trigger on and off calls hrt_call_every(&_disengagecall, 500 + (_activation_time * 1000), (_interval * 1000), - (hrt_callout)&CameraTrigger::disengage, this); + (hrt_callout)&CameraTrigger::disengage, this); + } else { // cancel all calls hrt_cancel(&_engagecall); hrt_cancel(&_disengagecall); // ensure that the pin is off hrt_call_after(&_disengagecall, 500, - (hrt_callout)&CameraTrigger::disengage, this); + (hrt_callout)&CameraTrigger::disengage, this); } _trigger_enabled = on; @@ -249,7 +251,7 @@ CameraTrigger::start() for (unsigned i = 0; i < sizeof(_pins) / sizeof(_pins[0]); i++) { stm32_configgpio(_gpios[_pins[i]]); - stm32_gpiowrite(_gpios[_pins[i]], !_polarity); + stm32_gpiowrite(_gpios[_pins[i]], !_polarity); } // enable immediate if configured that way @@ -316,7 +318,7 @@ CameraTrigger::cycle_trampoline(void *arg) } work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline, - camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); + camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec)); } void @@ -361,7 +363,7 @@ CameraTrigger::info() { warnx("state : %s", _trigger_enabled ? "enabled" : "disabled"); warnx("pins 1-3 : %d,%d,%d polarity : %s", _pins[0], _pins[1], _pins[2], - _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); + _polarity ? "ACTIVE_HIGH" : "ACTIVE_LOW"); warnx("interval : %.2f", (double)_interval); } From ff6676ef8cde121df09c327821f3a8060e2ef013 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:16:11 +0200 Subject: [PATCH 038/155] GPS: Fixed code style --- src/drivers/gps/ashtech.cpp | 40 +++--- src/drivers/gps/gps.cpp | 75 ++++++----- src/drivers/gps/mtk.cpp | 8 +- src/drivers/gps/ubx.cpp | 240 ++++++++++++++++++++++++------------ 4 files changed, 237 insertions(+), 126 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index eca775745f..322d275549 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -37,7 +37,7 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { - char * endp; + char *endp; if (len < 7) { return 0; } @@ -69,7 +69,8 @@ int ASHTECH::handle_message(int len) Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ double ashtech_time = 0.0; - int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0; + int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, + local_time_off_min __attribute__((unused)) = 0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -110,12 +111,14 @@ int ASHTECH::handle_message(int len) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += usecs; + } else { _gps_position->time_utc_usec = 0; } @@ -266,7 +269,8 @@ int ASHTECH::handle_message(int len) double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; - double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; + double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, + tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } @@ -281,7 +285,9 @@ int ASHTECH::handle_message(int len) * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. */ lat = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -289,7 +295,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -297,7 +305,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -349,7 +359,8 @@ int ASHTECH::handle_message(int len) _gps_position->vel_n_m_s = velocity_north; /** GPS ground speed in m/s */ _gps_position->vel_e_m_s = velocity_east; /** GPS ground speed in m/s */ _gps_position->vel_d_m_s = static_cast(-vertic_vel); /** GPS ground speed in m/s */ - _gps_position->cog_rad = track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->cog_rad = + track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ _gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1f; _gps_position->timestamp_velocity = hrt_absolute_time(); @@ -381,7 +392,8 @@ int ASHTECH::handle_message(int len) 9 The checksum data, always begins with * */ double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; + double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, + deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -400,7 +412,7 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } _gps_position->eph = sqrtf(static_cast(lat_err) * static_cast(lat_err) - + static_cast(lon_err) * static_cast(lon_err)); + + static_cast(lon_err) * static_cast(lon_err)); _gps_position->epv = static_cast(alt_err); _gps_position->s_variance_m_s = 0; @@ -571,7 +583,7 @@ int ASHTECH::parse_char(uint8_t b) int iRet = 0; switch (_decode_state) { - /* First, look for sync1 */ + /* First, look for sync1 */ case NME_DECODE_UNINIT: if (b == '$') { _decode_state = NME_DECODE_GOT_SYNC1; @@ -636,13 +648,13 @@ void ASHTECH::decode_init(void) */ const char comm[] = "$PASHS,POP,20\r\n"\ - "$PASHS,NME,ZDA,B,ON,3\r\n"\ - "$PASHS,NME,GGA,B,OFF\r\n"\ - "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.05\r\n"\ - "$PASHS,NME,GSV,B,ON,3\r\n"\ - "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,9\r\n"; + "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,9\r\n"; int ASHTECH::configure(unsigned &baudrate) { diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 35ee8658a0..05ff3e5b91 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -211,8 +211,9 @@ GPS::~GPS() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { task_delete(_task); + } g_dev = nullptr; @@ -224,12 +225,13 @@ GPS::init() int ret = ERROR; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -305,9 +307,10 @@ GPS::task_main() _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + _report_gps_pos.vel_ned_valid = true; /* no time and satellite information simulated */ @@ -392,14 +395,16 @@ GPS::task_main() } int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { - // lock(); + // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { if (helper_ret & 1) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } + if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); @@ -508,7 +513,7 @@ void GPS::print_info() { //GPS Mode - if(_fake_gps) { + if (_fake_gps) { warnx("protocol: SIMULATED"); } @@ -522,27 +527,27 @@ GPS::print_info() warnx("protocol: MTK"); break; - case GPS_DRIVER_MODE_ASHTECH: - warnx("protocol: ASHTECH"); - break; + case GPS_DRIVER_MODE_ASHTECH: + warnx("protocol: ASHTECH"); + break; default: break; - } + } } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, - (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); @@ -575,17 +580,20 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new GPS(uart_path, fake_gps, enable_sat_info); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(GPS0_DEVICE_PATH, O_RDONLY); @@ -639,11 +647,13 @@ reset() { int fd = open(GPS0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "reset failed"); + } exit(0); } @@ -654,8 +664,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "not running"); + } g_dev->print_info(); @@ -690,39 +701,45 @@ gps_main(int argc, char *argv[]) /* Detect fake gps option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-f")) + if (!strcmp(argv[i], "-f")) { fake_gps = true; + } } /* Detect sat info option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-s")) + if (!strcmp(argv[i], "-s")) { enable_sat_info = true; + } } gps::start(device_name, fake_gps, enable_sat_info); } - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { gps::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { gps::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { gps::reset(); + } /* * Print driver status. */ - if (!strcmp(argv[1], "status")) + if (!strcmp(argv[1], "status")) { gps::info(); + } out: errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 371f17c1a9..25991b090a 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -66,8 +66,9 @@ int MTK::configure(unsigned &baudrate) { /* set baudrate first */ - if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) { return -1; + } baudrate = MTK_BAUDRATE; @@ -207,8 +208,9 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) } else if (_decode_state == MTK_DECODE_GOT_CK_B) { // Add to checksum - if (_rx_count < 33) + if (_rx_count < 33) { add_byte_to_checksum(b); + } // Fill packet buffer ((uint8_t *)(&packet))[_rx_count] = b; @@ -288,12 +290,14 @@ MTK::handle_message(gps_mtk_packet_t &packet) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; + } else { _gps_position->time_utc_usec = 0; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index a7247a262d..643b339be3 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -199,6 +199,7 @@ UBX::configure(unsigned &baudrate) if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } + #endif /* configure message rates */ @@ -207,41 +208,50 @@ UBX::configure(unsigned &baudrate) /* try to set rate for NAV-PVT */ /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ configure_message_rate(UBX_MSG_NAV_PVT, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { _use_nav_pvt = false; + } else { _use_nav_pvt = true; } + UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); if (!_use_nav_pvt) { configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } } configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_MON_HW, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -269,9 +279,11 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) if (_ack_state == UBX_ACK_GOT_ACK) { ret = 0; // ACK received ok + } else if (report) { if (_ack_state == UBX_ACK_GOT_NAK) { UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); + } else { UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } @@ -359,6 +371,7 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("\nA"); _decode_state = UBX_DECODE_SYNC2; } + break; /* Expecting Sync2 */ @@ -370,6 +383,7 @@ UBX::parse_char(const uint8_t b) } else { // Sync1 not followed by Sync2: reset parser decode_init(); } + break; /* Expecting Class */ @@ -401,38 +415,48 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("F"); add_byte_to_checksum(b); _rx_payload_length |= b << 8; // calculate payload size + if (payload_rx_init() != 0) { // start payload reception // payload will not be handled, discard message decode_init(); + } else { _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1; } + break; /* Expecting payload */ case UBX_DECODE_PAYLOAD: UBX_TRACE_PARSER("."); add_byte_to_checksum(b); + switch (_rx_msg) { case UBX_MSG_NAV_SVINFO: ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte break; + case UBX_MSG_MON_VER: ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte break; + default: ret = payload_rx_add(b); // add a payload byte break; } + if (ret < 0) { // payload not handled, discard message decode_init(); + } else if (ret > 0) { // payload complete, expecting checksum _decode_state = UBX_DECODE_CHKSUM1; + } else { // expecting more payload, stay in state UBX_DECODE_PAYLOAD } + ret = 0; break; @@ -441,18 +465,22 @@ UBX::parse_char(const uint8_t b) if (_rx_ck_a != b) { UBX_WARN("ubx checksum err"); decode_init(); + } else { _decode_state = UBX_DECODE_CHKSUM2; } + break; /* Expecting second checksum byte */ case UBX_DECODE_CHKSUM2: if (_rx_ck_b != b) { UBX_WARN("ubx checksum err"); + } else { ret = payload_rx_done(); // finish payload processing } + decode_init(); break; @@ -475,83 +503,116 @@ UBX::payload_rx_init() switch (_rx_msg) { case UBX_MSG_NAV_PVT: - if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ - && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (!_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + } + break; case UBX_MSG_NAV_POSLLH: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SOL: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_TIMEUTC: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SVINFO: - if (_satellite_info == nullptr) - _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else - memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + if (_satellite_info == nullptr) { + _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else { + memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + } + break; case UBX_MSG_NAV_VELNED: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_MON_VER: break; // unconditionally handle this message case UBX_MSG_MON_HW: - if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ - && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */ + if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ + && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + } + break; case UBX_MSG_ACK_ACK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; case UBX_MSG_ACK_NAK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; default: @@ -624,32 +685,39 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Part 1 complete: decode Part 1 buffer _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES); - UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); + UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, + (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); } - if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) { + + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof( + ubx_payload_rx_nav_svinfo_part2_t)) { // Still room in _satellite_info: fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01); _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno); _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n", - (unsigned)sat_index + 1, - (unsigned)_satellite_info->used[sat_index], - (unsigned)_satellite_info->snr[sat_index], - (unsigned)_satellite_info->elevation[sat_index], - (unsigned)_satellite_info->azimuth[sat_index], - (unsigned)_satellite_info->svid[sat_index] - ); + (unsigned)sat_index + 1, + (unsigned)_satellite_info->used[sat_index], + (unsigned)_satellite_info->snr[sat_index], + (unsigned)_satellite_info->elevation[sat_index], + (unsigned)_satellite_info->azimuth[sat_index], + (unsigned)_satellite_info->svid[sat_index] + ); } } } @@ -672,6 +740,7 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings @@ -681,9 +750,12 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); } + // fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof( + ubx_payload_rx_mon_ver_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); @@ -717,13 +789,11 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-PVT\n"); //Check if position fix flag is good - if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) - { + if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; _gps_position->vel_ned_valid = true; - } - else - { + + } else { _gps_position->fix_type = 0; _gps_position->vel_ned_valid = false; } @@ -748,10 +818,9 @@ UBX::payload_rx_done(void) _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; //Check if time and date fix flags are good - if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) - { + if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { /* convert to unix timestamp */ struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; @@ -770,12 +839,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -827,8 +898,7 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); - if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) - { + if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; @@ -849,12 +919,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -922,6 +994,7 @@ UBX::payload_rx_done(void) ret = 0; // don't handle message break; } + break; case UBX_MSG_ACK_ACK: @@ -999,39 +1072,44 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len header.length = length; // Calculate checksum - calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - if (payload != nullptr) + calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + + if (payload != nullptr) { calc_checksum(payload, length, &checksum); + } // Send message write(_fd, (const void *)&header, sizeof(header)); - if (payload != nullptr) + + if (payload != nullptr) { write(_fd, (const void *)payload, length); + } + write(_fd, (const void *)&checksum, sizeof(checksum)); } uint32_t UBX::fnv1_32_str(uint8_t *str, uint32_t hval) { - uint8_t *s = str; + uint8_t *s = str; - /* - * FNV-1 hash each octet in the buffer - */ - while (*s) { + /* + * FNV-1 hash each octet in the buffer + */ + while (*s) { - /* multiply by the 32 bit FNV magic prime mod 2^32 */ + /* multiply by the 32 bit FNV magic prime mod 2^32 */ #if defined(NO_FNV_GCC_OPTIMIZATION) - hval *= FNV1_32_PRIME; + hval *= FNV1_32_PRIME; #else - hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24); + hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24); #endif - /* xor the bottom with the current octet */ - hval ^= (uint32_t)*s++; - } + /* xor the bottom with the current octet */ + hval ^= (uint32_t) * s++; + } - /* return our new hash value */ - return hval; + /* return our new hash value */ + return hval; } From b5f0d3e937888b08292f8b9752eaad9a9c54b46b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:16:25 +0200 Subject: [PATCH 039/155] HMC5883: Fixed code style --- src/drivers/hmc5883/hmc5883.cpp | 296 ++++++++++++++++++---------- src/drivers/hmc5883/hmc5883_i2c.cpp | 16 +- src/drivers/hmc5883/hmc5883_spi.cpp | 22 +-- 3 files changed, 209 insertions(+), 125 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 77f26e0ca4..cdea2a4e02 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -321,25 +321,25 @@ private: * * @return 0 if calibration is ok, 1 else */ - int check_calibration(); + int check_calibration(); - /** - * Check the current scale calibration - * - * @return 0 if scale calibration is ok, 1 else - */ - int check_scale(); + /** + * Check the current scale calibration + * + * @return 0 if scale calibration is ok, 1 else + */ + int check_scale(); - /** - * Check the current offset calibration - * - * @return 0 if offset calibration is ok, 1 else - */ - int check_offset(); + /** + * Check the current offset calibration + * + * @return 0 if offset calibration is ok, 1 else + */ + int check_offset(); /* this class has pointer data members, do not allow copying it */ - HMC5883(const HMC5883&); - HMC5883 operator=(const HMC5883&); + HMC5883(const HMC5883 &); + HMC5883 operator=(const HMC5883 &); }; /* @@ -397,11 +397,13 @@ HMC5883::~HMC5883() /* make sure we are truly inactive */ stop(); - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_instance); + } // free perf counters perf_free(_sample_perf); @@ -417,6 +419,7 @@ HMC5883::init() int ret = ERROR; ret = CDev::init(); + if (ret != OK) { DEVICE_DEBUG("CDev init failed"); goto out; @@ -424,8 +427,10 @@ HMC5883::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_reports == nullptr) + + if (_reports == nullptr) { goto out; + } /* reset the device configuration */ reset(); @@ -489,14 +494,16 @@ int HMC5883::set_range(unsigned range) */ ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } return !(range_bits_in == (_range_bits << 5)); } @@ -512,15 +519,19 @@ void HMC5883::check_range(void) uint8_t range_bits_in = 0; ret = read_reg(ADDR_CONF_B, range_bits_in); + if (OK != ret) { perf_count(_comms_errors); return; } - if (range_bits_in != (_range_bits<<5)) { + + if (range_bits_in != (_range_bits << 5)) { perf_count(_range_errors); ret = write_reg(ADDR_CONF_B, (_range_bits << 5)); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } } } @@ -535,15 +546,19 @@ void HMC5883::check_conf(void) uint8_t conf_reg_in = 0; ret = read_reg(ADDR_CONF_A, conf_reg_in); + if (OK != ret) { perf_count(_comms_errors); return; } + if (conf_reg_in != _conf_reg) { perf_count(_conf_errors); ret = write_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } } } @@ -555,8 +570,9 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -611,77 +627,84 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ - case SENSOR_POLLRATE_MANUAL: - stop(); - _measure_ticks = 0; - return OK; + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; /* external signalling (DRDY) not supported */ - case SENSOR_POLLRATE_EXTERNAL: + case SENSOR_POLLRATE_EXTERNAL: /* zero would be bad */ - case 0: - return -EINVAL; + case 0: + return -EINVAL; /* set default/max polling rate */ - case SENSOR_POLLRATE_MAX: - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL); - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; - } + return OK; + } /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); - /* convert hz to tick interval via microseconds */ - unsigned ticks = USEC2TICK(1000000 / arg); + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); - /* check against maximum rate */ - if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) - return -EINVAL; + /* check against maximum rate */ + if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) { + return -EINVAL; + } - /* update interval for next measurement */ - _measure_ticks = ticks; + /* update interval for next measurement */ + _measure_ticks = ticks; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } - return 1000000/TICK2USEC(_measure_ticks); + return 1000000 / TICK2USEC(_measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } irqstate_t flags = irqsave(); + if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } + irqrestore(flags); return OK; @@ -699,7 +722,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCGSAMPLERATE: /* same as pollrate because device is in single measurement mode*/ - return 1000000/TICK2USEC(_measure_ticks); + return 1000000 / TICK2USEC(_measure_ticks); case MAGIOCSRANGE: return set_range(arg); @@ -815,8 +838,9 @@ HMC5883::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { DEVICE_DEBUG("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -839,8 +863,9 @@ HMC5883::measure() */ ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } return ret; } @@ -872,7 +897,7 @@ HMC5883::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); - new_report.error_count = perf_event_count(_comms_errors); + new_report.error_count = perf_event_count(_comms_errors); /* * @note We could read the status register here, which could tell us that @@ -908,6 +933,7 @@ HMC5883::collect() /* get measurements from the device */ new_report.temperature = 0; + if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) { /* if temperature compensation is enabled read the @@ -923,13 +949,16 @@ HMC5883::collect() ret = _interface->read(ADDR_TEMP_OUT_MSB, raw_temperature, sizeof(raw_temperature)); + if (ret == OK) { int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) + - raw_temperature[1]; - new_report.temperature = 25 + (temp16 / (16*8.0f)); + raw_temperature[1]; + new_report.temperature = 25 + (temp16 / (16 * 8.0f)); _temperature_error_count = 0; + } else { _temperature_error_count++; + if (_temperature_error_count == 10) { /* it probably really is an old HMC5883, @@ -940,6 +969,7 @@ HMC5883::collect() set_temperature_compensation(0); } } + } else { new_report.temperature = _last_report.temperature; } @@ -961,17 +991,18 @@ HMC5883::collect() // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy; sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + if (sensor_is_onboard) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; report.x = -report.x; - } + } - /* the standard external mag by 3DR has x pointing to the + /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x * and y and invert y */ - xraw_f = -report.y; + xraw_f = -report.y; yraw_f = report.x; zraw_f = report.z; @@ -989,12 +1020,14 @@ HMC5883::collect() if (_mag_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); + } else { _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, - &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); - if (_mag_topic == nullptr) + if (_mag_topic == nullptr) { DEVICE_DEBUG("ADVERT FAIL"); + } } } @@ -1016,9 +1049,11 @@ HMC5883::collect() vehicles have it is worth checking for. */ check_counter = perf_event_count(_sample_perf) % 256; + if (check_counter == 0) { check_range(); } + if (check_counter == 128) { check_conf(); } @@ -1075,7 +1110,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) } /* Set to 2.5 Gauss. We ask for 3 to get the right part of - * the chained if statement above. */ + * the chained if statement above. */ if (OK != ioctl(filp, MAGIOCSRANGE, 3)) { warnx("FAILED: MAGIOCSRANGE 3.3 Ga"); ret = 1; @@ -1146,9 +1181,11 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) ret = -EIO; goto out; } + float cal[3] = {fabsf(expected_cal[0] / report.x), fabsf(expected_cal[1] / report.y), - fabsf(expected_cal[2] / report.z)}; + fabsf(expected_cal[2] / report.z) + }; if (cal[0] > 0.7f && cal[0] < 1.35f && cal[1] > 0.7f && cal[1] < 1.35f && @@ -1211,10 +1248,11 @@ int HMC5883::check_scale() bool scale_valid; if ((-FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && - (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { + (-FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < FLT_EPSILON + 1.0f) && + (-FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < FLT_EPSILON + 1.0f)) { /* scale is one */ scale_valid = false; + } else { scale_valid = true; } @@ -1228,10 +1266,11 @@ int HMC5883::check_offset() bool offset_valid; if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && - (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { + (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { /* offset is zero */ offset_valid = false; + } else { offset_valid = true; } @@ -1247,7 +1286,7 @@ int HMC5883::check_calibration() if (_calibrated != (offset_valid && scale_valid)) { warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", - (offset_valid) ? "" : "offset invalid"); + (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); } @@ -1261,10 +1300,12 @@ int HMC5883::set_excitement(unsigned enable) /* arm the excitement strap */ ret = read_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } _conf_reg &= ~0x03; // reset previous excitement mode + if (((int)enable) < 0) { _conf_reg |= 0x01; @@ -1273,12 +1314,13 @@ int HMC5883::set_excitement(unsigned enable) } - // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); + // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); ret = write_reg(ADDR_CONF_A, _conf_reg); - if (OK != ret) + if (OK != ret) { perf_count(_comms_errors); + } uint8_t conf_reg_ret = 0; read_reg(ADDR_CONF_A, conf_reg_ret); @@ -1317,11 +1359,12 @@ int HMC5883::set_temperature_compensation(unsigned enable) if (OK != ret) { perf_count(_comms_errors); - return -EIO; - } + return -EIO; + } if (enable != 0) { _conf_reg |= HMC5983_TEMP_SENSOR_ENABLE; + } else { _conf_reg &= ~HMC5983_TEMP_SENSOR_ENABLE; } @@ -1334,6 +1377,7 @@ int HMC5883::set_temperature_compensation(unsigned enable) } uint8_t conf_reg_ret = 0; + if (read_reg(ADDR_CONF_A, conf_reg_ret) != OK) { perf_count(_comms_errors); return -EIO; @@ -1383,7 +1427,7 @@ HMC5883::print_info() printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, - (double)(1.0f/_range_scale), (double)_range_ga); + (double)(1.0f / _range_scale), (double)_range_ga); printf("temperature %.2f\n", (double)_last_report.temperature); _reports->print_info("report queue"); } @@ -1436,16 +1480,20 @@ void usage(); bool start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) { - if (bus.dev != nullptr) - errx(1,"bus option already started"); + if (bus.dev != nullptr) { + errx(1, "bus option already started"); + } device::Device *interface = bus.interface_constructor(bus.busnum); + if (interface->init() != OK) { delete interface; warnx("no device on bus %u", (unsigned)bus.busid); return false; } + bus.dev = new HMC5883(interface, bus.devpath, rotation); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; @@ -1453,14 +1501,16 @@ start_bus(struct hmc5883_bus_option &bus, enum Rotation rotation) } int fd = open(bus.devpath, O_RDONLY); + if (fd < 0) { return false; } if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { close(fd); - errx(1,"Failed to setup poll rate"); + errx(1, "Failed to setup poll rate"); } + close(fd); return true; @@ -1483,10 +1533,12 @@ start(enum HMC5883_BUS busid, enum Rotation rotation) // this device is already started continue; } + if (busid != HMC5883_BUS_ALL && bus_options[i].busid != busid) { // not the one that is asked for continue; } + started |= start_bus(bus_options[i], rotation); } @@ -1506,6 +1558,7 @@ struct hmc5883_bus_option &find_bus(enum HMC5883_BUS busid) return bus_options[i]; } } + errx(1, "bus %u not started", (unsigned)busid); } @@ -1526,31 +1579,37 @@ test(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'hmc5883 start')", path); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); /* check if mag is onboard or external */ - if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); + } + warnx("device active: %s", ret ? "external" : "onboard"); /* set the queue depth to 5 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) + if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) { errx(1, "failed to set queue depth"); + } /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -1561,14 +1620,16 @@ test(enum HMC5883_BUS busid) fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); @@ -1628,8 +1689,9 @@ int calibrate(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", path); + } if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) { warnx("failed to enable sensor calibration mode"); @@ -1651,14 +1713,17 @@ reset(enum HMC5883_BUS busid) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -1675,11 +1740,13 @@ temp_enable(enum HMC5883_BUS busid, bool enable) int fd = open(path, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) + if (ioctl(fd, MAGIOCSTEMPCOMP, (unsigned)enable) < 0) { err(1, "set temperature compensation failed"); + } close(fd); return 0; @@ -1719,7 +1786,7 @@ hmc5883_main(int argc, char *argv[]) int ch; enum HMC5883_BUS busid = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; - bool calibrate = false; + bool calibrate = false; bool temp_compensation = false; while ((ch = getopt(argc, argv, "XISR:CT")) != EOF) { @@ -1728,22 +1795,28 @@ hmc5883_main(int argc, char *argv[]) rotation = (enum Rotation)atoi(optarg); break; #if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) + case 'I': busid = HMC5883_BUS_I2C_INTERNAL; break; #endif + case 'X': busid = HMC5883_BUS_I2C_EXTERNAL; break; + case 'S': busid = HMC5883_BUS_SPI; break; + case 'C': calibrate = true; break; + case 'T': temp_compensation = true; break; + default: hmc5883::usage(); exit(0); @@ -1757,42 +1830,51 @@ hmc5883_main(int argc, char *argv[]) */ if (!strcmp(verb, "start")) { hmc5883::start(busid, rotation); + if (calibrate && hmc5883::calibrate(busid) != 0) { errx(1, "calibration failed"); } + if (temp_compensation) { // we consider failing to setup temperature // compensation as non-fatal hmc5883::temp_enable(busid, true); } + exit(0); } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { hmc5883::test(busid); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { hmc5883::reset(busid); + } /* * enable/disable temperature compensation */ - if (!strcmp(verb, "tempoff")) + if (!strcmp(verb, "tempoff")) { hmc5883::temp_enable(busid, false); - if (!strcmp(verb, "tempon")) + } + + if (!strcmp(verb, "tempon")) { hmc5883::temp_enable(busid, true); + } /* * Print driver information. */ - if (!strcmp(verb, "info") || !strcmp(verb, "status")) + if (!strcmp(verb, "info") || !strcmp(verb, "status")) { hmc5883::info(busid); + } /* * Autocalibrate the scaling diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 6d5d36864b..d29ffbe0b4 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file HMC5883_I2C.cpp - * - * I2C interface for HMC5883 / HMC 5983 - */ +/** + * @file HMC5883_I2C.cpp + * + * I2C interface for HMC5883 / HMC 5983 + */ /* XXX trim includes */ #include @@ -72,7 +72,7 @@ public: virtual ~HMC5883_I2C(); virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); + virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); @@ -118,11 +118,13 @@ HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 if (_bus == PX4_I2C_BUS_EXPANSION) { return 1; + } else { return 0; } + #else - return 1; + return 1; #endif case DEVIOCGDEVICEID: diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp index 2aae792f7c..2710f35802 100644 --- a/src/drivers/hmc5883/hmc5883_spi.cpp +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file HMC5883_SPI.cpp - * - * SPI interface for HMC5983 - */ +/** + * @file HMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ /* XXX trim includes */ #include @@ -77,7 +77,7 @@ public: virtual ~HMC5883_SPI(); virtual int init(); - virtual int read(unsigned address, void *data, unsigned count); + virtual int read(unsigned address, void *data, unsigned count); virtual int write(unsigned address, void *data, unsigned count); virtual int ioctl(unsigned operation, unsigned &arg); @@ -91,7 +91,7 @@ HMC5883_SPI_interface(int bus) } HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : - SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) + SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; } @@ -106,6 +106,7 @@ HMC5883_SPI::init() int ret; ret = SPI::init(); + if (ret != OK) { DEVICE_DEBUG("SPI init failed"); return -EIO; @@ -148,10 +149,9 @@ HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) case DEVIOCGDEVICEID: return CDev::ioctl(nullptr, operation, arg); - default: - { - ret = -EINVAL; - } + default: { + ret = -EINVAL; + } } return ret; From 0642624416e1feccd1deba402084be578954f0bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:16:35 +0200 Subject: [PATCH 040/155] gimbal: Fixed code style --- src/drivers/gimbal/gimbal.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/drivers/gimbal/gimbal.cpp b/src/drivers/gimbal/gimbal.cpp index 249fc22dac..40b86d1421 100644 --- a/src/drivers/gimbal/gimbal.cpp +++ b/src/drivers/gimbal/gimbal.cpp @@ -331,7 +331,7 @@ Gimbal::cycle() orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd); if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL - || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { + || cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) { _control_cmd = cmd; _control_cmd_set = true; @@ -358,20 +358,21 @@ Gimbal::cycle() if (_control_cmd_set) { unsigned mountMode = _control_cmd.param7; - DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2); + DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, + (double)_control_cmd.param1, (double)_control_cmd.param2); if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { /* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */ roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1; pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2; yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3; - + updated = true; } if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT && - mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { + mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) { float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4}; math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler(); From 6bb9c8ef07f9e9cb91d68655a2fdce6076eb21ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:16:47 +0200 Subject: [PATCH 041/155] IR lock: Fixed code style --- src/drivers/irlock/irlock.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/irlock/irlock.cpp b/src/drivers/irlock/irlock.cpp index 19b0de6d5f..8be2f8429b 100644 --- a/src/drivers/irlock/irlock.cpp +++ b/src/drivers/irlock/irlock.cpp @@ -388,8 +388,8 @@ int IRLOCK::read_device_block(struct irlock_s *block) /** convert to angles **/ block->target_num = target_num; - block->angle_x = (((float)(pixel_x-IRLOCK_CENTER_X))/IRLOCK_PIXELS_PER_RADIAN_X); - block->angle_y = (((float)(pixel_y-IRLOCK_CENTER_Y))/IRLOCK_PIXELS_PER_RADIAN_Y); + block->angle_x = (((float)(pixel_x - IRLOCK_CENTER_X)) / IRLOCK_PIXELS_PER_RADIAN_X); + block->angle_y = (((float)(pixel_y - IRLOCK_CENTER_Y)) / IRLOCK_PIXELS_PER_RADIAN_Y); block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X; block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y; From 9e34124210f4e7b2875153dd66352dd628d6b537 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:17:00 +0200 Subject: [PATCH 042/155] mkblctrl: Fixed code style --- src/drivers/mkblctrl/mkblctrl.cpp | 121 ++++++++++++++++++------------ 1 file changed, 72 insertions(+), 49 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index de37f0d089..fd0ac59551 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -180,9 +180,9 @@ private: void task_main(); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(file *filp, int cmd, unsigned long arg); int mk_servo_arm(bool status); @@ -191,7 +191,7 @@ private: int mk_servo_locate(); short scaling(float val, float inMin, float inMax, float outMin, float outMax); void play_beep(int count); - + }; @@ -266,8 +266,9 @@ MK::~MK() } /* clean up the alternate device node */ - if (_primary_pwm_device) + if (_primary_pwm_device) { unregister_driver(_device); + } g_mk = nullptr; } @@ -276,7 +277,7 @@ int MK::init(unsigned motors) { _param_indicate_esc = param_find("MKBLCTRL_TEST"); - + _num_outputs = motors; debugCounter = 0; int ret; @@ -303,11 +304,11 @@ MK::init(unsigned motors) /* start the IO interface task */ _task = px4_task_spawn_cmd("mkblctrl", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 20, - 1500, - (main_t)&MK::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 20, + 1500, + (main_t)&MK::task_main_trampoline, + nullptr); if (_task < 0) { @@ -448,7 +449,7 @@ void MK::play_beep(int count) { int buzzer = ::open(TONEALARM0_DEVICE_PATH, O_WRONLY); - + for (int i = 0; i < count; i++) { ::ioctl(buzzer, TONE_SET_ALARM, TONE_SINGLE_BEEP_TUNE); usleep(300000); @@ -467,7 +468,7 @@ MK::task_main() */ _t_actuators = orb_subscribe(ORB_ID(actuator_controls_0)); orb_set_interval(_t_actuators, int(1000 / _update_rate)); /* set the topic update rate (200Hz)*/ - + /* * Subscribe to actuator_armed topic. */ @@ -481,7 +482,7 @@ MK::task_main() memset(&outputs, 0, sizeof(outputs)); int dummy; _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), - &outputs, &dummy, ORB_PRIO_HIGH); + &outputs, &dummy, ORB_PRIO_HIGH); /* * advertise the blctrl status. @@ -504,9 +505,11 @@ MK::task_main() /* loop until killed */ while (!_task_should_exit) { - param_get(_param_indicate_esc ,¶m_mkblctrl_test); + param_get(_param_indicate_esc , ¶m_mkblctrl_test); + if (param_mkblctrl_test > 0) { _indicate_esc = true; + } else { _indicate_esc = false; } @@ -575,7 +578,8 @@ MK::task_main() /* output to BLCtrl's */ if (_motortest != true && _indicate_esc != true) { Motor[i].SetPoint_PX4 = outputs.output[i]; - mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047)); // scale the output to 0 - 2047 and sent to output routine + mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, + 2047)); // scale the output to 0 - 2047 and sent to output routine } } } @@ -615,6 +619,7 @@ MK::task_main() if (Motor[i].Version == 1) { // BLCtrl 2.0 (11Bit) esc.esc[i].esc_setpoint_raw = (uint16_t)(Motor[i].SetPoint << 3) | Motor[i].SetPointLowerBits; + } else { // BLCtrl < 2.0 (8Bit) esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint; @@ -628,7 +633,7 @@ MK::task_main() if (_motortest == true) { mk_servo_test(i); } - + // if esc locate is requested if (_indicate_esc == true) { mk_servo_locate(); @@ -719,7 +724,8 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) fprintf(stderr, "[mkblctrl] MotorsFound: %i\n", foundMotorCount); for (unsigned i = 0; i < foundMotorCount; i++) { - fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); + fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, + Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature); } @@ -776,14 +782,14 @@ MK::mk_servo_set(unsigned int chan, short val) Motor[chan].Temperature = 255;; } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } Motor[chan].RoundCount = 0; } else { if (OK != transfer(&msg[0], 1, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } } @@ -806,14 +812,14 @@ MK::mk_servo_set(unsigned int chan, short val) Motor[chan].Temperature = result[2]; } else { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } Motor[chan].RoundCount = 0; } else { if (OK != transfer(&msg[0], bytesToSendBL2, nullptr, 0)) { - if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) Motor[chan].State++; // error + if ((Motor[chan].State & MOTOR_STATE_ERROR_MASK) < MOTOR_STATE_ERROR_MASK) { Motor[chan].State++; } // error } } @@ -830,7 +836,8 @@ MK::mk_servo_set(unsigned int chan, short val) for (unsigned int i = 0; i < _num_outputs; i++) { if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { - fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); + fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, + Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } } @@ -924,12 +931,12 @@ MK::mk_servo_locate() set_address(BLCTRL_BASE_ADDR + (chan + addrTranslator[chan])); chan++; - + if (chan <= _num_outputs) { fprintf(stderr, "[mkblctrl] ESC Locate - #%i:\tgreen\n", chan); play_beep(chan); } - + if (chan > _num_outputs) { chan = 0; } @@ -962,8 +969,9 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ - if (ret == -ENOTTY) + if (ret == -ENOTTY) { ret = CDev::ioctl(filp, cmd, arg); + } return ret; } @@ -1064,8 +1072,9 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } if (_mixers == nullptr) { ret = -ENOMEM; @@ -1086,30 +1095,36 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg) } case PWM_SERVO_SET_MIN_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - set_rc_min_value((unsigned)pwm->values[0]); - ret = OK; - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + set_rc_min_value((unsigned)pwm->values[0]); + ret = OK; + break; + } case PWM_SERVO_GET_MIN_PWM: ret = OK; break; case PWM_SERVO_SET_MAX_PWM: { - struct pwm_output_values* pwm = (struct pwm_output_values*)arg; - if (pwm->channel_count > _max_actuators) - /* fail with error */ - return -E2BIG; + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; - set_rc_max_value((unsigned)pwm->values[0]); - ret = OK; - break; - } + if (pwm->channel_count > _max_actuators) + /* fail with error */ + { + return -E2BIG; + } + + set_rc_max_value((unsigned)pwm->values[0]); + ret = OK; + break; + } case PWM_SERVO_GET_MAX_PWM: ret = OK; @@ -1168,7 +1183,8 @@ enum FrameType { int -mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, unsigned rcmax) +mk_new_mode(int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks, unsigned rcmin, + unsigned rcmax) { int shouldStop = 0; @@ -1215,8 +1231,9 @@ mk_start(unsigned motors, const char *device_path) // try i2c3 first g_mk = new MK(3, device_path); - if (!g_mk) + if (!g_mk) { return -ENOMEM; + } if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c3...\n"); @@ -1233,8 +1250,9 @@ mk_start(unsigned motors, const char *device_path) // fallback to bus 1 g_mk = new MK(1, device_path); - if (!g_mk) + if (!g_mk) { return -ENOMEM; + } if (OK == g_mk->init(motors)) { warnx("[mkblctrl] scanning i2c1...\n"); @@ -1330,13 +1348,15 @@ mkblctrl_main(int argc, char *argv[]) } /* look for the optional -rc_min parameter */ - if (strcmp(argv[i], "-rc_min") == 0 ) { + if (strcmp(argv[i], "-rc_min") == 0) { if (argc > i + 1) { rc_min_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { errx(1, "bad pwm val (-rc_min)"); return 1; } + } else { errx(1, "missing value (-rc_min)"); return 1; @@ -1344,13 +1364,15 @@ mkblctrl_main(int argc, char *argv[]) } /* look for the optional -rc_max parameter */ - if (strcmp(argv[i], "-rc_max") == 0 ) { + if (strcmp(argv[i], "-rc_max") == 0) { if (argc > i + 1) { rc_max_value = strtoul(argv[i + 1], &ep, 0); + if (*ep != '\0') { errx(1, "bad pwm val (-rc_max)"); return 1; } + } else { errx(1, "missing value (-rc_max)"); return 1; @@ -1365,7 +1387,8 @@ mkblctrl_main(int argc, char *argv[]) fprintf(stderr, " [-mkmode {+/x}] [-b i2c_bus_number] [-d devicename] [--override-security-checks] [-h / --help]\n\n"); fprintf(stderr, "\t -mkmode {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n"); fprintf(stderr, "\t -d {devicepath & name}\t\t Create alternate pwm device.\n"); - fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); + fprintf(stderr, + "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n"); fprintf(stderr, "\t -rcmin {pwn-value}\t\t Set RC_MIN Value.\n"); fprintf(stderr, "\t -rcmax {pwn-value}\t\t Set RC_MAX Value.\n"); fprintf(stderr, "\n"); From 06076189da57879f8679d679496effa279cc21eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:17:15 +0200 Subject: [PATCH 043/155] PWM input: Fixed code style --- src/drivers/pwm_input/pwm_input.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/pwm_input/pwm_input.cpp b/src/drivers/pwm_input/pwm_input.cpp index 06d660d92c..4d73f6204d 100644 --- a/src/drivers/pwm_input/pwm_input.cpp +++ b/src/drivers/pwm_input/pwm_input.cpp @@ -302,6 +302,7 @@ PWMIN::init() CDev::init(); _reports = new ringbuffer::RingBuffer(2, sizeof(struct pwm_input_s)); + if (_reports == nullptr) { return -ENOMEM; } @@ -490,6 +491,7 @@ PWMIN::read(struct file *filp, char *buffer, size_t buflen) buf++; } } + /* if there was no data, warn the caller */ return ret ? ret : -EAGAIN; } From 88a69382d9bd00f78ab0ce9ddf3883d1434e9c01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:17:27 +0200 Subject: [PATCH 044/155] PWM out sim: Fixed code style --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 88 +++++++++++++++---------- 1 file changed, 54 insertions(+), 34 deletions(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index f42d166535..8040cc4d95 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -133,9 +133,9 @@ private: void task_main(); static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + uint8_t control_group, + uint8_t control_index, + float &input); int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); @@ -194,6 +194,7 @@ PWMSim::~PWMSim() _task_should_exit = true; unsigned i = 10; + do { /* wait 50ms - it should wake every 100ms or so worst-case */ usleep(50000); @@ -226,17 +227,18 @@ PWMSim::init() if (ret != OK) { return ret; + } else { _primary_pwm_device = true; } /* start the PWMSim interface task */ _task = px4_task_spawn_cmd("pwm_out_sim", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1000, - (px4_main_t)&PWMSim::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1000, + (px4_main_t)&PWMSim::task_main_trampoline, + nullptr); if (_task < 0) { DEVICE_DEBUG("task start failed: %d", errno); @@ -277,7 +279,7 @@ PWMSim::set_mode(Mode mode) _num_outputs = 4; break; - case MODE_8PWM: + case MODE_8PWM: DEVICE_DEBUG("MODE_8PWM"); /* multi-port as 8 PWM outs */ _update_rate = 50; /* default output rate */ @@ -308,6 +310,7 @@ PWMSim::set_mode(Mode mode) default: return -EINVAL; } + _mode = mode; return OK; } @@ -315,8 +318,9 @@ PWMSim::set_mode(Mode mode) int PWMSim::set_pwm_rate(unsigned rate) { - if ((rate > 500) || (rate < 10)) + if ((rate > 500) || (rate < 10)) { return -EINVAL; + } _update_rate = rate; return OK; @@ -383,8 +387,11 @@ PWMSim::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - if (update_rate_in_ms < 2) + + if (update_rate_in_ms < 2) { update_rate_in_ms = 2; + } + orb_set_interval(_t_actuators, update_rate_in_ms); // up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; @@ -403,7 +410,7 @@ PWMSim::task_main() if (fds[0].revents & POLLIN) { /* get controls - must always do this to avoid spinning */ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); + ORB_ID(actuator_controls_1), _t_actuators, &_controls); /* can we mix? */ if (_armed && _mixers != nullptr) { @@ -415,11 +422,12 @@ PWMSim::task_main() for (unsigned i = 0; i < num_outputs; i++) { /* last resort: catch NaN, INF and out-of-band errors */ if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { + PX4_ISFINITE(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { /* scale for PWM output 900 - 2100us */ outputs.output[i] = 1500 + (600 * outputs.output[i]); + } else { /* * Value is NaN, INF or out of band - set to the minimum value. @@ -460,18 +468,20 @@ PWMSim::task_main() int PWMSim::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) + uint8_t control_group, + uint8_t control_index, + float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; if (_armed) { input = controls->control[control_index]; + } else { /* clamp actuator to zero if not armed */ input = 0.0f; } + return 0; } @@ -481,7 +491,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) int ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ - switch(_mode) { + switch (_mode) { case MODE_2PWM: case MODE_4PWM: case MODE_8PWM: @@ -489,6 +499,7 @@ PWMSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) case MODE_16PWM: ret = PWMSim::pwm_ioctl(filp, cmd, arg); break; + default: ret = -ENOTTY; DEVICE_DEBUG("not in a PWM mode"); @@ -596,7 +607,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_SET(0): case PWM_SERVO_SET(1): if (arg < 2100) { @@ -625,7 +636,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) break; } - /* FALLTHROUGH */ + /* FALLTHROUGH */ case PWM_SERVO_GET(1): case PWM_SERVO_GET(0): { *(servo_position_t *)arg = 1500; @@ -633,12 +644,12 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) } case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): { - // no restrictions on output grouping - unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); + // no restrictions on output grouping + unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0); - *(uint32_t *)arg = (1 << channel); - break; - } + *(uint32_t *)arg = (1 << channel); + break; + } case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: @@ -648,6 +659,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) } else if (_mode == MODE_4PWM) { *(unsigned *)arg = 4; + } else { *(unsigned *)arg = 2; @@ -688,8 +700,9 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) const char *buf = (const char *)arg; unsigned buflen = strnlen(buf, 1024); - if (_mixers == nullptr) + if (_mixers == nullptr) { _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + } if (_mixers == nullptr) { ret = -ENOMEM; @@ -705,6 +718,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) ret = -EINVAL; } } + break; } @@ -752,7 +766,7 @@ hil_new_mode(PortMode new_mode) switch (new_mode) { case PORT_MODE_UNDEFINED: case PORT1_MODE_UNSET: - case PORT2_MODE_UNSET: + case PORT2_MODE_UNSET: /* nothing more to do here */ break; @@ -852,7 +866,8 @@ fake(int argc, char *argv[]) extern "C" __EXPORT int pwm_out_sim_main(int argc, char *argv[]); static void -usage() { +usage() +{ PX4_WARN("pwm_out_sim: unrecognized command, try:"); PX4_WARN(" mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16"); } @@ -868,10 +883,12 @@ pwm_out_sim_main(int argc, char *argv[]) usage(); return -EINVAL; } + verb = argv[1]; if (g_pwm_sim == nullptr) { g_pwm_sim = new PWMSim; + if (g_pwm_sim == nullptr) { return -ENOMEM; } @@ -881,7 +898,7 @@ pwm_out_sim_main(int argc, char *argv[]) * Mode switches. */ - // this was all cut-and-pasted from the FMU driver; it's junk + // this was all cut-and-pasted from the FMU driver; it's junk if (!strcmp(verb, "mode_pwm")) { new_mode = PORT1_FULL_PWM; @@ -905,13 +922,14 @@ pwm_out_sim_main(int argc, char *argv[]) if (new_mode != PORT_MODE_UNDEFINED) { /* yes but it's the same mode */ - if (new_mode == g_port_mode) + if (new_mode == g_port_mode) { return OK; + } /* switch modes */ ret = hil_new_mode(new_mode); - } - else if (!strcmp(verb, "test")) { + + } else if (!strcmp(verb, "test")) { ret = test(); } @@ -924,13 +942,15 @@ pwm_out_sim_main(int argc, char *argv[]) ret = -EINVAL; } - if ( ret == OK && g_pwm_sim->_task == -1 ) { + if (ret == OK && g_pwm_sim->_task == -1) { ret = g_pwm_sim->init(); + if (ret != OK) { warnx("failed to start the pwm_out_sim driver"); delete g_pwm_sim; g_pwm_sim = nullptr; } } + return ret; } From 174464e2cc574a45aacd65d0f0c43adb51d32b0d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:17:43 +0200 Subject: [PATCH 045/155] PX4 Flow: Fixed code style --- src/drivers/px4flow/i2c_frame.h | 52 ++++++++++++++++----------------- src/drivers/px4flow/px4flow.cpp | 35 +++++++++++++++------- 2 files changed, 50 insertions(+), 37 deletions(-) diff --git a/src/drivers/px4flow/i2c_frame.h b/src/drivers/px4flow/i2c_frame.h index 43e10e417c..f73d4a92cc 100644 --- a/src/drivers/px4flow/i2c_frame.h +++ b/src/drivers/px4flow/i2c_frame.h @@ -44,38 +44,36 @@ #define __STDC_FORMAT_MACROS #include -typedef struct i2c_frame -{ - uint16_t frame_count; - int16_t pixel_flow_x_sum; - int16_t pixel_flow_y_sum; - int16_t flow_comp_m_x; - int16_t flow_comp_m_y; - int16_t qual; - int16_t gyro_x_rate; - int16_t gyro_y_rate; - int16_t gyro_z_rate; - uint8_t gyro_range; - uint8_t sonar_timestamp; - int16_t ground_distance; +typedef struct i2c_frame { + uint16_t frame_count; + int16_t pixel_flow_x_sum; + int16_t pixel_flow_y_sum; + int16_t flow_comp_m_x; + int16_t flow_comp_m_y; + int16_t qual; + int16_t gyro_x_rate; + int16_t gyro_y_rate; + int16_t gyro_z_rate; + uint8_t gyro_range; + uint8_t sonar_timestamp; + int16_t ground_distance; } i2c_frame; #define I2C_FRAME_SIZE (sizeof(i2c_frame)) -typedef struct i2c_integral_frame -{ - uint16_t frame_count_since_last_readout; - int16_t pixel_flow_x_integral; - int16_t pixel_flow_y_integral; - int16_t gyro_x_rate_integral; - int16_t gyro_y_rate_integral; - int16_t gyro_z_rate_integral; - uint32_t integration_timespan; - uint32_t sonar_timestamp; - uint16_t ground_distance; - int16_t gyro_temperature; - uint8_t qual; +typedef struct i2c_integral_frame { + uint16_t frame_count_since_last_readout; + int16_t pixel_flow_x_integral; + int16_t pixel_flow_y_integral; + int16_t gyro_x_rate_integral; + int16_t gyro_y_rate_integral; + int16_t gyro_z_rate_integral; + uint32_t integration_timespan; + uint32_t sonar_timestamp; + uint16_t ground_distance; + int16_t gyro_temperature; + uint8_t qual; } i2c_integral_frame; #define I2C_INTEGRAL_FRAME_SIZE (sizeof(i2c_integral_frame)) diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 134ce6a52b..92cac76312 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -246,7 +246,7 @@ PX4FLOW::init() struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + &_orb_class_instance, ORB_PRIO_HIGH); if (_distance_sensor_topic == nullptr) { DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); @@ -503,22 +503,34 @@ PX4FLOW::collect() struct optical_flow_s report; report.timestamp = hrt_absolute_time(); + report.pixel_flow_x_integral = static_cast(f_integral.pixel_flow_x_integral) / 10000.0f;//convert to radians + report.pixel_flow_y_integral = static_cast(f_integral.pixel_flow_y_integral) / 10000.0f;//convert to radians + report.frame_count_since_last_readout = f_integral.frame_count_since_last_readout; + report.ground_distance_m = static_cast(f_integral.ground_distance) / 1000.0f;//convert to meters + report.quality = f_integral.qual; //0:bad ; 255 max quality + report.gyro_x_rate_integral = static_cast(f_integral.gyro_x_rate_integral) / 10000.0f; //convert to radians + report.gyro_y_rate_integral = static_cast(f_integral.gyro_y_rate_integral) / 10000.0f; //convert to radians + report.gyro_z_rate_integral = static_cast(f_integral.gyro_z_rate_integral) / 10000.0f; //convert to radians + report.integration_timespan = f_integral.integration_timespan; //microseconds + report.time_since_last_sonar_update = f_integral.sonar_timestamp;//microseconds + report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius report.sensor_id = 0; /* rotate measurements according to parameter */ float zeroval = 0.0f; + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic == nullptr) { @@ -659,12 +671,13 @@ int start() { int fd; - + /* entry check: */ if (start_in_progress) { warnx("start already in progress"); return 1; } + start_in_progress = true; if (g_dev != nullptr) { @@ -676,22 +689,24 @@ start() warnx("scanning I2C buses for device.."); int retry_nr = 0; + while (1) { const int busses_to_try[] = { PX4_I2C_BUS_EXPANSION, - #ifdef PX4_I2C_BUS_ESC +#ifdef PX4_I2C_BUS_ESC PX4_I2C_BUS_ESC, - #endif +#endif PX4_I2C_BUS_ONBOARD, -1 }; const int *cur_bus = busses_to_try; - while(*cur_bus != -1) { + while (*cur_bus != -1) { /* create the driver */ /* warnx("trying bus %d", *cur_bus); */ g_dev = new PX4FLOW(*cur_bus); + if (g_dev == nullptr) { /* this is a fatal error */ break; @@ -713,7 +728,7 @@ start() /* check whether we found it: */ if (*cur_bus != -1) { - + /* check for failure: */ if (g_dev == nullptr) { break; @@ -740,16 +755,17 @@ start() // warnx("PX4FLOW not found on I2C busses. Retrying in %d ms. Giving up in %d retries.", START_RETRY_TIMEOUT, START_RETRY_COUNT - retry_nr); usleep(START_RETRY_TIMEOUT * 1000); retry_nr++; + } else { break; } } - + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; } - + start_in_progress = false; return 1; } @@ -793,8 +809,7 @@ test() /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) - { + if (sz != sizeof(report)) { warnx("immediate read failed"); } From 7c0407d0ef4d6569be0cc1d806841c7b7356c92f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:17:57 +0200 Subject: [PATCH 046/155] ETS airspeed: Fixed code style --- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 51c009717c..83f83896c6 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -162,7 +162,7 @@ ETSAirspeed::collect() // caller could end up using this value as part of an // average perf_count(_comms_errors); - DEVICE_LOG("zero value from sensor"); + DEVICE_LOG("zero value from sensor"); return -1; } From 789e595df7d1d4dbd685d26afbe3a3629a2c6a75 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:18:11 +0200 Subject: [PATCH 047/155] FrSky Telem: Fixed code style --- src/drivers/frsky_telemetry/frsky_telemetry.c | 34 +++++++++++-------- 1 file changed, 19 insertions(+), 15 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 75d1124a61..6100f5f1d0 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -83,6 +83,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or /* Back up the original UART configuration to restore it after exit */ int termios_state; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); close(uart); @@ -120,9 +121,9 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static void usage() { fprintf(stderr, - "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); exit(1); } @@ -139,6 +140,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) argv += 2; int ch; + while ((ch = getopt(argc, argv, "d:")) != EOF) { switch (ch) { case 'd': @@ -155,8 +157,9 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) struct termios uart_config_original; const int uart = frsky_open_uart(device_name, &uart_config_original); - if (uart < 0) + if (uart < 0) { err(1, "could not open %s", device_name); + } /* Subscribe to topics */ frsky_init(); @@ -165,6 +168,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) /* Main thread loop */ unsigned int iteration = 0; + while (!thread_should_exit) { /* Sleep 200 ms */ @@ -174,14 +178,12 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) frsky_send_frame1(uart); /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) - { + if (iteration % 5 == 0) { frsky_send_frame2(uart); } /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) - { + if (iteration % 25 == 0) { frsky_send_frame3(uart); iteration = 0; @@ -212,16 +214,17 @@ int frsky_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { /* this is not an error */ - if (thread_running) + if (thread_running) { errx(0, "frsky_telemetry already running"); + } thread_should_exit = false; frsky_task = px4_task_spawn_cmd("frsky_telemetry", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2000, - frsky_telemetry_thread_main, - (char * const *)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + frsky_telemetry_thread_main, + (char *const *)argv); while (!thread_running) { usleep(200); @@ -233,8 +236,9 @@ int frsky_telemetry_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { /* this is not an error */ - if (!thread_running) + if (!thread_running) { errx(0, "frsky_telemetry already stopped"); + } thread_should_exit = true; From 2993c5dd12a2faab625abce7b904e082ad5dd1e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:18:24 +0200 Subject: [PATCH 048/155] L3GD20: Fixed code style --- src/drivers/l3gd20/l3gd20.cpp | 315 ++++++++++++++++++++-------------- 1 file changed, 189 insertions(+), 126 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 43a24359eb..266c5283be 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -206,7 +206,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI { public: - L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation); + L3GD20(int bus, const char *path, spi_dev_e device, enum Rotation rotation); virtual ~L3GD20(); virtual int init(); @@ -389,11 +389,11 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /* this class does not allow copying */ - L3GD20(const L3GD20&); - L3GD20 operator=(const L3GD20&); + L3GD20(const L3GD20 &); + L3GD20 operator=(const L3GD20 &); }; /* @@ -401,16 +401,18 @@ private: that ADDR_WHO_AM_I must be first in the list. */ const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, - ADDR_CTRL_REG1, - ADDR_CTRL_REG2, - ADDR_CTRL_REG3, - ADDR_CTRL_REG4, - ADDR_CTRL_REG5, - ADDR_FIFO_CTRL_REG, - ADDR_LOW_ODR }; + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_FIFO_CTRL_REG, + ADDR_LOW_ODR + }; -L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : - SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), +L3GD20::L3GD20(int bus, const char *path, spi_dev_e device, enum Rotation rotation) : + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, + 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, _call_interval(0), _reports(nullptr), @@ -456,11 +458,13 @@ L3GD20::~L3GD20() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -475,14 +479,16 @@ L3GD20::init() int ret = ERROR; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } _class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); @@ -495,7 +501,7 @@ L3GD20::init() _reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { DEVICE_DEBUG("failed to create sensor_gyro publication"); @@ -517,13 +523,15 @@ L3GD20::probe() /* verify that the device is attached and functioning, accept * L3GD20, L3GD20H and L3G4200D */ - if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { + if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; - } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { + + } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; success = true; - } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { + + } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { /* Detect the L3G4200D used on AeroCore */ _is_l3g4200d = true; _orientation = SENSOR_BOARD_ROTATION_DEFAULT; @@ -546,8 +554,9 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_interval > 0) { @@ -588,28 +597,29 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: if (_is_l3g4200d) { return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE); } + return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_call_interval == 0); @@ -618,23 +628,25 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _call_interval = ticks; - _call.period = _call_interval - L3GD20_TIMER_REDUCTION; + _call.period = _call_interval - L3GD20_TIMER_REDUCTION; /* adjust filters */ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -642,25 +654,29 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -676,12 +692,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _current_rate; case GYROIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_interval; - set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); - return OK; - } + return OK; + } case GYROIOCGLOWPASS: return static_cast(_gyro_filter_x.get_cutoff_freq()); @@ -741,7 +757,8 @@ void L3GD20::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; iprint_info("report queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; i zero or larger than 25 dps */ - if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + } + + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) { return 1; + } + + if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) { + return 1; + } return 0; } @@ -1165,36 +1204,42 @@ start(bool external_bus, enum Rotation rotation) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation); #else errx(0, "External SPI not available"); #endif + } else { g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation); } - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: @@ -1222,18 +1267,21 @@ test() /* get the driver */ fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", L3GD20_DEVICE_PATH); + } /* reset to manual polling */ - if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); - if (sz != sizeof(g_report)) + if (sz != sizeof(g_report)) { err(1, "immediate gyro read failed"); + } warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); @@ -1246,10 +1294,11 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); - if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); + } - close(fd_gyro); + close(fd_gyro); /* XXX add poll-rate tests here too */ errx(0, "PASS"); @@ -1263,16 +1312,19 @@ reset() { int fd = open(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "accel pollrate reset failed"); + } - close(fd); + close(fd); exit(0); } @@ -1283,8 +1335,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -1298,8 +1351,9 @@ info() void regdump(void) { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", g_dev); g_dev->print_registers(); @@ -1313,8 +1367,9 @@ regdump(void) void test_error(void) { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", g_dev); g_dev->test_error(); @@ -1346,9 +1401,11 @@ l3gd20_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + default: l3gd20::usage(); exit(0); @@ -1361,38 +1418,44 @@ l3gd20_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { l3gd20::start(external_bus, rotation); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { l3gd20::test(); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { l3gd20::reset(); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { l3gd20::info(); + } /* * Print register information. */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { l3gd20::regdump(); + } /* * trigger an error */ - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { l3gd20::test_error(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } From 7a33697b850c8a0caaa08e3e41b37669da676f93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:18:34 +0200 Subject: [PATCH 049/155] led: Fixed code style --- src/drivers/led/led.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/drivers/led/led.cpp b/src/drivers/led/led.cpp index 831e8f4634..f74253fa37 100644 --- a/src/drivers/led/led.cpp +++ b/src/drivers/led/led.cpp @@ -125,6 +125,7 @@ LED::ioctl(device::file_t *filp, int cmd, unsigned long arg) result = VDev::ioctl(filp, cmd, arg); #endif } + return result; } @@ -138,7 +139,9 @@ drv_led_start(void) { if (gLED == nullptr) { gLED = new LED; - if (gLED != nullptr) + + if (gLED != nullptr) { gLED->init(); + } } } From 19e3da71695ef3119e650568ff8f38d2b95592dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:18:47 +0200 Subject: [PATCH 050/155] LSM303D: Fixed code style --- src/drivers/lsm303d/lsm303d.cpp | 410 +++++++++++++++++++------------- 1 file changed, 249 insertions(+), 161 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4235ecc6f5..1252f57c09 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -236,7 +236,7 @@ class LSM303D_mag; class LSM303D : public device::SPI { public: - LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation); + LSM303D(int bus, const char *path, spi_dev_e device, enum Rotation rotation); virtual ~LSM303D(); virtual int init(); @@ -497,8 +497,8 @@ private: int mag_set_samplerate(unsigned frequency); /* this class cannot be copied */ - LSM303D(const LSM303D&); - LSM303D operator=(const LSM303D&); + LSM303D(const LSM303D &); + LSM303D operator=(const LSM303D &); }; /* @@ -512,7 +512,8 @@ const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADD ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7 }; + ADDR_CTRL_REG7 + }; /** * Helper class implementing the mag driver node. @@ -544,13 +545,14 @@ private: void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ - LSM303D_mag(const LSM303D_mag&); - LSM303D_mag operator=(const LSM303D_mag&); + LSM303D_mag(const LSM303D_mag &); + LSM303D_mag operator=(const LSM303D_mag &); }; -LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : - SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), +LSM303D::LSM303D(int bus, const char *path, spi_dev_e device, enum Rotation rotation) : + SPI("LSM303D", path, bus, device, SPIDEV_MODE3, + 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */), _mag(new LSM303D_mag(this)), _accel_call{}, _mag_call{}, @@ -621,13 +623,17 @@ LSM303D::~LSM303D() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_mag_reports != nullptr) - delete _mag_reports; + } - if (_accel_class_instance != -1) + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } delete _mag; @@ -653,18 +659,21 @@ LSM303D::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + if (_accel_reports == nullptr) { goto out; + } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_mag_reports == nullptr) + if (_mag_reports == nullptr) { goto out; + } reset(); /* do CDev init for the mag device node */ ret = _mag->init(); + if (ret != OK) { warnx("MAG init failed"); goto out; @@ -679,7 +688,7 @@ LSM303D::init() /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { warnx("ADVERT ERR"); @@ -694,7 +703,7 @@ LSM303D::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { warnx("ADVERT ERR"); @@ -762,7 +771,7 @@ LSM303D::probe() if (success) { _checked_values[0] = WHO_I_AM; return OK; - } + } return -EIO; } @@ -775,8 +784,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { @@ -798,8 +808,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) + if (_accel_reports->get(arb)) { ret = sizeof(*arb); + } return ret; } @@ -812,8 +823,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { @@ -837,8 +849,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) _mag->measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) + if (_mag_reports->get(mrb)) { ret = sizeof(*mrb); + } return ret; } @@ -849,7 +862,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -871,56 +884,62 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned ticks = 1000000 / arg; - /* check against maximum sane rate */ - if (ticks < 500) - return -EINVAL; + /* check against maximum sane rate */ + if (ticks < 500) { + return -EINVAL; + } - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _call_accel_interval = ticks; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _call_accel_interval = ticks; - _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; + _accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) + if (_call_accel_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_accel_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_accel_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -936,23 +955,25 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } case ACCELIOCGLOWPASS: return static_cast(_accel_filter_x.get_cutoff_freq()); case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } } - } case ACCELIOCSRANGE: /* arg needs to be in G */ @@ -960,7 +981,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / LSM303D_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -982,7 +1003,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -1012,16 +1033,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ _mag_call.period = _call_mag_interval = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -1029,25 +1052,29 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) + if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_mag_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_mag_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); @@ -1102,24 +1129,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) int LSM303D::accel_self_test() { - if (_accel_read == 0) + if (_accel_read == 0) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -1127,21 +1164,25 @@ LSM303D::accel_self_test() int LSM303D::mag_self_test() { - if (_mag_read == 0) + if (_mag_read == 0) { return 1; + } /** * inspect mag offsets * don't check mag scale because it seems this is calibrated on chip */ - if (fabsf(_mag_scale.x_offset) < 0.000001f) + if (fabsf(_mag_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_mag_scale.y_offset) < 0.000001f) + if (fabsf(_mag_scale.y_offset) < 0.000001f) { return 1; + } - if (fabsf(_mag_scale.z_offset) < 0.000001f) + if (fabsf(_mag_scale.z_offset) < 0.000001f) { return 1; + } return 0; } @@ -1174,7 +1215,8 @@ void LSM303D::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; i 20 && fabsf(z_in_new) > 20) { _constant_accel_count += 1; + } else { _constant_accel_count = 0; } + if (_constant_accel_count > 100) { // we've had 100 constant accel readings with large // values. The sensor is almost certainly dead. We @@ -1712,16 +1763,19 @@ LSM303D::print_info() perf_print_counter(_accel_duplicates); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); } } @@ -1881,17 +1940,19 @@ void start(bool external_bus, enum Rotation rotation, unsigned range) { int fd, fd_mag; + if (g_dev != nullptr) { errx(0, "already started"); } /* create the driver */ - if (external_bus) { - #ifdef PX4_SPI_BUS_EXT + if (external_bus) { +#ifdef PX4_SPI_BUS_EXT g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation); - #else +#else errx(0, "External SPI not available"); - #endif +#endif + } else { g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation); } @@ -1901,8 +1962,9 @@ start(bool external_bus, enum Rotation rotation, unsigned range) goto fail; } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); @@ -1928,8 +1990,8 @@ start(bool external_bus, enum Rotation rotation, unsigned range) } } - close(fd); - close(fd_mag); + close(fd); + close(fd_mag); exit(0); fail: @@ -1958,14 +2020,16 @@ test() /* get the driver */ fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd_accel < 0) + if (fd_accel < 0) { err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL); + } /* do a simple demand read */ sz = read(fd_accel, &accel_report, sizeof(accel_report)); - if (sz != sizeof(accel_report)) + if (sz != sizeof(accel_report)) { err(1, "immediate read failed"); + } warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); @@ -1976,10 +2040,13 @@ test() warnx("accel z: \t%d\traw", (int)accel_report.z_raw); warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); - if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) { warnx("accel antialias filter bandwidth: fail"); - else + + } else { warnx("accel antialias filter bandwidth: %d Hz", ret); + } int fd_mag = -1; struct mag_report m_report; @@ -1987,19 +2054,23 @@ test() /* get the driver */ fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); - if (fd_mag < 0) + if (fd_mag < 0) { err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG); + } /* check if mag is onboard or external */ - if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) { errx(1, "failed to get if mag is onboard or external"); + } + warnx("mag device active: %s", ret ? "external" : "onboard"); /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); - if (sz != sizeof(m_report)) + if (sz != sizeof(m_report)) { err(1, "immediate read failed"); + } warnx("mag x: \t% 9.5f\tga", (double)m_report.x); warnx("mag y: \t% 9.5f\tga", (double)m_report.y); @@ -2014,8 +2085,8 @@ test() err(1, "reset to default polling"); } - close(fd_accel); - close(fd_mag); + close(fd_accel); + close(fd_mag); reset(); errx(0, "PASS"); @@ -2029,28 +2100,33 @@ reset() { int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "accel pollrate reset failed"); + } - close(fd); + close(fd); fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY); if (fd < 0) { warnx("mag could not be opened, external mag might be used"); + } else { /* no need to reset the mag as well, the reset() is the same */ - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "mag pollrate reset failed"); + } } - close(fd); + close(fd); exit(0); } @@ -2061,8 +2137,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -2076,8 +2153,9 @@ info() void regdump() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } printf("regdump @ %p\n", g_dev); g_dev->print_registers(); @@ -2091,8 +2169,9 @@ regdump() void test_error() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running\n"); + } g_dev->test_error(); @@ -2124,12 +2203,15 @@ lsm303d_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + case 'a': accel_range = atoi(optarg); break; + default: lsm303d::usage(); exit(0); @@ -2142,38 +2224,44 @@ lsm303d_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { lsm303d::start(external_bus, rotation, accel_range); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { lsm303d::test(); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { lsm303d::reset(); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { lsm303d::info(); + } /* * dump device registers */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { lsm303d::regdump(); + } /* * trigger an error */ - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { lsm303d::test_error(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } From 0d395e893a78b181265d11fe9494703730d7c2b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:24:03 +0200 Subject: [PATCH 051/155] Aerocore: Fix formatting --- src/drivers/boards/aerocore/aerocore_init.c | 12 +++++++++--- src/drivers/boards/aerocore/aerocore_led.c | 14 ++++++++++---- src/drivers/boards/aerocore/aerocore_spi.c | 4 ++-- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c index 3bdb0d4238..83bc2f4234 100644 --- a/src/drivers/boards/aerocore/aerocore_init.c +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -117,12 +117,12 @@ __END_DECLS static GRAN_HANDLE dma_allocator; -/* - * The DMA heap size constrains the total number of things that can be +/* + * The DMA heap size constrains the total number of things that can be * ready to do DMA at a time. * * For example, FAT DMA depends on one sector-sized buffer per filesystem plus - * one sector-sized buffer per file. + * one sector-sized buffer per file. * * We use a fundamental alignment / granule size of 64B; this is sufficient * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). @@ -137,8 +137,10 @@ dma_alloc_init(void) sizeof(g_dma_heap), 7, /* 128B granule - must be > alignment (XXX bug?) */ 6); /* 64B alignment */ + if (dma_allocator == NULL) { message("[boot] DMA allocator setup FAILED"); + } else { g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); } @@ -262,11 +264,13 @@ __EXPORT int nsh_archinitialize(void) /* Configure Sensors on SPI bus #3 */ spi3 = up_spiinitialize(3); + if (!spi3) { message("[boot] FAILED to initialize SPI port 3\n"); up_ledon(LED_AMBER); return -ENODEV; } + /* Default: 1MHz, 8 bits, Mode 3 */ SPI_SETFREQUENCY(spi3, 10000000); SPI_SETBITS(spi3, 8); @@ -279,11 +283,13 @@ __EXPORT int nsh_archinitialize(void) /* Configure FRAM on SPI bus #4 */ spi4 = up_spiinitialize(4); + if (!spi4) { message("[boot] FAILED to initialize SPI port 4\n"); up_ledon(LED_AMBER); return -ENODEV; } + /* Default: ~10MHz, 8 bits, Mode 3 */ SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000); SPI_SETBITS(spi4, 8); diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index 22b3a2c240..89c6367419 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -103,17 +103,23 @@ __EXPORT void led_toggle(int led) { switch (led) { case 0: - if (stm32_gpioread(GPIO_LED0)) + if (stm32_gpioread(GPIO_LED0)) { stm32_gpiowrite(GPIO_LED0, false); - else + + } else { stm32_gpiowrite(GPIO_LED0, true); + } + break; case 1: - if (stm32_gpioread(GPIO_LED1)) + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } + break; default: diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c index 1ce3f35f88..28103c843e 100644 --- a/src/drivers/boards/aerocore/aerocore_spi.c +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -136,8 +136,8 @@ __EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); break; From caee7af9ae330a85a55062baa12101910b0104c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:24:18 +0200 Subject: [PATCH 052/155] STM disco: Fix formatting --- .../boards/px4-stm32f4discovery/board_config.h | 10 +++++----- .../px4-stm32f4discovery/px4discovery_led.c | 15 +++++++-------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/src/drivers/boards/px4-stm32f4discovery/board_config.h b/src/drivers/boards/px4-stm32f4discovery/board_config.h index c4f469caa1..d06a3a317b 100644 --- a/src/drivers/boards/px4-stm32f4discovery/board_config.h +++ b/src/drivers/boards/px4-stm32f4discovery/board_config.h @@ -54,7 +54,7 @@ __BEGIN_DECLS #include #define UDID_START 0x1FFF7A10 - + /**************************************************************************************************** * Definitions ****************************************************************************************************/ @@ -66,13 +66,13 @@ __BEGIN_DECLS #define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) #define GPIO_LED4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) + GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) /* SPI chip selects */ #define GPIO_SPI_CS_MEMS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) diff --git a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c index 1f42843edf..6a6c6eaa0d 100644 --- a/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c +++ b/src/drivers/boards/px4-stm32f4discovery/px4discovery_led.c @@ -69,8 +69,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 1) - { + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } @@ -78,8 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 1) - { + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } @@ -87,11 +85,12 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 1) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 1) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } } From 6be4a70fc3b53bce41d9f4fcc1faaf34f95a86f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:24:36 +0200 Subject: [PATCH 053/155] FMUv1: Fix formatting --- src/drivers/boards/px4fmu-v1/px4fmu_init.c | 32 ++++++++++----------- src/drivers/boards/px4fmu-v1/px4fmu_led.c | 33 +++++++++++----------- src/drivers/boards/px4fmu-v1/px4fmu_spi.c | 4 +-- 3 files changed, 35 insertions(+), 34 deletions(-) diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c index b0e741017e..dea7678014 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c @@ -223,23 +223,23 @@ __EXPORT int nsh_archinitialize(void) * Keep the SPI2 init optional and conditionally initialize the ADC pins */ - #ifdef CONFIG_STM32_SPI2 - spi2 = up_spiinitialize(2); - /* Default SPI2 to 1MHz and de-assert the known chip selects. */ - SPI_SETFREQUENCY(spi2, 10000000); - SPI_SETBITS(spi2, 8); - SPI_SETMODE(spi2, SPIDEV_MODE3); - SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); - SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); +#ifdef CONFIG_STM32_SPI2 + spi2 = up_spiinitialize(2); + /* Default SPI2 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi2, 10000000); + SPI_SETBITS(spi2, 8); + SPI_SETMODE(spi2, SPIDEV_MODE3); + SPI_SELECT(spi2, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false); - message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); - #else - spi2 = NULL; - message("[boot] Enabling IN12/13 instead of SPI2\n"); - /* no SPI2, use pins for ADC */ - stm32_configgpio(GPIO_ADC1_IN12); - stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards - #endif + message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n"); +#else + spi2 = NULL; + message("[boot] Enabling IN12/13 instead of SPI2\n"); + /* no SPI2, use pins for ADC */ + stm32_configgpio(GPIO_ADC1_IN12); + stm32_configgpio(GPIO_ADC1_IN13); // jumperable to MPU6000 DRDY on some boards +#endif /* Get the SPI port for the microSD slot */ diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c index 888e4f551b..8a92cc6e3f 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c @@ -70,13 +70,12 @@ __EXPORT void led_init(void) __EXPORT void led_on(int led) { - if (led == 0) - { + if (led == 0) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } - if (led == 1) - { + + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED2, false); } @@ -84,13 +83,12 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 0) - { + if (led == 0) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } - if (led == 1) - { + + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED2, true); } @@ -98,18 +96,21 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 0) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 0) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } - if (led == 1) - { - if (stm32_gpioread(GPIO_LED2)) + + if (led == 1) { + if (stm32_gpioread(GPIO_LED2)) { stm32_gpiowrite(GPIO_LED2, false); - else + + } else { stm32_gpiowrite(GPIO_LED2, true); + } } } diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c index 2a444796ff..e877a6dff4 100644 --- a/src/drivers/boards/px4fmu-v1/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v1/px4fmu_spi.c @@ -90,8 +90,8 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, switch (devid) { case PX4_SPIDEV_GYRO: - /* Making sure the other peripherals are not selected */ - stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1); break; From 50ce08b3eef3cd7f0a2b0ab27662522b6ff3c809 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:24:44 +0200 Subject: [PATCH 054/155] FMUv2: Fix formatting --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 18 +++++++++++------- src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 15 +++++++-------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index e4f32b34c6..116a6ff556 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -118,12 +118,12 @@ __END_DECLS static GRAN_HANDLE dma_allocator; -/* - * The DMA heap size constrains the total number of things that can be +/* + * The DMA heap size constrains the total number of things that can be * ready to do DMA at a time. * * For example, FAT DMA depends on one sector-sized buffer per filesystem plus - * one sector-sized buffer per file. + * one sector-sized buffer per file. * * We use a fundamental alignment / granule size of 64B; this is sufficient * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). @@ -138,8 +138,10 @@ dma_alloc_init(void) sizeof(g_dma_heap), 7, /* 128B granule - must be > alignment (XXX bug?) */ 6); /* 64B alignment */ + if (dma_allocator == NULL) { message("[boot] DMA allocator setup FAILED"); + } else { g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); } @@ -214,12 +216,12 @@ static struct sdio_dev_s *sdio; /*#ifdef __cplusplus*/ /*__EXPORT int matherr(struct __exception *e)*/ /*{*/ - /*return 1;*/ +/*return 1;*/ /*}*/ /*#else*/ /*__EXPORT int matherr(struct exception *e)*/ /*{*/ - /*return 1;*/ +/*return 1;*/ /*}*/ /*#endif*/ @@ -326,10 +328,11 @@ __EXPORT int nsh_archinitialize(void) SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); - #ifdef CONFIG_MMCSD +#ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ sdio = sdio_initialize(CONFIG_NSH_MMCSDSLOTNO); + if (!sdio) { message("[boot] Failed to initialize SDIO slot %d\n", CONFIG_NSH_MMCSDSLOTNO); @@ -338,6 +341,7 @@ __EXPORT int nsh_archinitialize(void) /* Now bind the SDIO interface to the MMC/SD driver */ int ret = mmcsd_slotinitialize(CONFIG_NSH_MMCSDMINOR, sdio); + if (ret != OK) { message("[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); return ret; @@ -346,7 +350,7 @@ __EXPORT int nsh_archinitialize(void) /* Then let's guess and say that there is a card in the slot. There is no card detect GPIO. */ sdio_mediachange(sdio, true); - #endif +#endif return OK; } diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index 74f77b3ef6..09edb2ba66 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -69,8 +69,7 @@ __EXPORT void led_init() __EXPORT void led_on(int led) { - if (led == 1) - { + if (led == 1) { /* Pull down to switch on */ stm32_gpiowrite(GPIO_LED1, false); } @@ -78,8 +77,7 @@ __EXPORT void led_on(int led) __EXPORT void led_off(int led) { - if (led == 1) - { + if (led == 1) { /* Pull up to switch off */ stm32_gpiowrite(GPIO_LED1, true); } @@ -87,11 +85,12 @@ __EXPORT void led_off(int led) __EXPORT void led_toggle(int led) { - if (led == 1) - { - if (stm32_gpioread(GPIO_LED1)) + if (led == 1) { + if (stm32_gpioread(GPIO_LED1)) { stm32_gpiowrite(GPIO_LED1, false); - else + + } else { stm32_gpiowrite(GPIO_LED1, true); + } } } From baf04a504aa6935c886fa87fd7a334b9532ff5c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:24:59 +0200 Subject: [PATCH 055/155] HoTT: Fix formatting --- .../hott/hott_sensors/hott_sensors.cpp | 21 ++++++++++++------- .../hott/hott_telemetry/hott_telemetry.cpp | 19 +++++++++++------ 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 29680a279f..f5c947d4f6 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -104,15 +104,16 @@ int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) { static const int timeout_ms = 1000; - + struct pollfd fds; fds.fd = uart; fds.events = POLLIN; - + // XXX should this poll be inside the while loop??? if (poll(&fds, 1, timeout_ms) > 0) { int i = 0; bool stop_byte_read = false; + while (true) { read(uart, &buffer[i], sizeof(buffer[i])); @@ -121,14 +122,17 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) *size = ++i; return OK; } + // XXX can some other field not have the STOP BYTE value? if (buffer[i] == STOP_BYTE) { *id = buffer[1]; stop_byte_read = true; } + i++; } } + return ERROR; } @@ -156,6 +160,7 @@ hott_sensors_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); + if (uart < 0) { errx(1, "Open fail, exiting."); thread_running = false; @@ -166,6 +171,7 @@ hott_sensors_thread_main(int argc, char *argv[]) uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; + while (!thread_should_exit) { // Currently we only support a General Air Module sensor. build_gam_request(&buffer[0], &size); @@ -179,6 +185,7 @@ hott_sensors_thread_main(int argc, char *argv[]) // Determine which moduel sent it and process accordingly. if (id == GAM_SENSOR_ID) { publish_gam_message(buffer); + } else { warnx("Unknown sensor ID: %d", id); } @@ -210,11 +217,11 @@ hott_sensors_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1024, - hott_sensors_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1024, + hott_sensors_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index a1a5b080c5..f5aa0ec58a 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -90,7 +90,7 @@ recv_req_id(int uart, uint8_t *id) static const int timeout_ms = 1000; // TODO make it a define uint8_t mode; - + struct pollfd fds; fds.fd = uart; fds.events = POLLIN; @@ -106,6 +106,7 @@ recv_req_id(int uart, uint8_t *id) /* Read the device ID being polled */ read(uart, id, sizeof(*id)); + } else { warnx("UART timeout on TX/RX port"); return ERROR; @@ -120,6 +121,7 @@ send_data(int uart, uint8_t *buffer, size_t size) usleep(POST_READ_DELAY_IN_USECS); uint16_t checksum = 0; + for (size_t i = 0; i < size; i++) { if (i == size - 1) { /* Set the checksum: the first uint8_t is taken as the checksum. */ @@ -167,6 +169,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); + if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; @@ -178,6 +181,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) size_t size = 0; uint8_t id = 0; bool connected = true; + while (!thread_should_exit) { // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { @@ -190,9 +194,11 @@ hott_telemetry_thread_main(int argc, char *argv[]) case EAM_SENSOR_ID: build_eam_response(buffer, &size); break; + case GAM_SENSOR_ID: build_gam_response(buffer, &size); break; + case GPS_SENSOR_ID: build_gps_response(buffer, &size); break; @@ -202,6 +208,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) } send_data(uart, buffer, size); + } else { connected = false; warnx("syncing"); @@ -236,11 +243,11 @@ hott_telemetry_main(int argc, char *argv[]) thread_should_exit = false; deamon_task = px4_task_spawn_cmd(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - hott_telemetry_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + hott_telemetry_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } From 2c8e8532941e405a751cb6e056dcca99df0138c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:25:10 +0200 Subject: [PATCH 056/155] Drivers: fix formatting --- src/drivers/boards/px4io-v1/board_config.h | 12 ++++++------ src/drivers/drv_device.h | 2 +- src/drivers/drv_pwm_output.h | 2 +- src/drivers/roboclaw/RoboClaw.hpp | 16 ++++++++-------- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/drivers/boards/px4io-v1/board_config.h b/src/drivers/boards/px4io-v1/board_config.h index 59c17431c7..e05e030713 100644 --- a/src/drivers/boards/px4io-v1/board_config.h +++ b/src/drivers/boards/px4io-v1/board_config.h @@ -30,10 +30,10 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - + /** * @file board_config.h - * + * * PX4IO hardware definitions. */ @@ -59,11 +59,11 @@ /* LEDs */ #define GPIO_LED1 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN14) #define GPIO_LED2 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN15) #define GPIO_LED3 (GPIO_OUTPUT|GPIO_CNF_OUTOD|GPIO_MODE_50MHz|\ - GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) + GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) /* Safety switch button *************************************************************/ @@ -86,7 +86,7 @@ #define GPIO_ADC_VBATT (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4) #define GPIO_ADC_IN5 (GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5) -/* +/* * High-resolution timer */ #define HRT_TIMER 1 /* use timer1 for the HRT */ diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index e0226590bc..02dbe7af67 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -68,7 +68,7 @@ #ifdef __PX4_POSIX #ifndef SIOCDEVPRIVATE - #define SIOCDEVPRIVATE 1 +#define SIOCDEVPRIVATE 1 #endif #define DIOC_GETPRIV SIOCDEVPRIVATE diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index fc619866be..bb04245333 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -65,7 +65,7 @@ __BEGIN_DECLS #define pwm_output_values output_pwm_s #ifndef PWM_OUTPUT_MAX_CHANNELS - #define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS +#define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS #endif /** diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 58994d6fa6..f2096088b9 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -61,7 +61,7 @@ public: CH_VOLTAGE_LEFT = 0, CH_VOLTAGE_RIGHT }; - + /** motors */ enum e_motor { MOTOR_1 = 0, @@ -70,15 +70,15 @@ public: /** * constructor - * @param deviceName the name of the + * @param deviceName the name of the * serial port e.g. "/dev/ttyS2" * @param address the adddress of the motor * (selectable on roboclaw) * @param pulsesPerRev # of encoder * pulses per revolution of wheel */ - RoboClaw(const char *deviceName, uint16_t address, - uint16_t pulsesPerRev); + RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev); /** * deconstructor @@ -153,7 +153,7 @@ private: // advanced motor control CMD_READ_SPEED_HIRES_1 = 30, - CMD_READ_SPEED_HIRES_2 = 31, + CMD_READ_SPEED_HIRES_2 = 31, CMD_SIGNED_DUTYCYCLE_1 = 32, CMD_SIGNED_DUTYCYCLE_2 = 33, }; @@ -181,12 +181,12 @@ private: int16_t _motor2Overflow; // private methods - uint16_t _sumBytes(uint8_t * buf, size_t n); - int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); + uint16_t _sumBytes(uint8_t *buf, size_t n); + int _sendCommand(e_command cmd, uint8_t *data, size_t n_data, uint16_t &prev_sum); }; // unit testing int roboclawTest(const char *deviceName, uint8_t address, - uint16_t pulsesPerRev); + uint16_t pulsesPerRev); // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 From 73545a43115c00c42e504423bdd21c17ff1b7dc0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:25:39 +0200 Subject: [PATCH 057/155] Platforms: Fix platform headers --- src/platforms/px4_adc.h | 11 ++- src/platforms/px4_app.h | 10 ++- src/platforms/px4_defines.h | 16 ++-- src/platforms/px4_i2c.h | 17 ++-- src/platforms/px4_log.h | 140 ++++++++++++++++----------------- src/platforms/px4_nodehandle.h | 16 ++-- src/platforms/px4_posix.h | 23 +++--- src/platforms/px4_publisher.h | 10 ++- src/platforms/px4_spi.h | 39 +++++---- src/platforms/px4_subscriber.h | 21 ++--- src/platforms/px4_tasks.h | 10 +-- src/platforms/px4_time.h | 3 +- src/platforms/px4_workqueue.h | 24 +++--- 13 files changed, 168 insertions(+), 172 deletions(-) diff --git a/src/platforms/px4_adc.h b/src/platforms/px4_adc.h index 8420f51971..c8c8ce01f0 100644 --- a/src/platforms/px4_adc.h +++ b/src/platforms/px4_adc.h @@ -52,13 +52,12 @@ // FIXME - this needs to be a px4_adc_msg_s type // Curently copied from NuttX -struct adc_msg_s -{ - uint8_t am_channel; /* The 8-bit ADC Channel */ - int32_t am_data; /* ADC convert result (4 bytes) */ -} __attribute__ ((packed)); +struct adc_msg_s { + uint8_t am_channel; /* The 8-bit ADC Channel */ + int32_t am_data; /* ADC convert result (4 bytes) */ +} __attribute__((packed)); -// Example settings +// Example settings #define ADC_BATTERY_VOLTAGE_CHANNEL 10 #define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 diff --git a/src/platforms/px4_app.h b/src/platforms/px4_app.h index 9aa285e228..df3a8d8294 100644 --- a/src/platforms/px4_app.h +++ b/src/platforms/px4_app.h @@ -40,9 +40,11 @@ #pragma once -namespace px4 { +namespace px4 +{ -class AppState { +class AppState +{ public: ~AppState() {} @@ -65,8 +67,8 @@ protected: bool _isRunning; #endif private: - AppState(const AppState&); - const AppState& operator=(const AppState&); + AppState(const AppState &); + const AppState &operator=(const AppState &); }; } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 69dc2d64eb..b80532b099 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -92,7 +92,7 @@ typedef param_t px4_param_t; */ #if defined(__PX4_NUTTX) -#define PX4_ROOTFSDIR +#define PX4_ROOTFSDIR /* XXX this is a hack to resolve conflicts with NuttX headers */ #if !defined(__PX4_TESTS) @@ -119,7 +119,7 @@ typedef param_t px4_param_t; #define PRId64 "lld" #endif -/* +/* * POSIX Specific defines */ #elif defined(__PX4_POSIX) @@ -138,7 +138,7 @@ typedef param_t px4_param_t; /* FIXME - Used to satisfy build */ //STM DocID018909 Rev 8 Sect 39.1 (Unique device ID Register) -#define UNIQUE_ID 0x1FFF7A10 +#define UNIQUE_ID 0x1FFF7A10 #define STM32_SYSMEM_UID "SIMULATIONID" /* FIXME - Used to satisfy build */ @@ -153,12 +153,12 @@ __END_DECLS #endif #define USEC_PER_TICK (1000000UL/PX4_TICKS_PER_SEC) -#define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) +#define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) #define px4_statfs_buf_f_bavail_t unsigned long #if defined(__PX4_QURT) -#define PX4_ROOTFSDIR +#define PX4_ROOTFSDIR #else #define PX4_ROOTFSDIR "rootfs" #endif @@ -166,7 +166,7 @@ __END_DECLS #endif -/* +/* * Defines for ROS and Linux */ #if defined(__PX4_ROS) || defined(__PX4_POSIX) @@ -226,7 +226,7 @@ __END_DECLS #if defined(__PX4_QURT) -#define PX4_ROOTFSDIR +#define PX4_ROOTFSDIR #define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" #define SIOCDEVPRIVATE 999999 @@ -244,7 +244,7 @@ __END_DECLS #endif -/* +/* *Defines for all platforms */ diff --git a/src/platforms/px4_i2c.h b/src/platforms/px4_i2c.h index 2b72f37c5d..1d49600473 100644 --- a/src/platforms/px4_i2c.h +++ b/src/platforms/px4_i2c.h @@ -75,23 +75,23 @@ typedef struct i2c_dev_s px4_i2c_dev_t; // NOTE - This is a copy of the NuttX i2c_msg_s structure typedef struct { - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; } px4_i2c_msg_t; // NOTE - This is a copy of the NuttX i2c_ops_s structure typedef struct { - const struct px4_i2c_ops_t *ops; /* I2C vtable */ + const struct px4_i2c_ops_t *ops; /* I2C vtable */ } px4_i2c_dev_t; // FIXME - Empty defines for I2C ops // Original version commented out //#define I2C_SETFREQUENCY(d,f) ((d)->ops->setfrequency(d,f)) -#define I2C_SETFREQUENCY(d,f) +#define I2C_SETFREQUENCY(d,f) //#define SPI_SELECT(d,id,s) ((d)->ops->select(d,id,s)) -#define SPI_SELECT(d,id,s) +#define SPI_SELECT(d,id,s) // FIXME - Stub implementation // Original version commented out @@ -101,8 +101,7 @@ inline int I2C_TRANSFER(px4_i2c_dev_t *dev, px4_i2c_msg_t *msg, int count) { ret #ifdef __PX4_QURT -struct i2c_msg -{ +struct i2c_msg { uint16_t addr; /* Slave address */ uint16_t flags; /* See I2C_M_* definitions */ uint8_t *buf; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 3dcb29515b..953b766f4d 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -178,11 +178,11 @@ __END_DECLS ****************************************************************************/ #define __px4_log_named_cond(name, cond, FMT, ...) \ __px4__log_printcond(cond,\ - "%s " \ - FMT\ - __px4__log_end_fmt \ - ,name, ##__VA_ARGS__\ - ) + "%s " \ + FMT\ + __px4__log_end_fmt \ + ,name, ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log: @@ -193,11 +193,11 @@ __END_DECLS ****************************************************************************/ #define __px4_log(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt \ - FMT\ - __px4__log_end_fmt \ - __px4__log_level_arg(level), ##__VA_ARGS__\ - ) + __px4__log_level_fmt \ + FMT\ + __px4__log_end_fmt \ + __px4__log_level_arg(level), ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log_timestamp: @@ -209,14 +209,14 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - FMT\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - , ##__VA_ARGS__\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log_timestamp_thread: @@ -228,16 +228,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp_thread(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - __px4__log_thread_fmt\ - FMT\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - __px4__log_thread_arg\ - , ##__VA_ARGS__\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + ) /**************************************************************************** * __px4_log_file_and_line: @@ -249,16 +249,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** * __px4_log_timestamp_file_and_line: @@ -271,16 +271,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** * __px4_log_thread_file_and_line: @@ -293,16 +293,16 @@ __END_DECLS ****************************************************************************/ #define __px4_log_thread_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_thread_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_thread_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** * __px4_log_timestamp_thread_file_and_line: @@ -315,18 +315,18 @@ __END_DECLS ****************************************************************************/ #define __px4_log_timestamp_thread_file_and_line(level, FMT, ...) \ __px4__log_printline(level,\ - __px4__log_level_fmt\ - __px4__log_timestamp_fmt\ - __px4__log_thread_fmt\ - FMT\ - __px4__log_file_and_line_fmt\ - __px4__log_end_fmt\ - __px4__log_level_arg(level)\ - __px4__log_timestamp_arg\ - __px4__log_thread_arg\ - , ##__VA_ARGS__\ - __px4__log_file_and_line_arg\ - ) + __px4__log_level_fmt\ + __px4__log_timestamp_fmt\ + __px4__log_thread_fmt\ + FMT\ + __px4__log_file_and_line_fmt\ + __px4__log_end_fmt\ + __px4__log_level_arg(level)\ + __px4__log_timestamp_arg\ + __px4__log_thread_arg\ + , ##__VA_ARGS__\ + __px4__log_file_and_line_arg\ + ) /**************************************************************************** diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 497f3be04c..dc806aa905 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -86,7 +86,7 @@ public: template Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -99,7 +99,7 @@ public: template Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber *)sub; } @@ -110,7 +110,7 @@ public: template Subscriber *subscribe(unsigned interval) { - SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); + SubscriberBase *sub = new SubscriberROS((ros::NodeHandle *)this); _subs.push_back(sub); return (Subscriber *)sub; } @@ -119,11 +119,11 @@ public: * Advertise topic */ template - Publisher* advertise() + Publisher *advertise() { - PublisherROS *pub = new PublisherROS((ros::NodeHandle*)this); - _pubs.push_back((PublisherBase*)pub); - return (Publisher*)pub; + PublisherROS *pub = new PublisherROS((ros::NodeHandle *)this); + _pubs.push_back((PublisherBase *)pub); + return (Publisher *)pub; } /** @@ -242,7 +242,7 @@ public: { PublisherUORB *pub = new PublisherUORB(); _pubs.add(pub); - return (Publisher*)pub; + return (Publisher *)pub; } /** diff --git a/src/platforms/px4_posix.h b/src/platforms/px4_posix.h index bd682d9ce5..ab53d03900 100644 --- a/src/platforms/px4_posix.h +++ b/src/platforms/px4_posix.h @@ -54,8 +54,7 @@ __BEGIN_DECLS -typedef struct -{ +typedef struct { pthread_mutex_t lock; pthread_cond_t wait; int value; @@ -97,7 +96,7 @@ typedef struct pollfd px4_pollfd_struct_t; #if defined(__cplusplus) #define _GLOBAL :: #else -#define _GLOBAL +#define _GLOBAL #endif #define px4_open _GLOBAL open #define px4_close _GLOBAL close @@ -118,14 +117,14 @@ typedef struct pollfd px4_pollfd_struct_t; typedef short pollevent_t; typedef struct { - /* This part of the struct is POSIX-like */ - int fd; /* The descriptor being polled */ - pollevent_t events; /* The input event flags */ - pollevent_t revents; /* The output event flags */ + /* This part of the struct is POSIX-like */ + int fd; /* The descriptor being polled */ + pollevent_t events; /* The input event flags */ + pollevent_t revents; /* The output event flags */ - /* Required for PX4 compatability */ - px4_sem_t *sem; /* Pointer to semaphore used to post output event */ - void *priv; /* For use by drivers */ + /* Required for PX4 compatability */ + px4_sem_t *sem; /* Pointer to semaphore used to post output event */ + void *priv; /* For use by drivers */ } px4_pollfd_struct_t; __BEGIN_DECLS @@ -150,8 +149,8 @@ extern int px4_errno; __EXPORT void px4_show_devices(void); __EXPORT void px4_show_files(void); -__EXPORT const char * px4_get_device_names(unsigned int *handle); +__EXPORT const char *px4_get_device_names(unsigned int *handle); __EXPORT void px4_show_topics(void); -__EXPORT const char * px4_get_topic_names(unsigned int *handle); +__EXPORT const char *px4_get_topic_names(unsigned int *handle); __END_DECLS diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index eb8e964e79..fb9ec50e10 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -86,7 +86,8 @@ public: */ PublisherROS(ros::NodeHandle *rnh) : Publisher(), - _ros_pub(rnh->advertisedata())>::type &>(T::handle(), kQueueSizeDefault)) + _ros_pub(rnh->advertise < typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type & > (T::handle(), + kQueueSizeDefault)) {} ~PublisherROS() {}; @@ -136,7 +137,8 @@ public: _uorb_pub(new uORB::PublicationBase(T::handle())) {} - ~PublisherUORB() { + ~PublisherUORB() + { delete _uorb_pub; }; @@ -145,7 +147,7 @@ public: */ int publish(const T &msg) { - _uorb_pub->update((void *)&(msg.data())); + _uorb_pub->update((void *) & (msg.data())); return 0; } @@ -154,7 +156,7 @@ public: */ void update() {} ; private: - uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + uORB::PublicationBase *_uorb_pub; /**< Handle to the publisher */ }; #endif diff --git a/src/platforms/px4_spi.h b/src/platforms/px4_spi.h index 17397ee7ac..78b747b775 100644 --- a/src/platforms/px4_spi.h +++ b/src/platforms/px4_spi.h @@ -3,32 +3,29 @@ #ifdef __PX4_NUTTX #include #elif defined(__PX4_POSIX) -enum spi_dev_e -{ - SPIDEV_NONE = 0, /* Not a valid value */ - SPIDEV_MMCSD, /* Select SPI MMC/SD device */ - SPIDEV_FLASH, /* Select SPI FLASH device */ - SPIDEV_ETHERNET, /* Select SPI ethernet device */ - SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ - SPIDEV_WIRELESS, /* Select SPI Wireless device */ - SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ - SPIDEV_EXPANDER, /* Select SPI I/O expander device */ - SPIDEV_MUX, /* Select SPI multiplexer device */ - SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ - SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ +enum spi_dev_e { + SPIDEV_NONE = 0, /* Not a valid value */ + SPIDEV_MMCSD, /* Select SPI MMC/SD device */ + SPIDEV_FLASH, /* Select SPI FLASH device */ + SPIDEV_ETHERNET, /* Select SPI ethernet device */ + SPIDEV_DISPLAY, /* Select SPI LCD/OLED display device */ + SPIDEV_WIRELESS, /* Select SPI Wireless device */ + SPIDEV_TOUCHSCREEN, /* Select SPI touchscreen device */ + SPIDEV_EXPANDER, /* Select SPI I/O expander device */ + SPIDEV_MUX, /* Select SPI multiplexer device */ + SPIDEV_AUDIO_DATA, /* Select SPI audio codec device data port */ + SPIDEV_AUDIO_CTRL, /* Select SPI audio codec device control port */ }; /* Certain SPI devices may required differnt clocking modes */ -enum spi_mode_e -{ - SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ - SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ - SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ - SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ +enum spi_mode_e { + SPIDEV_MODE0 = 0, /* CPOL=0 CHPHA=0 */ + SPIDEV_MODE1, /* CPOL=0 CHPHA=1 */ + SPIDEV_MODE2, /* CPOL=1 CHPHA=0 */ + SPIDEV_MODE3 /* CPOL=1 CHPHA=1 */ }; -struct spi_dev_s -{ +struct spi_dev_s { int unused; }; #else diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index e92c82fc66..59cfa5a191 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -86,12 +86,12 @@ public: /** * Get the last message value */ - virtual T& get() {return _msg_current;} + virtual T &get() {return _msg_current;} /** * Get the last native message value */ - virtual decltype(((T*)nullptr)->data()) data() + virtual decltype(((T *)nullptr)->data()) data() { return _msg_current.data(); } @@ -135,7 +135,7 @@ protected: * Called on topic update, saves the current message and then calls the provided callback function * needs to use the native type as it is called by ROS */ - void callback(const typename std::remove_referencedata())>::type &msg) + void callback(const typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type &msg) { /* Store data */ this->_msg_current.data() = msg; @@ -197,7 +197,8 @@ public: _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) {} - virtual ~SubscriberUORB() { + virtual ~SubscriberUORB() + { delete _uorb_sub; }; @@ -219,19 +220,19 @@ public: int getUORBHandle() { return _uorb_sub->getHandle(); } protected: - uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + uORB::SubscriptionBase *_uorb_sub; /**< Handle to the subscription */ #ifndef CONFIG_ARCH_BOARD_SIM - typename std::remove_referencedata())>::type getUORBData() + typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type getUORBData() { - return (typename std::remove_referencedata())>::type)*_uorb_sub; + return (typename std::remove_reference < decltype(((T *)nullptr)->data()) >::type) * _uorb_sub; } #endif /** * Get void pointer to last message value */ - void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } + void *get_void_ptr() { return (void *) & (this->_msg_current.data()); } }; @@ -248,9 +249,9 @@ public: */ SubscriberUORBCallback(unsigned interval #ifndef CONFIG_ARCH_BOARD_SIM - ,std::function cbf) + , std::function cbf) #else - ) + ) #endif : SubscriberUORB(interval), diff --git a/src/platforms/px4_tasks.h b/src/platforms/px4_tasks.h index 9dc237e63b..04585aaced 100644 --- a/src/platforms/px4_tasks.h +++ b/src/platforms/px4_tasks.h @@ -105,11 +105,11 @@ __EXPORT void px4_systemreset(bool to_bootloader) noreturn_function; /** Starts a task and performs any specific accounting, scheduler setup, etc. */ __EXPORT px4_task_t px4_task_spawn_cmd(const char *name, - int priority, - int scheduler, - int stack_size, - px4_main_t entry, - char * const argv[]); + int priority, + int scheduler, + int stack_size, + px4_main_t entry, + char *const argv[]); /** Deletes a task - does not do resource cleanup **/ __EXPORT int px4_task_delete(px4_task_t pid); diff --git a/src/platforms/px4_time.h b/src/platforms/px4_time.h index 22751b3a99..d57f8e9103 100644 --- a/src/platforms/px4_time.h +++ b/src/platforms/px4_time.h @@ -30,8 +30,7 @@ __BEGIN_DECLS #if 0 #if !defined(__cplusplus) -struct timespec -{ +struct timespec { time_t tv_sec; long tv_nsec; }; diff --git a/src/platforms/px4_workqueue.h b/src/platforms/px4_workqueue.h index 76e1218ba5..3e3170f2de 100644 --- a/src/platforms/px4_workqueue.h +++ b/src/platforms/px4_workqueue.h @@ -48,7 +48,7 @@ #include #ifdef __PX4_QURT - #include +#include #endif __BEGIN_DECLS @@ -57,10 +57,9 @@ __BEGIN_DECLS #define LPWORK 1 #define NWORKERS 2 -struct wqueue_s -{ - pid_t pid; /* The task ID of the worker thread */ - struct dq_queue_s q; /* The queue of pending work */ +struct wqueue_s { + pid_t pid; /* The task ID of the worker thread */ + struct dq_queue_s q; /* The queue of pending work */ }; extern struct wqueue_s g_work[NWORKERS]; @@ -69,13 +68,12 @@ extern struct wqueue_s g_work[NWORKERS]; typedef void (*worker_t)(void *arg); -struct work_s -{ - struct dq_entry_s dq; /* Implements a doubly linked list */ - worker_t worker; /* Work callback */ - void *arg; /* Callback argument */ - uint64_t qtime; /* Time work queued */ - uint32_t delay; /* Delay until work performed */ +struct work_s { + struct dq_entry_s dq; /* Implements a doubly linked list */ + worker_t worker; /* Work callback */ + void *arg; /* Callback argument */ + uint64_t qtime; /* Time work queued */ + uint32_t delay; /* Delay until work performed */ }; /**************************************************************************** @@ -144,6 +142,6 @@ int work_lpthread(int argc, char *argv[]); __END_DECLS -#else +#else #error "Unknown target OS" #endif From 567935dedb1926776f88bbc259cadc1d4464a596 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:35:05 +0200 Subject: [PATCH 058/155] QuRT: Fix formatting --- src/platforms/qurt/dspal/dspal_stub.c | 12 +- src/platforms/qurt/include/crc32.h | 2 +- src/platforms/qurt/include/i2c.h | 11 +- src/platforms/qurt/include/queue.h | 32 ++-- src/platforms/qurt/include/sched.h | 3 +- src/platforms/qurt/px4_layer/commands_hil.c | 4 +- src/platforms/qurt/px4_layer/commands_muorb.c | 70 ++++---- src/platforms/qurt/px4_layer/drv_hrt.c | 20 ++- src/platforms/qurt/px4_layer/lib_crc32.c | 85 +++++----- src/platforms/qurt/px4_layer/main.cpp | 114 +++++++------ src/platforms/qurt/px4_layer/params.c | 2 +- .../qurt/px4_layer/px4_qurt_impl.cpp | 24 +-- .../qurt/px4_layer/px4_qurt_tasks.cpp | 153 ++++++++++-------- src/platforms/qurt/px4_layer/qurt_stubs.c | 70 ++++---- src/platforms/qurt/stubs/stubs_posix.c | 2 +- .../qurt/tests/hello/hello_example.cpp | 5 +- .../qurt/tests/hello/hello_example.h | 3 +- .../qurt/tests/hello/hello_start_qurt.cpp | 10 +- .../qurt/tests/muorb/muorb_test_example.h | 3 +- .../qurt/tests/muorb/muorb_test_main.cpp | 2 +- .../tests/muorb/muorb_test_start_qurt.cpp | 14 +- 21 files changed, 344 insertions(+), 297 deletions(-) diff --git a/src/platforms/qurt/dspal/dspal_stub.c b/src/platforms/qurt/dspal/dspal_stub.c index fc736a169c..13542cdfd3 100644 --- a/src/platforms/qurt/dspal/dspal_stub.c +++ b/src/platforms/qurt/dspal/dspal_stub.c @@ -43,25 +43,29 @@ static void do_dlopen() char *error; void (*entry_function)() = NULL; - handle = dlopen ("libdspal_client.so", RTLD_LAZY); + handle = dlopen("libdspal_client.so", RTLD_LAZY); + if (!handle) { printf("Error opening libdspal_client.so\n"); return 1; } + entry_function = dlsym(handle, "dspal_entry"); + if (((error = dlerror()) != NULL) || (entry_function == NULL)) { printf("Error dlsym for dspal_entry"); ret = 2; } + dlclose(handle); #endif } - -int main(int argc, char* argv[]) + +int main(int argc, char *argv[]) { int ret = 0; - char *builtin[]={"libgcc.so", "libc.so", "libstdc++.so"}; + char *builtin[] = {"libgcc.so", "libc.so", "libstdc++.so"}; printf("In DSPAL main\n"); dlinit(3, builtin); diff --git a/src/platforms/qurt/include/crc32.h b/src/platforms/qurt/include/crc32.h index bf828e3e0e..34e080b1c2 100644 --- a/src/platforms/qurt/include/crc32.h +++ b/src/platforms/qurt/include/crc32.h @@ -63,7 +63,7 @@ extern "C" { ****************************************************************************/ EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); + uint32_t crc32val); /**************************************************************************** * Name: crc32 diff --git a/src/platforms/qurt/include/i2c.h b/src/platforms/qurt/include/i2c.h index ee3e4196dd..5021cf1814 100644 --- a/src/platforms/qurt/include/i2c.h +++ b/src/platforms/qurt/include/i2c.h @@ -34,10 +34,9 @@ ****************************************************************************/ #pragma once -struct i2c_msg_s -{ - uint16_t addr; /* Slave address */ - uint16_t flags; /* See I2C_M_* definitions */ - uint8_t *buffer; - int length; +struct i2c_msg_s { + uint16_t addr; /* Slave address */ + uint16_t flags; /* See I2C_M_* definitions */ + uint8_t *buffer; + int length; }; diff --git a/src/platforms/qurt/include/queue.h b/src/platforms/qurt/include/queue.h index 4d95541e02..30dc264f3f 100644 --- a/src/platforms/qurt/include/queue.h +++ b/src/platforms/qurt/include/queue.h @@ -69,30 +69,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -112,11 +108,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/qurt/include/sched.h b/src/platforms/qurt/include/sched.h index 7d7724c5fa..b67db23b1d 100644 --- a/src/platforms/qurt/include/sched.h +++ b/src/platforms/qurt/include/sched.h @@ -3,8 +3,7 @@ #define SCHED_FIFO 1 #define SCHED_RR 2 -struct sched_param -{ +struct sched_param { int sched_priority; }; diff --git a/src/platforms/qurt/px4_layer/commands_hil.c b/src/platforms/qurt/px4_layer/commands_hil.c index 3ff299b89d..87aa6479f8 100644 --- a/src/platforms/qurt/px4_layer/commands_hil.c +++ b/src/platforms/qurt/px4_layer/commands_hil.c @@ -41,7 +41,7 @@ const char *get_commands(void); const char *get_commands() { - static const char *commands = + static const char *commands = "uorb start\n" "param set CAL_GYRO0_ID 2293760\n" "param set CAL_ACC0_ID 1310720\n" @@ -104,5 +104,5 @@ const char *get_commands() ; return commands; - + } diff --git a/src/platforms/qurt/px4_layer/commands_muorb.c b/src/platforms/qurt/px4_layer/commands_muorb.c index 27318af9c6..de6381a35a 100644 --- a/src/platforms/qurt/px4_layer/commands_muorb.c +++ b/src/platforms/qurt/px4_layer/commands_muorb.c @@ -39,42 +39,42 @@ const char *get_commands() { - static const char *commands = - "uorb start\n" - "muorb_test start\n"; + static const char *commands = + "uorb start\n" + "muorb_test start\n"; -/* - "hil mode_pwm\n" - "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; -*/ -/* - "param show\n" - "param set CAL_GYRO_ID 2293760\n" - "param set CAL_ACC0_ID 1310720\n" - "param set CAL_ACC1_ID 1376256\n" - "param set CAL_MAG0_ID 196608\n" - "gyrosim start\n" - "accelsim start\n" - "rgbled start\n" - "tone_alarm start\n" - "simulator start -s\n" - "commander start\n" - "sensors start\n" - "ekf_att_pos_estimator start\n" - "mc_pos_control start\n" - "mc_att_control start\n" - "param set MAV_TYPE 2\n" - "param set RC1_MAX 2015\n" - "param set RC1_MIN 996\n" - "param set RC_TRIM 1502\n" -*/ + /* + "hil mode_pwm\n" + "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"; + */ + /* + "param show\n" + "param set CAL_GYRO_ID 2293760\n" + "param set CAL_ACC0_ID 1310720\n" + "param set CAL_ACC1_ID 1376256\n" + "param set CAL_MAG0_ID 196608\n" + "gyrosim start\n" + "accelsim start\n" + "rgbled start\n" + "tone_alarm start\n" + "simulator start -s\n" + "commander start\n" + "sensors start\n" + "ekf_att_pos_estimator start\n" + "mc_pos_control start\n" + "mc_att_control start\n" + "param set MAV_TYPE 2\n" + "param set RC1_MAX 2015\n" + "param set RC1_MIN 996\n" + "param set RC_TRIM 1502\n" + */ return commands; -/*====================================== Working set -======================================*/ - - //"muorb_test start\n" - //"gyrosim start\n" - //"adcsim start\n" - + /*====================================== Working set + ======================================*/ + + //"muorb_test start\n" + //"gyrosim start\n" + //"adcsim start\n" + } diff --git a/src/platforms/qurt/px4_layer/drv_hrt.c b/src/platforms/qurt/px4_layer/drv_hrt.c index b5d6bd20ff..c15c818172 100644 --- a/src/platforms/qurt/px4_layer/drv_hrt.c +++ b/src/platforms/qurt/px4_layer/drv_hrt.c @@ -219,7 +219,7 @@ hrt_call_enter(struct hrt_call *entry) * * This routine simulates a timer interrupt handler */ -static void +static void hrt_tim_isr(void *p) { @@ -249,7 +249,7 @@ hrt_call_reschedule() uint32_t ticks = USEC2TICK(HRT_INTERVAL_MAX); //printf("hrt_call_reschedule\n"); - + /* * Determine what the next deadline will be. * @@ -275,10 +275,10 @@ hrt_call_reschedule() } } - // There is no timer ISR, so simulate one by putting an event on the + // There is no timer ISR, so simulate one by putting an event on the // high priority work queue //printf("ticks = %u\n", ticks); - work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); + work_queue(HPWORK, &_hrt_work, (worker_t)&hrt_tim_isr, NULL, ticks); } static void @@ -286,6 +286,7 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte { //printf("hrt_call_internal\n"); hrt_lock(); + //printf("hrt_call_internal after lock\n"); /* if the entry is currently queued, remove it */ /* note that we are using a potentially uninitialised @@ -295,8 +296,9 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte queue for the uninitialised entry->link but we don't do anything actually unsafe. */ - if (entry->deadline != 0) + if (entry->deadline != 0) { sq_rem(&entry->link, &callout_queue); + } entry->deadline = deadline; entry->period = interval; @@ -360,17 +362,20 @@ hrt_call_invoke(void) hrt_abstime deadline; hrt_lock(); + while (true) { /* get the current time */ hrt_abstime now = hrt_absolute_time(); call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) + if (call == NULL) { break; + } - if (call->deadline > now) + if (call->deadline > now) { break; + } sq_rem(&call->link, &callout_queue); //lldbg("call pop\n"); @@ -404,6 +409,7 @@ hrt_call_invoke(void) hrt_call_enter(call); } } + hrt_unlock(); } diff --git a/src/platforms/qurt/px4_layer/lib_crc32.c b/src/platforms/qurt/px4_layer/lib_crc32.c index 4ba6fbf6df..c14ebfceeb 100644 --- a/src/platforms/qurt/px4_layer/lib_crc32.c +++ b/src/platforms/qurt/px4_layer/lib_crc32.c @@ -48,46 +48,45 @@ #include // Needed for Linux -#define FAR +#define FAR /************************************************************************************************ * Private Data ************************************************************************************************/ - -static const uint32_t crc32_tab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + +static const uint32_t crc32_tab[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; /************************************************************************************************ @@ -103,13 +102,13 @@ static const uint32_t crc32_tab[] = uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) { - size_t i; + size_t i; - for (i = 0; i < len; i++) - { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - return crc32val; + for (i = 0; i < len; i++) { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } + + return crc32val; } /************************************************************************************************ @@ -122,5 +121,5 @@ uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) uint32_t crc32(FAR const uint8_t *src, size_t len) { - return crc32part(src, len, 0); + return crc32part(src, len, 0); } diff --git a/src/platforms/qurt/px4_layer/main.cpp b/src/platforms/qurt/px4_layer/main.cpp index 15907ded78..fc8801b60e 100644 --- a/src/platforms/qurt/px4_layer/main.cpp +++ b/src/platforms/qurt/px4_layer/main.cpp @@ -50,8 +50,8 @@ using namespace std; -extern void init_app_map(map &apps); -extern void list_builtins(map &apps); +extern void init_app_map(map &apps); +extern void list_builtins(map &apps); static px4_task_t g_dspal_task = -1; __BEGIN_DECLS @@ -61,24 +61,27 @@ extern const char *get_commands(void); void qurt_external_hook(void) __attribute__((weak)); __END_DECLS -static void run_cmd(map &apps, const vector &appargs) { +static void run_cmd(map &apps, const vector &appargs) +{ // command is appargs[0] string command = appargs[0]; + if (apps.find(command) != apps.end()) { - const char *arg[2+1]; + const char *arg[2 + 1]; unsigned int i = 0; + while (i < appargs.size() && appargs[i].c_str()[0] != '\0') { arg[i] = (char *)appargs[i].c_str(); PX4_WARN(" arg = '%s'\n", arg[i]); ++i; } + arg[i] = (char *)0; //PX4_DEBUG_PRINTF(i); - apps[command](i,(char **)arg); - } - else - { + apps[command](i, (char **)arg); + + } else { PX4_WARN("NOT FOUND."); list_builtins(apps); } @@ -88,14 +91,15 @@ void eat_whitespace(const char *&b, int &i) { // Eat whitespace while (b[i] == ' ' || b[i] == '\t') { ++i; } + b = &b[i]; - i=0; + i = 0; } -static void process_commands(map &apps, const char *cmds) +static void process_commands(map &apps, const char *cmds) { vector appargs; - int i=0; + int i = 0; const char *b = cmds; bool found_first_char = false; char arg[256]; @@ -103,7 +107,7 @@ static void process_commands(map &apps, const char *cmds) // Eat leading whitespace eat_whitespace(b, i); - for(;;) { + for (;;) { // End of command line if (b[i] == '\n' || b[i] == '\0') { strncpy(arg, b, i); @@ -112,20 +116,26 @@ static void process_commands(map &apps, const char *cmds) // If we have a command to run if (appargs.size() > 0) { - PX4_WARN("Processing command: %s",appargs[0].c_str()); - for(int ai=1;ai<(int)appargs.size();ai++) - PX4_WARN(" > arg: %s",appargs[ai].c_str()); - run_cmd(apps, appargs); + PX4_WARN("Processing command: %s", appargs[0].c_str()); + + for (int ai = 1; ai < (int)appargs.size(); ai++) { + PX4_WARN(" > arg: %s", appargs[ai].c_str()); + } + + run_cmd(apps, appargs); } + appargs.clear(); + if (b[i] == '\n') { eat_whitespace(b, ++i); continue; - } - else { + + } else { break; } } + // End of arg else if (b[i] == ' ') { strncpy(arg, b, i); @@ -134,11 +144,13 @@ static void process_commands(map &apps, const char *cmds) eat_whitespace(b, ++i); continue; } + ++i; } } -namespace px4 { +namespace px4 +{ extern void init_once(void); }; @@ -147,23 +159,25 @@ int dspal_main(int argc, char *argv[]); __END_DECLS -int dspal_entry( int argc, char* argv[] ) +int dspal_entry(int argc, char *argv[]) { PX4_INFO("In main\n"); - map apps; + map apps; init_app_map(apps); px4::init_once(); px4::init(argc, (char **)argv, "mainapp"); process_commands(apps, get_commands()); sleep(1); // give time for all commands to execute before starting external function - if(qurt_external_hook) - { + + if (qurt_external_hook) { qurt_external_hook(); } - for(;;){ + + for (;;) { sleep(1); } - return 0; + + return 0; } static void usage() @@ -174,32 +188,32 @@ static void usage() extern "C" { -int dspal_main(int argc, char *argv[]) -{ - int ret = 0; - if (argc == 2 && strcmp(argv[1], "start") == 0) { + int dspal_main(int argc, char *argv[]) + { + int ret = 0; + + if (argc == 2 && strcmp(argv[1], "start") == 0) { g_dspal_task = px4_task_spawn_cmd("dspal", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - dspal_entry, - argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + dspal_entry, + argv); - } - else if (argc == 2 && strcmp(argv[1], "stop") == 0) { - if (g_dspal_task < 0) { - PX4_WARN("start up thread not running"); - } - else { - px4_task_delete(g_dspal_task); - g_dspal_task = -1; - } - } - else { - usage(); - ret = -1; - } + } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { + if (g_dspal_task < 0) { + PX4_WARN("start up thread not running"); - return ret; -} + } else { + px4_task_delete(g_dspal_task); + g_dspal_task = -1; + } + + } else { + usage(); + ret = -1; + } + + return ret; + } } diff --git a/src/platforms/qurt/px4_layer/params.c b/src/platforms/qurt/px4_layer/params.c index 3ad35a6a12..654b07df00 100644 --- a/src/platforms/qurt/px4_layer/params.c +++ b/src/platforms/qurt/px4_layer/params.c @@ -39,5 +39,5 @@ /** * @board QuRT_App */ -PARAM_DEFINE_INT32(MAV_TYPE,2); +PARAM_DEFINE_INT32(MAV_TYPE, 2); diff --git a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp index 14ef3304bc..be419d4069 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_impl.cpp @@ -61,11 +61,13 @@ extern uint64_t get_ticks_per_us(); //long PX4_TICKS_PER_SEC = 1000L; -unsigned int sleep(unsigned int sec) -{ - for (unsigned int i=0; i< sec; i++) +unsigned int sleep(unsigned int sec) +{ + for (unsigned int i = 0; i < sec; i++) { usleep(1000000); - return 0; + } + + return 0; } extern void hrt_init(void); @@ -96,16 +98,16 @@ void init_once(void) { // Required for QuRT //_posix_init(); - PX4_WARN( "Before calling work_queue_init" ); + PX4_WARN("Before calling work_queue_init"); // _shell_task_id = pthread_self(); // PX4_INFO("Shell id is %lu", _shell_task_id); work_queues_init(); - PX4_WARN( "Before calling hrt_init" ); + PX4_WARN("Before calling hrt_init"); hrt_work_queue_init(); hrt_init(); - PX4_WARN( "after calling hrt_init" ); + PX4_WARN("after calling hrt_init"); } void init(int argc, char *argv[], const char *app_name) @@ -142,9 +144,11 @@ dm_write( size_t strnlen(const char *s, size_t maxlen) { - size_t i=0; - while (s[i] != '\0' && i < maxlen) + size_t i = 0; + + while (s[i] != '\0' && i < maxlen) { ++i; + } return i; } @@ -154,7 +158,7 @@ int ioctl(int a, int b, unsigned long c) return -1; } -int write(int a, char const* b, int c) +int write(int a, char const *b, int c) { return -1; } diff --git a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp index 0e7fdc209a..4216bbf599 100644 --- a/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp +++ b/src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp @@ -66,8 +66,7 @@ pthread_t _shell_task_id = 0; -struct task_entry -{ +struct task_entry { pthread_t pid; std::string name; bool isused; @@ -76,31 +75,30 @@ struct task_entry static task_entry taskmap[PX4_MAX_TASKS]; -typedef struct -{ +typedef struct { px4_main_t entry; int argc; char *argv[]; - // strings are allocated after the + // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data; + data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); - PX4_WARN( "Before waiting infinte busy loop" ); - //for( ;; ) - //{ - // volatile int x = 0; - // ++x; - // } + PX4_WARN("Before waiting infinte busy loop"); + //for( ;; ) + //{ + // volatile int x = 0; + // ++x; + // } free(ptr); - px4_task_exit(0); + px4_task_exit(0); return NULL; -} +} void px4_systemreset(bool to_bootloader) @@ -108,7 +106,8 @@ px4_systemreset(bool to_bootloader) PX4_WARN("Called px4_system_reset"); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, + char *const argv[]) { int rv; int argc = 0; @@ -116,83 +115,96 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; + char *p = (char *)argv; PX4_DEBUG("Creating %s\n", name); - pthread_t task; + pthread_t task; pthread_attr_t attr; struct sched_param param; // Calculate argc while (p != (char *)0) { p = argv[argc]; - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; - } - structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); - pthdata_t *taskdata; - - // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize+len); - offset = ((unsigned long)taskdata)+structsize; - taskdata->entry = entry; + if (p == (char *)0) { + break; + } + + ++argc; + len += strlen(p) + 1; + } + + structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); + pthdata_t *taskdata; + + // not safe to pass stack data to the thread creation + taskdata = (pthdata_t *)malloc(structsize + len); + offset = ((unsigned long)taskdata) + structsize; + + taskdata->entry = entry; taskdata->argc = argc; - for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); - offset+=strlen(argv[i])+1; + offset += strlen(argv[i]) + 1; } + // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; rv = pthread_attr_init(&attr); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } + #if 0 rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; } + #endif - size_t fixed_stacksize = -1; - pthread_attr_getstacksize(&attr, &fixed_stacksize); - PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size ); - fixed_stacksize = 8 * 1024; - fixed_stacksize = ( fixed_stacksize < (size_t)stack_size )? (size_t)stack_size:fixed_stacksize; + size_t fixed_stacksize = -1; + pthread_attr_getstacksize(&attr, &fixed_stacksize); + PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size); + fixed_stacksize = 8 * 1024; + fixed_stacksize = (fixed_stacksize < (size_t)stack_size) ? (size_t)stack_size : fixed_stacksize; + + PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize); + pthread_attr_setstacksize(&attr, fixed_stacksize); + //pthread_attr_setstacksize(&attr, stack_size); - PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize ); - pthread_attr_setstacksize(&attr, fixed_stacksize); - //pthread_attr_setstacksize(&attr, stack_size); - param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { return (rv < 0) ? rv : -rv; } - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { return -ENOSPC; } - return i; + + return i; } int px4_task_delete(px4_task_t id) @@ -212,15 +226,18 @@ int px4_task_delete(px4_task_t id) pthread_t pid; PX4_WARN("Called px4_task_delete"); - if (id < PX4_MAX_TASKS && taskmap[id].isused) + if (id < PX4_MAX_TASKS && taskmap[id].isused) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { + if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); + } else { rv = pthread_cancel(pid); } @@ -232,20 +249,21 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { - int i; + int i; pthread_t pid = pthread_self(); // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - } - else { + + } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } @@ -258,10 +276,12 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str()); - if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].pid != 0) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel rv = pthread_kill(pid, sig); @@ -275,15 +295,17 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { PX4_INFO(" %-10s %d", taskmap[idx].name.c_str(), taskmap[idx].pid); count++; } } - if (count == 0) + + if (count == 0) { PX4_INFO(" No running tasks"); + } } @@ -297,11 +319,12 @@ int px4_getpid() // return SHELL_TASK_ID; // Get pthread ID from the opaque ID - for (int i=0; i -class HelloExample { +class HelloExample +{ public: HelloExample() {}; diff --git a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp index e4180307ce..56aa52b478 100644 --- a/src/platforms/qurt/tests/hello/hello_start_qurt.cpp +++ b/src/platforms/qurt/tests/hello/hello_start_qurt.cpp @@ -71,11 +71,11 @@ int hello_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); return 0; } diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h index 266b0a2b42..68236eb854 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -37,7 +37,8 @@ #include "uORB/topics/esc_status.h" #include "uORB/topics/vehicle_command.h" -class MuorbTestExample { +class MuorbTestExample +{ public: MuorbTestExample() {}; ~MuorbTestExample() {}; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp index 8f8b7640e2..c6818a9270 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -42,7 +42,7 @@ #include #include "muorb_test_example.h" -extern "C" __EXPORT int muorb_test_entry( int argc, char** argv ); +extern "C" __EXPORT int muorb_test_entry(int argc, char **argv); int muorb_test_entry(int argc, char **argv) { diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp index 6d64cfec8b..ba44169ec3 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -80,15 +80,15 @@ int muorb_test_main(int argc, char *argv[]) /* this is not an error */ return 0; } - - PX4_INFO( "before starting the muorb_test_entry task" ); + + PX4_INFO("before starting the muorb_test_entry task"); daemon_task = px4_task_spawn_cmd("muorb_test", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 8192, - muorb_test_entry, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 8192, + muorb_test_entry, + (char *const *)argv); return 0; } From 480bc2f3c6b11a821087ff24252a48ae2acf35c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:35:17 +0200 Subject: [PATCH 059/155] POSIX Tests: Fix formatting --- .../posix/tests/hello/hello_example.cpp | 5 +- .../posix/tests/hello/hello_example.h | 3 +- .../posix/tests/hello/hello_start_posix.cpp | 12 +- .../posix/tests/hrt_test/hrt_test.cpp | 3 +- src/platforms/posix/tests/hrt_test/hrt_test.h | 3 +- .../tests/hrt_test/hrt_test_start_posix.cpp | 10 +- .../posix/tests/muorb/muorb_test_example.cpp | 229 +++++++++--------- .../posix/tests/muorb/muorb_test_example.h | 13 +- .../posix/tests/muorb/muorb_test_main.cpp | 4 +- .../tests/muorb/muorb_test_start_posix.cpp | 10 +- .../tests/vcdev_test/vcdevtest_example.cpp | 100 +++++--- .../tests/vcdev_test/vcdevtest_example.h | 3 +- .../vcdev_test/vcdevtest_start_posix.cpp | 12 +- .../posix/tests/wqueue/wqueue_start_posix.cpp | 12 +- .../posix/tests/wqueue/wqueue_test.cpp | 10 +- .../posix/tests/wqueue/wqueue_test.h | 9 +- 16 files changed, 244 insertions(+), 194 deletions(-) diff --git a/src/platforms/posix/tests/hello/hello_example.cpp b/src/platforms/posix/tests/hello/hello_example.cpp index a30aeb57bc..5d3cb76852 100644 --- a/src/platforms/posix/tests/hello/hello_example.cpp +++ b/src/platforms/posix/tests/hello/hello_example.cpp @@ -49,8 +49,9 @@ int HelloExample::main() { appState.setRunning(true); - int i=0; - while (!appState.exitRequested() && i<5) { + int i = 0; + + while (!appState.exitRequested() && i < 5) { sleep(2); printf(" Doing work...\n"); diff --git a/src/platforms/posix/tests/hello/hello_example.h b/src/platforms/posix/tests/hello/hello_example.h index a4ae517056..bf589996d1 100644 --- a/src/platforms/posix/tests/hello/hello_example.h +++ b/src/platforms/posix/tests/hello/hello_example.h @@ -41,7 +41,8 @@ #include -class HelloExample { +class HelloExample +{ public: HelloExample() {}; diff --git a/src/platforms/posix/tests/hello/hello_start_posix.cpp b/src/platforms/posix/tests/hello/hello_start_posix.cpp index 8dde729a6e..583baaf56a 100644 --- a/src/platforms/posix/tests/hello/hello_start_posix.cpp +++ b/src/platforms/posix/tests/hello/hello_start_posix.cpp @@ -55,7 +55,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int hello_main(int argc, char *argv[]); int hello_main(int argc, char *argv[]) { - + if (argc < 2) { PX4_WARN("usage: hello {start|stop|status}\n"); return 1; @@ -70,11 +70,11 @@ int hello_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.cpp b/src/platforms/posix/tests/hrt_test/hrt_test.cpp index 8dd1f4bde3..528980cea8 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test.cpp @@ -54,6 +54,7 @@ static void timer_expired(void *arg) { static int i = 0; PX4_INFO("Test\n"); + if (i < 5) { i++; hrt_call_after(&t1, update_interval, timer_expired, (void *)0); @@ -79,7 +80,7 @@ int HRTTest::main() memset(&t1, 0, sizeof(t1)); PX4_INFO("HRT_CALL %d\n", hrt_called(&t1)); - + hrt_call_after(&t1, update_interval, timer_expired, (void *)0); sleep(2); PX4_INFO("HRT_CALL - %d\n", hrt_called(&t1)); diff --git a/src/platforms/posix/tests/hrt_test/hrt_test.h b/src/platforms/posix/tests/hrt_test/hrt_test.h index c4c97be6d2..ac4450e919 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test.h +++ b/src/platforms/posix/tests/hrt_test/hrt_test.h @@ -41,7 +41,8 @@ #include -class HRTTest { +class HRTTest +{ public: HRTTest() {}; diff --git a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp index 2544804496..ed718b2cc4 100644 --- a/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp +++ b/src/platforms/posix/tests/hrt_test/hrt_test_start_posix.cpp @@ -63,11 +63,11 @@ int hrttest_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("hrttest", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.cpp b/src/platforms/posix/tests/muorb/muorb_test_example.cpp index 3bae353f2f..4547c6db57 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_example.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_example.cpp @@ -57,127 +57,128 @@ int MuorbTestExample::main() int MuorbTestExample::DefaultTest() { - int i=0; - orb_advert_t pub_id = orb_advertise( ORB_ID( esc_status ), & m_esc_status ); - if( pub_id == 0 ) - { - PX4_ERR( "error publishing esc_status" ); - return -1; - } - orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc ); - if( pub_id_vc == 0 ) - { - PX4_ERR( "error publishing vehicle_command" ); - return -1; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - return -1; - } - int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) ); - if ( sub_vc == PX4_ERROR ) - { - PX4_ERR( "Error subscribing to vehicle_command topic" ); - return -1; - } + int i = 0; + orb_advert_t pub_id = orb_advertise(ORB_ID(esc_status), & m_esc_status); - while (!appState.exitRequested() && i<100) { + if (pub_id == 0) { + PX4_ERR("error publishing esc_status"); + return -1; + } - PX4_DEBUG("[%d] Doing work...", i ); - if( orb_publish( ORB_ID( esc_status ), pub_id, &m_esc_status ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the esc status message for iter", i ); - break; - } - bool updated = false; - if( orb_check( sub_vc, &updated ) == 0 ) - { - if( updated ) - { - PX4_DEBUG( "[%d]Vechicle Status is updated... reading new value", i ); - if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 ) - { - PX4_ERR( "[%d]Error calling orb copy for vechivle status... ", i ); - break; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - break; - } - } - else - { - PX4_DEBUG( "[%d] VC topic is not updated", i ); - } - } - else - { - PX4_ERR( "[%d]Error checking the updated status for vechile command... ", i ); - break; - } - - ++i; - } - return 0; + orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); + + if (pub_id_vc == 0) { + PX4_ERR("error publishing vehicle_command"); + return -1; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + return -1; + } + + int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); + + if (sub_vc == PX4_ERROR) { + PX4_ERR("Error subscribing to vehicle_command topic"); + return -1; + } + + while (!appState.exitRequested() && i < 100) { + + PX4_DEBUG("[%d] Doing work...", i); + + if (orb_publish(ORB_ID(esc_status), pub_id, &m_esc_status) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the esc status message for iter", i); + break; + } + + bool updated = false; + + if (orb_check(sub_vc, &updated) == 0) { + if (updated) { + PX4_DEBUG("[%d]Vechicle Status is updated... reading new value", i); + + if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) { + PX4_ERR("[%d]Error calling orb copy for vechivle status... ", i); + break; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_DEBUG("[%d] VC topic is not updated", i); + } + + } else { + PX4_ERR("[%d]Error checking the updated status for vechile command... ", i); + break; + } + + ++i; + } + + return 0; } int MuorbTestExample::PingPongTest() { - int i=0; - orb_advert_t pub_id_vc = orb_advertise( ORB_ID( vehicle_command ), & m_vc ); - if( pub_id_vc == 0 ) - { - PX4_ERR( "error publishing vehicle_command" ); - return -1; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - return -1; - } - int sub_esc_status = orb_subscribe( ORB_ID( esc_status ) ); - if ( sub_esc_status == PX4_ERROR ) - { - PX4_ERR( "Error subscribing to esc_status topic" ); - return -1; - } + int i = 0; + orb_advert_t pub_id_vc = orb_advertise(ORB_ID(vehicle_command), & m_vc); - while (!appState.exitRequested() ) { + if (pub_id_vc == 0) { + PX4_ERR("error publishing vehicle_command"); + return -1; + } - PX4_INFO("[%d] Doing work...", i ); - bool updated = false; - if( orb_check( sub_esc_status, &updated ) == 0 ) - { - if( updated ) - { - PX4_INFO( "[%d]ESC status is updated... reading new value", i ); - if( orb_copy( ORB_ID( esc_status ), sub_esc_status, &m_esc_status ) != 0 ) - { - PX4_ERR( "[%d]Error calling orb copy for esc status... ", i ); - break; - } - if( orb_publish( ORB_ID( vehicle_command ), pub_id_vc, &m_vc ) == PX4_ERROR ) - { - PX4_ERR( "[%d]Error publishing the vechile command message", i ); - break; - } - } - else - { - PX4_INFO( "[%d] esc status topic is not updated", i ); - } - } - else - { - PX4_ERR( "[%d]Error checking the updated status for esc status... ", i ); - break; - } - // sleep for 1 sec. - usleep( 1000000 ); + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + return -1; + } - ++i; - } - return 0; + int sub_esc_status = orb_subscribe(ORB_ID(esc_status)); + + if (sub_esc_status == PX4_ERROR) { + PX4_ERR("Error subscribing to esc_status topic"); + return -1; + } + + while (!appState.exitRequested()) { + + PX4_INFO("[%d] Doing work...", i); + bool updated = false; + + if (orb_check(sub_esc_status, &updated) == 0) { + if (updated) { + PX4_INFO("[%d]ESC status is updated... reading new value", i); + + if (orb_copy(ORB_ID(esc_status), sub_esc_status, &m_esc_status) != 0) { + PX4_ERR("[%d]Error calling orb copy for esc status... ", i); + break; + } + + if (orb_publish(ORB_ID(vehicle_command), pub_id_vc, &m_vc) == PX4_ERROR) { + PX4_ERR("[%d]Error publishing the vechile command message", i); + break; + } + + } else { + PX4_INFO("[%d] esc status topic is not updated", i); + } + + } else { + PX4_ERR("[%d]Error checking the updated status for esc status... ", i); + break; + } + + // sleep for 1 sec. + usleep(1000000); + + ++i; + } + + return 0; } diff --git a/src/platforms/posix/tests/muorb/muorb_test_example.h b/src/platforms/posix/tests/muorb/muorb_test_example.h index c5d699ae7d..4f4dcf02d6 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_example.h +++ b/src/platforms/posix/tests/muorb/muorb_test_example.h @@ -43,7 +43,8 @@ #include "uORB/topics/esc_status.h" #include "uORB/topics/vehicle_command.h" -class MuorbTestExample { +class MuorbTestExample +{ public: MuorbTestExample() {}; @@ -53,9 +54,9 @@ public: static px4::AppState appState; /* track requests to terminate app */ private: - int DefaultTest(); - int PingPongTest(); - struct esc_status_s m_esc_status; - struct vehicle_command_s m_vc; - + int DefaultTest(); + int PingPongTest(); + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + }; diff --git a/src/platforms/posix/tests/muorb/muorb_test_main.cpp b/src/platforms/posix/tests/muorb/muorb_test_main.cpp index effa9ff88b..d9e9d1c6a9 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_main.cpp @@ -43,14 +43,14 @@ #include "muorb_test_example.h" #include #include "uORB/uORBManager.hpp" -#include "uORBKraitFastRpcChannel.hpp" +#include "uORBKraitFastRpcChannel.hpp" int PX4_MAIN(int argc, char **argv) { px4::init(argc, argv, "muorb_test"); PX4_DEBUG("muorb_test"); - + MuorbTestExample hello; hello.main(); diff --git a/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp index 943605f531..20227ba54e 100644 --- a/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp +++ b/src/platforms/posix/tests/muorb/muorb_test_start_posix.cpp @@ -71,11 +71,11 @@ int muorb_test_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("muorb_test", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char* const*)argv); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 16000, + PX4_MAIN, + (char *const *)argv); return 0; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp index ec709cc0ef..1276b740a2 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.cpp @@ -60,44 +60,50 @@ static int writer_main(int argc, char *argv[]) char buf[1]; int fd = px4_open(TESTDEV, PX4_F_WRONLY); + if (fd < 0) { PX4_INFO("Writer: Open failed %d %d", fd, px4_errno); return -px4_errno; } int ret; - int i=0; + int i = 0; + while (!g_exit) { // Wait for 2 seconds PX4_INFO("Writer: Sleeping for 2 sec"); ret = sleep(2); + if (ret < 0) { PX4_INFO("Writer: sleep failed %d %d", ret, errno); return ret; } - buf[0] = 'A'+(char)(i % 26); + buf[0] = 'A' + (char)(i % 26); PX4_INFO("Writer: writing char '%c'", buf[0]); ret = px4_write(fd, buf, 1); - ++i; + ++i; } + px4_close(fd); PX4_INFO("Writer: stopped"); return ret; } -class PrivData { +class PrivData +{ public: PrivData() : _read_offset(0) {} ~PrivData() {} - + size_t _read_offset; }; - -class VCDevNode : public VDev { + +class VCDevNode : public VDev +{ public: - VCDevNode() : - VDev("vcdevtest", TESTDEV), + VCDevNode() : + VDev("vcdevtest", TESTDEV), _is_open_for_write(false), _write_offset(0) {}; @@ -120,22 +126,25 @@ int VCDevNode::open(device::file_t *handlep) errno = EBUSY; return -1; } + int ret = VDev::open(handlep); + if (ret != 0) { return ret; } + handlep->priv = new PrivData; if (_is_open_for_write && (handlep->flags & PX4_F_WRONLY)) { _is_open_for_write = true; } - + return 0; } int VCDevNode::close(device::file_t *handlep) { - delete (PrivData *)handlep->priv; + delete(PrivData *)handlep->priv; handlep->priv = nullptr; VDev::close(handlep); @@ -143,12 +152,13 @@ int VCDevNode::close(device::file_t *handlep) if ((handlep->flags & PX4_F_WRONLY) && _is_open_for_write) { _is_open_for_write = false; } + return 0; } ssize_t VCDevNode::write(device::file_t *handlep, const char *buffer, size_t buflen) { - for (size_t i=0; ipriv; ssize_t chars_read = 0; PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset); - for (size_t i=0; i_read_offset < _write_offset); i++) { + + for (size_t i = 0; i < buflen && (p->_read_offset < _write_offset); i++) { buffer[i] = _buf[p->_read_offset]; p->_read_offset++; chars_read++; } - + return chars_read; } -VCDevExample::~VCDevExample() { +VCDevExample::~VCDevExample() +{ if (_node) { delete _node; _node = 0; @@ -183,16 +195,19 @@ VCDevExample::~VCDevExample() { static int test_pub_block(int fd, unsigned long blocked) { int ret = px4_ioctl(fd, DEVIOCSPUBBLOCK, blocked); + if (ret < 0) { PX4_INFO("ioctl PX4_DEVIOCSPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } ret = px4_ioctl(fd, DEVIOCGPUBBLOCK, 0); + if (ret < 0) { PX4_INFO("ioctl PX4_DEVIOCGPUBBLOCK failed %d %d", ret, px4_errno); return -px4_errno; } + PX4_INFO("pub_blocked = %d %s", ret, (unsigned long)ret == blocked ? "PASS" : "FAIL"); return 0; @@ -214,32 +229,39 @@ int VCDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after // Test indefinte blocking poll while ((!appState.exitRequested()) && (loop_count < iterations)) { pollret = px4_poll(fds, 1, timeout); + if (pollret < 0) { PX4_ERR("Reader: px4_poll failed %d %d FAIL", pollret, px4_errno); goto fail; - } + } + PX4_INFO("Reader: px4_poll returned %d", pollret); + if (pollret) { readret = px4_read(fd, readbuf, 10); + if (readret != 1) { if (mustblock) { PX4_ERR("Reader: read failed %d FAIL", readret); goto fail; - } - else { + + } else { PX4_INFO("Reader: read failed %d FAIL", readret); } - } - else { + + } else { readbuf[readret] = '\0'; PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf); } } + if (delayms_after_poll) { - usleep(delayms_after_poll*1000); + usleep(delayms_after_poll * 1000); } + loop_count++; } + return 0; fail: return 1; @@ -269,47 +291,61 @@ int VCDevExample::main() void *p = 0; int ret = px4_ioctl(fd, DIOC_GETPRIV, (unsigned long)&p); + if (ret < 0) { PX4_INFO("ioctl DIOC_GETPRIV failed %d %d", ret, px4_errno); return -px4_errno; } + PX4_INFO("priv data = %p %s", p, p == (void *)_node ? "PASS" : "FAIL"); ret = test_pub_block(fd, 1); - if (ret < 0) + + if (ret < 0) { return ret; + } + ret = test_pub_block(fd, 0); - if (ret < 0) + + if (ret < 0) { return ret; + } // Start a task that will write something in 4 seconds - (void)px4_task_spawn_cmd("writer", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 6, - 2000, - writer_main, - (char* const*)NULL); + (void)px4_task_spawn_cmd("writer", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 6, + 2000, + writer_main, + (char *const *)NULL); ret = 0; PX4_INFO("TEST: BLOCKING POLL ---------------"); + if (do_poll(fd, -1, 3, 0)) { ret = 1; goto fail2; } + PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - if(do_poll(fd, 0, 3, 0)) { + + if (do_poll(fd, 0, 3, 0)) { ret = 1; goto fail2; } + PX4_INFO("TEST: 100ms TIMEOUT POLL -----------"); - if(do_poll(fd, 0, 30, 100)) { + + if (do_poll(fd, 0, 30, 100)) { ret = 1; goto fail2; } + PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); - if(do_poll(fd, 1000, 3, 0)) { + + if (do_poll(fd, 1000, 3, 0)) { ret = 1; goto fail2; } diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h index 10befc795c..a1cc325f00 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_example.h @@ -43,7 +43,8 @@ class VCDevNode; -class VCDevExample { +class VCDevExample +{ public: VCDevExample() : _node(0) {}; diff --git a/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp index 5ed9269b2d..135ca8f3f7 100644 --- a/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp +++ b/src/platforms/posix/tests/vcdev_test/vcdevtest_start_posix.cpp @@ -50,7 +50,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int vcdevtest_main(int argc, char *argv[]); int vcdevtest_main(int argc, char *argv[]) { - + if (argc < 2) { printf("usage: vcdevtest {start|stop|status}\n"); return 1; @@ -65,11 +65,11 @@ int vcdevtest_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("vcdevtest", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp index 2479020097..d4652f8e62 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_start_posix.cpp @@ -52,7 +52,7 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int wqueue_test_main(int argc, char *argv[]); int wqueue_test_main(int argc, char *argv[]) { - + if (argc < 2) { PX4_INFO("usage: wqueue_test {start|stop|status}\n"); return 1; @@ -67,11 +67,11 @@ int wqueue_test_main(int argc, char *argv[]) } daemon_task = px4_task_spawn_cmd("wqueue", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - PX4_MAIN, - (argv) ? (char* const*)&argv[2] : (char* const*)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + PX4_MAIN, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); return 0; } diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.cpp b/src/platforms/posix/tests/wqueue/wqueue_test.cpp index 6c9ececc14..aec10269b8 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.cpp +++ b/src/platforms/posix/tests/wqueue/wqueue_test.cpp @@ -65,8 +65,11 @@ void WQueueTest::do_lp_work() { static int iter = 0; printf("done lp work\n"); - if (iter > 5) + + if (iter > 5) { _lpwork_done = true; + } + ++iter; work_queue(LPWORK, &_lpwork, (worker_t)&lp_worker_cb, this, 1000); @@ -76,8 +79,11 @@ void WQueueTest::do_hp_work() { static int iter = 0; printf("done hp work\n"); - if (iter > 5) + + if (iter > 5) { _hpwork_done = true; + } + ++iter; // requeue diff --git a/src/platforms/posix/tests/wqueue/wqueue_test.h b/src/platforms/posix/tests/wqueue/wqueue_test.h index 6db3fc1e25..56e66a0b1b 100644 --- a/src/platforms/posix/tests/wqueue/wqueue_test.h +++ b/src/platforms/posix/tests/wqueue/wqueue_test.h @@ -43,11 +43,12 @@ #include #include -class WQueueTest { +class WQueueTest +{ public: - WQueueTest() : - _lpwork_done(false), - _hpwork_done(false) + WQueueTest() : + _lpwork_done(false), + _hpwork_done(false) { memset(&_lpwork, 0, sizeof(_lpwork)); memset(&_hpwork, 0, sizeof(_hpwork)); From 0b7f6902a91e410cf1e8661fbac12b64631522f2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:35:32 +0200 Subject: [PATCH 060/155] POSIX Sim drivers: Fix formatting --- .../posix/drivers/accelsim/accelsim.cpp | 225 +++++++------ src/platforms/posix/drivers/adcsim/adcsim.cpp | 20 +- .../posix/drivers/airspeedsim/airspeedsim.cpp | 67 ++-- .../posix/drivers/airspeedsim/airspeedsim.h | 2 +- .../drivers/airspeedsim/meas_airspeed_sim.cpp | 6 +- src/platforms/posix/drivers/barosim/baro.cpp | 102 ++++-- .../posix/drivers/barosim/baro_sim.cpp | 25 +- src/platforms/posix/drivers/gpssim/gpssim.cpp | 45 +-- .../posix/drivers/gyrosim/gyrosim.cpp | 318 +++++++++++------- .../posix/drivers/tonealrmsim/tone_alarm.cpp | 179 +++++++--- 10 files changed, 617 insertions(+), 372 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index fa0a256605..1f9fa0da05 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -99,7 +99,7 @@ class ACCELSIM_mag; class ACCELSIM : public device::VDev { public: - ACCELSIM(const char* path, enum Rotation rotation); + ACCELSIM(const char *path, enum Rotation rotation); virtual ~ACCELSIM(); virtual int init(); @@ -310,8 +310,8 @@ private: int mag_set_samplerate(unsigned frequency); /* this class cannot be copied */ - ACCELSIM(const ACCELSIM&); - ACCELSIM operator=(const ACCELSIM&); + ACCELSIM(const ACCELSIM &); + ACCELSIM operator=(const ACCELSIM &); }; /* @@ -350,12 +350,12 @@ private: void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ - ACCELSIM_mag(const ACCELSIM_mag&); - ACCELSIM_mag operator=(const ACCELSIM_mag&); + ACCELSIM_mag(const ACCELSIM_mag &); + ACCELSIM_mag operator=(const ACCELSIM_mag &); }; -ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : +ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : VDev("ACCELSIM", path), _mag(new ACCELSIM_mag(this)), _accel_call{}, @@ -397,7 +397,7 @@ ACCELSIM::ACCELSIM(const char* path, enum Rotation rotation) : _debug_enabled = false; _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; - + /* Prime _mag with parents devid. */ _mag->_device_id.devid = _device_id.devid; _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; @@ -425,13 +425,17 @@ ACCELSIM::~ACCELSIM() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_mag_reports != nullptr) - delete _mag_reports; + } - if (_accel_class_instance != -1) + if (_mag_reports != nullptr) { + delete _mag_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } delete _mag; @@ -460,18 +464,21 @@ ACCELSIM::init() /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); - if (_accel_reports == nullptr) + if (_accel_reports == nullptr) { goto out; + } _mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report)); - if (_mag_reports == nullptr) + if (_mag_reports == nullptr) { goto out; + } reset(); /* do VDev init for the mag device node */ ret = _mag->init(); + if (ret != OK) { PX4_WARN("MAG init failed"); goto out; @@ -485,7 +492,7 @@ ACCELSIM::init() /* measurement will have generated a report, publish */ _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, - &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -499,7 +506,7 @@ ACCELSIM::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_DEFAULT); + &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { PX4_WARN("ADVERT ERR"); @@ -522,16 +529,20 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd & DIR_READ) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); - if (sim == NULL) + + if (sim == NULL) { return ENODEV; + } // FIXME - not sure what interrupt status should be recv[1] = 0; + // skip cmd and status bytes if (cmd & ACC_READ) { - sim->getRawAccelReport(&recv[2], len-2); + sim->getRawAccelReport(&recv[2], len - 2); + } else if (cmd & MAG_READ) { - sim->getMagReport(&recv[2], len-2); + sim->getMagReport(&recv[2], len - 2); } } @@ -546,8 +557,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_accel_interval > 0) { @@ -569,8 +581,9 @@ ACCELSIM::read(device::file_t *filp, char *buffer, size_t buflen) measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) + if (_accel_reports->get(arb)) { ret = sizeof(*arb); + } return ret; } @@ -583,8 +596,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_call_mag_interval > 0) { @@ -608,8 +622,9 @@ ACCELSIM::mag_read(device::file_t *filp, char *buffer, size_t buflen) _mag->measure(); /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) + if (_mag_reports->get(mrb)) { ret = sizeof(*mrb); + } return ret; } @@ -620,7 +635,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -642,52 +657,56 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { - /* do we need to start internal polling? */ - bool want_start = (_call_accel_interval == 0); + /* do we need to start internal polling? */ + bool want_start = (_call_accel_interval == 0); - /* convert hz to hrt interval via microseconds */ - unsigned period = 1000000 / arg; + /* convert hz to hrt interval via microseconds */ + unsigned period = 1000000 / arg; - /* check against maximum sane rate */ - if (period < 500) - return -EINVAL; + /* check against maximum sane rate */ + if (period < 500) { + return -EINVAL; + } - /* adjust filters */ - accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); + /* adjust filters */ + accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq()); - /* update interval for next measurement */ - /* XXX this is a bit shady, but no other way to adjust... */ - _accel_call.period = _call_accel_interval = period; + /* update interval for next measurement */ + /* XXX this is a bit shady, but no other way to adjust... */ + _accel_call.period = _call_accel_interval = period; - /* if we need to start the poll state machine, do it */ - if (want_start) - start(); + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } - return OK; + return OK; + } } } - } case SENSORIOCGPOLLRATE: - if (_call_accel_interval == 0) + if (_call_accel_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_accel_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -702,20 +721,22 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return _accel_samplerate; case ACCELIOCSLOWPASS: { - return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); - } + return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg); + } case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_scale *s = (struct accel_scale *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - } else { - return -EINVAL; + /* copy scale, but only if off by a few percent */ + struct accel_scale *s = (struct accel_scale *) arg; + float sum = s->x_scale + s->y_scale + s->z_scale; + + if (sum > 2.0f && sum < 4.0f) { + memcpy(&_accel_scale, s, sizeof(_accel_scale)); + return OK; + + } else { + return -EINVAL; + } } - } case ACCELIOCSRANGE: /* arg needs to be in G */ @@ -723,7 +744,7 @@ ACCELSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/ACCELSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / ACCELSIM_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -745,7 +766,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCSPOLLRATE: { - switch (arg) { + switch (arg) { /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: @@ -775,8 +796,9 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned period = 1000000 / arg; /* check against maximum sane rate (1ms) */ - if (period < 10000) + if (period < 10000) { return -EINVAL; + } /* update interval for next measurement */ /* XXX this is a bit shady, but no other way to adjust... */ @@ -785,8 +807,9 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) //PX4_INFO("SET _call_mag_interval=%u", _call_mag_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -794,23 +817,25 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_mag_interval == 0) + if (_call_mag_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_mag_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_mag_reports->resize(arg)) { - return -ENOMEM; + if (!_mag_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _mag_reports->size(); @@ -854,6 +879,7 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return OK; + default: /* give it to the superclass */ return VDev::ioctl(filp, cmd, arg); @@ -888,7 +914,8 @@ void ACCELSIM::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->mag_ioctl(filp, cmd, arg); } } @@ -1311,8 +1342,9 @@ int start(enum Rotation rotation) { int fd, fd_mag; + if (g_dev != nullptr) { - PX4_WARN( "already started"); + PX4_WARN("already started"); return 0; } @@ -1339,7 +1371,7 @@ start(enum Rotation rotation) if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - px4_close(fd); + px4_close(fd); goto fail; } @@ -1350,14 +1382,13 @@ start(enum Rotation rotation) if (px4_ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - } - else - { + + } else { PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); } - px4_close(fd); - px4_close(fd_mag); + px4_close(fd); + px4_close(fd_mag); return 0; fail: @@ -1405,7 +1436,7 @@ accelsim_main(int argc, char *argv[]) enum Rotation rotation = ROTATION_NONE; int ret; int myoptind = 1; - const char * myoptarg = NULL; + const char *myoptarg = NULL; /* jump over start/off/etc and look at options first */ while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { @@ -1413,6 +1444,7 @@ accelsim_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(myoptarg); break; + default: accelsim::usage(); return 0; @@ -1424,18 +1456,21 @@ accelsim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = accelsim::start(rotation); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = accelsim::info(); + } else { accelsim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/adcsim/adcsim.cpp b/src/platforms/posix/drivers/adcsim/adcsim.cpp index b7d4071111..d9f0dd4f15 100644 --- a/src/platforms/posix/drivers/adcsim/adcsim.cpp +++ b/src/platforms/posix/drivers/adcsim/adcsim.cpp @@ -37,7 +37,7 @@ * Driver for the ADCSIM. * * This is a designed for simulating sampling things like voltages - * and so forth. + * and so forth. */ #include @@ -83,7 +83,7 @@ protected: private: static const hrt_abstime _tickrate = 10000; /**< 100Hz base rate */ - + hrt_call _call; perf_counter_t _sample_perf; @@ -126,11 +126,13 @@ ADCSIM::ADCSIM(uint32_t channels) : _channel_count++; } } + _samples = new adc_msg_s[_channel_count]; /* prefill the channel numbers in the sample array */ if (_samples != nullptr) { unsigned index = 0; + for (unsigned i = 0; i < 32; i++) { if (channels & (1 << i)) { _samples[index].am_channel = i; @@ -143,8 +145,9 @@ ADCSIM::ADCSIM(uint32_t channels) : ADCSIM::~ADCSIM() { - if (_samples != nullptr) + if (_samples != nullptr) { delete _samples; + } } int @@ -167,8 +170,9 @@ ADCSIM::read(device::file_t *filp, char *buffer, size_t len) { const size_t maxsize = sizeof(adc_msg_s) * _channel_count; - if (len > maxsize) + if (len > maxsize) { len = maxsize; + } /* block interrupts while copying samples to avoid racing with an update */ memcpy(buffer, _samples, len); @@ -205,8 +209,10 @@ void ADCSIM::_tick() { /* scan the channel set and sample each */ - for (unsigned i = 0; i < _channel_count; i++) + for (unsigned i = 0; i < _channel_count; i++) { _samples[i].am_data = _sample(_samples[i].am_channel); + } + update_system_power(); } @@ -240,6 +246,7 @@ test(void) { int fd = px4_open(ADCSIM0_DEVICE_PATH, O_RDONLY); + if (fd < 0) { PX4_ERR("can't open ADCSIM device"); return 1; @@ -290,8 +297,9 @@ adcsim_main(int argc, char *argv[]) } if (argc > 1) { - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { ret = test(); + } } return ret; diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp index 9cb0ac73d2..58f6aa9929 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.cpp @@ -71,7 +71,7 @@ #include "airspeedsim.h" -AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char* path) : +AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path) : VDev("AIRSPEEDSIM", path), _reports(nullptr), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), @@ -101,12 +101,14 @@ AirspeedSim::~AirspeedSim() /* make sure we are truly inactive */ stop(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -127,6 +129,7 @@ AirspeedSim::init() /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); + if (_reports == nullptr) { goto out; } @@ -145,8 +148,9 @@ AirspeedSim::init() /* measurement will have generated a report, publish */ _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - if (_airspeed_pub == nullptr) + if (_airspeed_pub == nullptr) { PX4_WARN("uORB started?"); + } } ret = OK; @@ -165,7 +169,7 @@ AirspeedSim::probe() _retries = 4; int ret = measure(); - // drop back to 2 retries once initialised + // drop back to 2 retries once initialised _retries = 2; return ret; } @@ -178,20 +182,20 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _measure_ticks = 0; return OK; - /* external signalling (DRDY) not supported */ + /* external signalling (DRDY) not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -201,13 +205,14 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(_conversion_interval); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -216,15 +221,17 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) + if (ticks < USEC2TICK(_conversion_interval)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -232,21 +239,24 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) + if ((arg < 1) || (arg > 100)) { return -EINVAL; + } //irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { //irqrestore(flags); return -ENOMEM; } + //irqrestore(flags); return OK; @@ -260,16 +270,16 @@ AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg) return -EINVAL; case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - _diff_pres_offset = s->offset_pa; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + _diff_pres_offset = s->offset_pa; + return OK; } case AIRSPEEDIOCGSCALE: { - struct airspeed_scale *s = (struct airspeed_scale*)arg; - s->offset_pa = _diff_pres_offset; - s->scale = 1.0f; - return OK; + struct airspeed_scale *s = (struct airspeed_scale *)arg; + s->offset_pa = _diff_pres_offset; + s->scale = 1.0f; + return OK; } default: @@ -287,8 +297,9 @@ AirspeedSim::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -369,6 +380,7 @@ AirspeedSim::update_status() if (_subsys_pub != nullptr) { orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); } @@ -402,21 +414,26 @@ AirspeedSim::print_info() void AirspeedSim::new_report(const differential_pressure_s &report) { - if (!_reports->force(&report)) + if (!_reports->force(&report)) { perf_count(_buffer_overflows); + } } int -AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { +AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) +{ if (recv_len > 0) { // this is equivalent to the collect phase Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getAirspeedSample(recv, recv_len); + } else { // we don't need measure phase } diff --git a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h index 2c668fc97f..91db618d9b 100644 --- a/src/platforms/posix/drivers/airspeedsim/airspeedsim.h +++ b/src/platforms/posix/drivers/airspeedsim/airspeedsim.h @@ -127,7 +127,7 @@ protected: virtual int collect() = 0; virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); + uint8_t *recv, unsigned recv_len); /** * Update the subsystem status diff --git a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp index 3474b0bcaa..89691f7483 100644 --- a/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp +++ b/src/platforms/posix/drivers/airspeedsim/meas_airspeed_sim.cpp @@ -153,12 +153,12 @@ MEASAirspeedSim::collect() int ret = -EIO; /* read from the sensor */ - #pragma pack(push, 1) +#pragma pack(push, 1) struct { float temperature; float diff_pressure; } airspeed_report; - #pragma pack(pop) +#pragma pack(pop) perf_begin(_sample_perf); @@ -458,7 +458,7 @@ test() if (fd < 0) { PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525); - return 1; + return 1; } /* do a simple demand read */ diff --git a/src/platforms/posix/drivers/barosim/baro.cpp b/src/platforms/posix/drivers/barosim/baro.cpp index f69363fd40..975fa3687c 100644 --- a/src/platforms/posix/drivers/barosim/baro.cpp +++ b/src/platforms/posix/drivers/barosim/baro.cpp @@ -89,7 +89,7 @@ enum BAROSIM_BUS { class BAROSIM : public device::VDev { public: - BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path); + BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path); ~BAROSIM(); virtual int init(); @@ -195,7 +195,7 @@ protected: */ extern "C" __EXPORT int barosim_main(int argc, char *argv[]); -BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char* path) : +BAROSIM::BAROSIM(device::Device *interface, barosim::prom_u &prom_buf, const char *path) : VDev("BAROSIM", path), _interface(interface), _prom(prom_buf.s), @@ -224,12 +224,14 @@ BAROSIM::~BAROSIM() /* make sure we are truly inactive */ stop_cycle(); - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(get_devname(), _class_instance); + } /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } // free perf counters perf_free(_sample_perf); @@ -247,6 +249,7 @@ BAROSIM::init() DEVICE_DEBUG("BAROSIM::init"); ret = VDev::init(); + if (ret != OK) { DEVICE_DEBUG("VDev init failed"); goto out; @@ -270,7 +273,7 @@ BAROSIM::init() _reports->flush(); _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); if (_baro_topic == nullptr) { PX4_ERR("failed to create sensor_baro publication"); @@ -329,8 +332,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) int ret = 0; /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is enabled */ if (_measure_ticks > 0) { @@ -383,8 +387,9 @@ BAROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* state machine will have generated a report, copy it out */ - if (_reports->get(brp)) + if (_reports->get(brp)) { ret = sizeof(*brp); + } } while (0); @@ -396,23 +401,23 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case SENSORIOCSPOLLRATE: + case SENSORIOCSPOLLRATE: switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop_cycle(); _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: { /* do we need to start internal polling? */ @@ -422,13 +427,14 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) _measure_ticks = USEC2TICK(BAROSIM_CONVERSION_INTERVAL); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* do we need to start internal polling? */ bool want_start = (_measure_ticks == 0); @@ -437,36 +443,41 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) unsigned long ticks = USEC2TICK(1000000 / arg); /* check against maximum rate */ - if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) + if (ticks < USEC2TICK(BAROSIM_CONVERSION_INTERVAL)) { return -EINVAL; + } /* update interval for next measurement */ _measure_ticks = ticks; /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start_cycle(); + } return OK; } } case SENSORIOCGPOLLRATE: - if (_measure_ticks == 0) + if (_measure_ticks == 0) { return SENSOR_POLLRATE_MANUAL; + } return (1000 / _measure_ticks); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_reports->resize(arg)) { - return -ENOMEM; + if (!_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -481,8 +492,9 @@ BAROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case BAROIOCSMSLPRESSURE: /* range-check for sanity */ - if ((arg < 80000) || (arg > 120000)) + if ((arg < 80000) || (arg > 120000)) { return -EINVAL; + } _msl_pressure = arg; return OK; @@ -537,6 +549,7 @@ BAROSIM::cycle() /* perform collection */ ret = collect(); + if (ret != OK) { /* issue a reset command to the sensor */ _interface->dev_ioctl(IOCTL_RESET, dummy); @@ -569,6 +582,7 @@ BAROSIM::cycle() /* measurement phase */ ret = measure(); + if (ret != OK) { //DEVICE_LOG("measure error %d", ret); /* issue a reset command to the sensor */ @@ -605,8 +619,10 @@ BAROSIM::measure() * Send the command to begin measuring. */ ret = _interface->dev_ioctl(IOCTL_MEASURE, addr); - if (OK != ret) + + if (OK != ret) { perf_count(_comms_errors); + } perf_end(_measure_perf); @@ -631,10 +647,11 @@ BAROSIM::collect() struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->dev_read(0, (void *)&baro_report, sizeof(baro_report)); + if (ret < 0) { perf_count(_comms_errors); perf_end(_sample_perf); @@ -647,6 +664,7 @@ BAROSIM::collect() report.altitude = baro_report.altitude; report.temperature = baro_report.temperature; report.timestamp = hrt_absolute_time(); + } else { report.pressure = baro_report.pressure; report.altitude = baro_report.altitude; @@ -658,6 +676,7 @@ BAROSIM::collect() if (_baro_topic != nullptr) { /* publish it */ orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); + } else { PX4_WARN("BAROSIM::collect _baro_topic not initialized"); } @@ -792,6 +811,7 @@ start_bus(struct barosim_bus_option &bus) prom_u prom_buf; device::Device *interface = bus.interface_constructor(prom_buf, bus.busnum); + if (interface->init() != OK) { delete interface; PX4_ERR("no device on bus %u", (unsigned)bus.busid); @@ -799,13 +819,14 @@ start_bus(struct barosim_bus_option &bus) } bus.dev = new BAROSIM(interface, prom_buf, bus.devpath); + if (bus.dev != nullptr && OK != bus.dev->init()) { delete bus.dev; bus.dev = NULL; PX4_ERR("bus init failed %p", bus.dev); return false; } - + int fd = px4_open(bus.devpath, O_RDONLY); /* set the poll rate to default, starts automatic data collection */ @@ -813,6 +834,7 @@ start_bus(struct barosim_bus_option &bus) PX4_ERR("can't open baro device"); return false; } + if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); PX4_ERR("failed setting default poll rate"); @@ -863,6 +885,7 @@ test() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("open failed (try 'barosim start' if the driver is not running)"); return 1; @@ -940,6 +963,7 @@ reset() int fd; fd = px4_open(bus.devpath, O_RDONLY); + if (fd < 0) { PX4_ERR("failed "); return 1; @@ -954,6 +978,7 @@ reset() PX4_ERR("driver poll restart failed"); return 1; } + return 0; } @@ -963,13 +988,15 @@ reset() int info() { - for (uint8_t i=0; iprint_info(); } } + return 0; } @@ -1078,26 +1105,30 @@ barosim_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ret = barosim::start(); + } /* * Test the driver/device. */ - else if (!strcmp(verb, "test")) + else if (!strcmp(verb, "test")) { ret = barosim::test(); + } /* * Reset the driver. */ - else if (!strcmp(verb, "reset")) + else if (!strcmp(verb, "reset")) { ret = barosim::reset(); + } /* * Print driver information. */ - else if (!strcmp(verb, "info")) + else if (!strcmp(verb, "info")) { ret = barosim::info(); + } /* * Perform MSL pressure calibration given an altitude in metres @@ -1112,10 +1143,11 @@ barosim_main(int argc, char *argv[]) long altitude = strtol(argv[2], nullptr, 10); ret = barosim::calibrate(altitude); - } - else { + + } else { barosim::usage(); return 1; } + return ret; } diff --git a/src/platforms/posix/drivers/barosim/baro_sim.cpp b/src/platforms/posix/drivers/barosim/baro_sim.cpp index 8e66dc3ac6..252d1428c9 100644 --- a/src/platforms/posix/drivers/barosim/baro_sim.cpp +++ b/src/platforms/posix/drivers/barosim/baro_sim.cpp @@ -31,11 +31,11 @@ * ****************************************************************************/ - /** - * @file baro_sim.cpp - * - * Simulation interface for barometer - */ +/** + * @file baro_sim.cpp + * + * Simulation interface for barometer + */ /* XXX trim includes */ #include @@ -182,7 +182,7 @@ int BARO_SIM::_measure(unsigned addr) { /* - * Disable retries on this command; we can't know whether failure + * Disable retries on this command; we can't know whether failure * means the device did or did not see the command. */ _retries = 0; @@ -201,25 +201,28 @@ BARO_SIM::_read_prom() int BARO_SIM::transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len) + uint8_t *recv, unsigned recv_len) { // TODO add Simulation data connection so calls retrieve // data from the simulator if (recv_len == 0) { PX4_DEBUG("BARO_SIM measurement requested"); - } - else if (send_len != 1 || send[0] != 0 ) { + + } else if (send_len != 1 || send[0] != 0) { PX4_WARN("BARO_SIM::transfer invalid param %u %u %u", send_len, send[0], recv_len); return 1; - } - else { + + } else { Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_ERR("Error BARO_SIM::transfer no simulator"); return -ENODEV; } + PX4_DEBUG("BARO_SIM::transfer getting sample"); sim->getBaroSample(recv, recv_len); } + return 0; } diff --git a/src/platforms/posix/drivers/gpssim/gpssim.cpp b/src/platforms/posix/drivers/gpssim/gpssim.cpp index d94fc35f8a..889d793486 100644 --- a/src/platforms/posix/drivers/gpssim/gpssim.cpp +++ b/src/platforms/posix/drivers/gpssim/gpssim.cpp @@ -204,8 +204,9 @@ GPSSIM::~GPSSIM() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { px4_task_delete(_task); + } g_dev = nullptr; } @@ -216,12 +217,13 @@ GPSSIM::init() int ret = ERROR; /* do regular cdev init */ - if (VDev::init() != OK) + if (VDev::init() != OK) { goto out; + } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); + SCHED_PRIORITY_DEFAULT, 1500, (px4_main_t)&GPSSIM::task_main_trampoline, nullptr); if (_task < 0) { PX4_ERR("task start failed: %d", errno); @@ -263,7 +265,8 @@ GPSSIM::task_main_trampoline(void *arg) } int -GPSSIM::receive(int timeout) { +GPSSIM::receive(int timeout) +{ Simulator *sim = Simulator::getInstance(); simulator::RawGPSData gps; sim->getGPSSample((uint8_t *)&gps, sizeof(gps)); @@ -275,11 +278,11 @@ GPSSIM::receive(int timeout) { _report_gps_pos.timestamp_variance = hrt_absolute_time(); _report_gps_pos.eph = (float)gps.eph * 1e-2f; _report_gps_pos.epv = (float)gps.epv * 1e-2f; - _report_gps_pos.vel_m_s = (float)(gps.vel)/100.0f; - _report_gps_pos.vel_n_m_s = (float)(gps.vn)/100.0f; - _report_gps_pos.vel_e_m_s = (float)(gps.ve)/100.0f; - _report_gps_pos.vel_d_m_s = (float)(gps.vd)/100.0f; - _report_gps_pos.cog_rad = (float)(gps.cog)*3.1415f/(100.0f * 180.0f); + _report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f; + _report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f; + _report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f; + _report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f; + _report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f); _report_gps_pos.fix_type = gps.fix_type; _report_gps_pos.satellites_used = gps.satellites_visible; @@ -309,9 +312,10 @@ GPSSIM::task_main() _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + _report_gps_pos.vel_ned_valid = true; //no time and satellite information simulated @@ -325,6 +329,7 @@ GPSSIM::task_main() } usleep(2e5); + } else { //Publish initial report that we have access to a GPS //Make sure to clear any stale data in case driver is reset @@ -350,7 +355,8 @@ GPSSIM::task_main() /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_p_report_sat_info) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); @@ -361,6 +367,7 @@ GPSSIM::task_main() } } } + lock(); } } @@ -383,7 +390,7 @@ void GPSSIM::print_info() { //GPS Mode - if(_fake_gps) { + if (_fake_gps) { PX4_INFO("protocol: faked"); } @@ -392,17 +399,17 @@ GPSSIM::print_info() } PX4_INFO("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - PX4_INFO("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + PX4_INFO("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { PX4_INFO("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); PX4_INFO("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); PX4_INFO("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, - (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); PX4_INFO("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); //PX4_INFO("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); //PX4_INFO("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); diff --git a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp index cbe1dbef41..0ac023285c 100644 --- a/src/platforms/posix/drivers/gyrosim/gyrosim.cpp +++ b/src/platforms/posix/drivers/gyrosim/gyrosim.cpp @@ -34,7 +34,7 @@ /** * @file gyrosim.cpp * - * Driver for the simulated gyro + * Driver for the simulated gyro * * @author Andrew Tridgell * @author Pat Hickey @@ -242,7 +242,7 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /** * Accel self test @@ -256,7 +256,7 @@ private: * * @return 0 on success, 1 on failure */ - int gyro_self_test(); + int gyro_self_test(); /* set sample rate (approximate) - 1kHz to 5Hz @@ -264,8 +264,8 @@ private: void _set_sample_rate(unsigned desired_sample_rate_hz); /* do not allow to copy this class due to pointer data members */ - GYROSIM(const GYROSIM&); - GYROSIM operator=(const GYROSIM&); + GYROSIM(const GYROSIM &); + GYROSIM operator=(const GYROSIM &); #pragma pack(push, 1) /** @@ -314,8 +314,8 @@ private: int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ - GYROSIM_gyro(const GYROSIM_gyro&); - GYROSIM_gyro operator=(const GYROSIM_gyro&); + GYROSIM_gyro(const GYROSIM_gyro &); + GYROSIM_gyro operator=(const GYROSIM_gyro &); }; /** driver 'main' command */ @@ -389,13 +389,17 @@ GYROSIM::~GYROSIM() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) + if (_accel_reports != nullptr) { delete _accel_reports; - if (_gyro_reports != nullptr) - delete _gyro_reports; + } - if (_accel_class_instance != -1) + if (_gyro_reports != nullptr) { + delete _gyro_reports; + } + + if (_accel_class_instance != -1) { unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -419,16 +423,19 @@ GYROSIM::init() } struct accel_report arp = {}; + struct gyro_report grp = {}; /* allocate basic report buffers */ _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + if (_accel_reports == nullptr) { PX4_WARN("_accel_reports creation failed"); goto out; } _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + if (_gyro_reports == nullptr) { PX4_WARN("_gyro_reports creation failed"); goto out; @@ -457,6 +464,7 @@ GYROSIM::init() /* do VDev init for the gyro device node, keep it optional */ ret = _gyro->init(); + /* if probe/setup failed, bail now */ if (ret != OK) { DEVICE_DEBUG("gyro init failed"); @@ -472,12 +480,12 @@ GYROSIM::init() /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, - &_accel_orb_class_instance, ORB_PRIO_HIGH); + &_accel_orb_class_instance, ORB_PRIO_HIGH); if (_accel_topic == nullptr) { PX4_WARN("ADVERT FAIL"); - } - else { + + } else { _pub_blocked = false; } @@ -486,7 +494,7 @@ GYROSIM::init() _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); + &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); if (_gyro->_gyro_topic == nullptr) { PX4_WARN("ADVERT FAIL"); @@ -511,6 +519,7 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) if (cmd == MPUREAD) { // Get data from the simulator Simulator *sim = Simulator::getInstance(); + if (sim == NULL) { PX4_WARN("failed accessing simulator"); return ENODEV; @@ -519,17 +528,18 @@ GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) // FIXME - not sure what interrupt status should be recv[1] = 0; // skip cmd and status bytes - sim->getMPUReport(&recv[2], len-2); - } - else if (cmd & DIR_READ) - { - PX4_DEBUG("Reading %u bytes from register %u", len-1, reg); - memcpy(&_regdata[reg-MPUREG_PRODUCT_ID], &send[1], len-1); - } - else { - PX4_DEBUG("Writing %u bytes to register %u", len-1, reg); - if (recv) - memcpy(&recv[1], &_regdata[reg-MPUREG_PRODUCT_ID], len-1); + sim->getMPUReport(&recv[2], len - 2); + + } else if (cmd & DIR_READ) { + PX4_DEBUG("Reading %u bytes from register %u", len - 1, reg); + memcpy(&_regdata[reg - MPUREG_PRODUCT_ID], &send[1], len - 1); + + } else { + PX4_DEBUG("Writing %u bytes to register %u", len - 1, reg); + + if (recv) { + memcpy(&recv[1], &_regdata[reg - MPUREG_PRODUCT_ID], len - 1); + } } return PX4_OK; @@ -542,23 +552,26 @@ void GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) { PX4_INFO("GYROSIM::_set_sample_rate %uHz", desired_sample_rate_hz); + if (desired_sample_rate_hz == 0 || - desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || - desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { + desired_sample_rate_hz == GYRO_SAMPLERATE_DEFAULT || + desired_sample_rate_hz == ACCEL_SAMPLERATE_DEFAULT) { desired_sample_rate_hz = GYROSIM_GYRO_DEFAULT_RATE; } uint8_t div = 1000 / desired_sample_rate_hz; - if(div>200) div=200; - if(div<1) div=1; + + if (div > 200) { div = 200; } + + if (div < 1) { div = 1; } // This does nothing in the simulator but writes the value in the "register" so // register dumps look correct - write_reg(MPUREG_SMPLRT_DIV, div-1); + write_reg(MPUREG_SMPLRT_DIV, div - 1); _sample_rate = 1000 / div; PX4_INFO("GYROSIM: Changed sample rate to %uHz", _sample_rate); - _call_interval = 1000000/_sample_rate; + _call_interval = 1000000 / _sample_rate; hrt_cancel(&_call); hrt_call_every(&_call, _call_interval, _call_interval, (hrt_callout)&GYROSIM::measure_trampoline, this); } @@ -569,8 +582,9 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(accel_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -579,17 +593,21 @@ GYROSIM::read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) + if (_accel_reports->empty()) { return -EAGAIN; + } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ accel_report *arp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_accel_reports->get(arp)) + if (!_accel_reports->get(arp)) { break; + } + transferred++; arp++; } @@ -614,24 +632,34 @@ GYROSIM::accel_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* inspect accel offsets */ - if (fabsf(_accel_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) + if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) { return 1; + } - if (fabsf(_accel_scale.z_offset) < 0.000001f) + if (fabsf(_accel_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) + } + + if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) { return 1; + } + + if (fabsf(_accel_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) { + return 1; + } return 0; } @@ -641,8 +669,9 @@ GYROSIM::gyro_self_test() { return OK; - if (self_test()) + if (self_test()) { return 1; + } /* * Maximum deviation of 20 degrees, according to @@ -658,27 +687,35 @@ GYROSIM::gyro_self_test() const float max_scale = 0.3f; /* evaluate gyro offsets, complain if offset -> zero or larger than 20 dps. */ - if (fabsf(_gyro_scale.x_offset) > max_offset) + if (fabsf(_gyro_scale.x_offset) > max_offset) { return 1; + } /* evaluate gyro scale, complain if off by more than 30% */ - if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.x_scale - 1.0f) > max_scale) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > max_offset) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) + if (fabsf(_gyro_scale.y_offset) > max_offset) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > max_offset) + if (fabsf(_gyro_scale.y_scale - 1.0f) > max_scale) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) + } + + if (fabsf(_gyro_scale.z_offset) > max_offset) { return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > max_scale) { + return 1; + } /* check if all scales are zero */ if ((fabsf(_gyro_scale.x_offset) < 0.000001f) && - (fabsf(_gyro_scale.y_offset) < 0.000001f) && - (fabsf(_gyro_scale.z_offset) < 0.000001f)) { + (fabsf(_gyro_scale.y_offset) < 0.000001f) && + (fabsf(_gyro_scale.z_offset) < 0.000001f)) { /* if all are zero, this device is not calibrated */ return 1; } @@ -692,8 +729,9 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) unsigned count = buflen / sizeof(gyro_report); /* buffer must be large enough */ - if (count < 1) + if (count < 1) { return -ENOSPC; + } /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { @@ -702,17 +740,21 @@ GYROSIM::gyro_read(device::file_t *filp, char *buffer, size_t buflen) } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) + if (_gyro_reports->empty()) { return -EAGAIN; + } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ gyro_report *grp = reinterpret_cast(buffer); int transferred = 0; + while (count--) { - if (!_gyro_reports->get(grp)) + if (!_gyro_reports->get(grp)) { break; + } + transferred++; grp++; } @@ -732,34 +774,35 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: return ioctl(filp, SENSORIOCSPOLLRATE, 1000); case SENSOR_POLLRATE_DEFAULT: return ioctl(filp, SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); - /* adjust to a legal polling interval in Hz */ + /* adjust to a legal polling interval in Hz */ default: { /* convert hz to hrt interval via microseconds */ unsigned ticks = 1000000 / arg; /* check against maximum sane rate */ - if (ticks < 1000) + if (ticks < 1000) { return -EINVAL; + } /* update interval for next measurement */ _call_interval = ticks; @@ -778,23 +821,25 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_interval; case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_accel_reports->resize(arg)) { - return -ENOMEM; + if (!_accel_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _accel_reports->size(); @@ -808,14 +853,15 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) case ACCELIOCSLOWPASS: return OK; - case ACCELIOCSSCALE: - { + case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ struct accel_scale *s = (struct accel_scale *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; + if (sum > 2.0f && sum < 4.0f) { memcpy(&_accel_scale, s, sizeof(_accel_scale)); return OK; + } else { return -EINVAL; } @@ -830,7 +876,7 @@ GYROSIM::ioctl(device::file_t *filp, int cmd, unsigned long arg) return set_accel_range(arg); case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/GYROSIM_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2) / GYROSIM_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -846,24 +892,25 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - /* these are shared with the accel side */ + /* these are shared with the accel side */ case SENSORIOCSPOLLRATE: case SENSORIOCGPOLLRATE: case SENSORIOCRESET: return ioctl(filp, cmd, arg); case SENSORIOCSQUEUEDEPTH: { - /* lower bound is mandatory, upper bound is a sanity check */ - if ((arg < 1) || (arg > 100)) - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } - if (!_gyro_reports->resize(arg)) { - return -ENOMEM; + if (!_gyro_reports->resize(arg)) { + return -ENOMEM; + } + + return OK; } - return OK; - } - case SENSORIOCGQUEUEDEPTH: return _gyro_reports->size(); @@ -893,6 +940,7 @@ GYROSIM::gyro_ioctl(device::file_t *filp, int cmd, unsigned long arg) // _gyro_range_scale = xx // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f); @@ -924,7 +972,7 @@ GYROSIM::write_reg(unsigned reg, uint8_t value) cmd[0] = reg | DIR_WRITE; cmd[1] = value; - // general register transfer at low clock speed + // general register transfer at low clock speed transfer(cmd, nullptr, sizeof(cmd)); } @@ -933,11 +981,11 @@ GYROSIM::set_accel_range(unsigned max_g_in) { // workaround for bugged versions of MPU6k (rev C) switch (_product) { - case GYROSIMES_REV_C4: - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; - return OK; + case GYROSIMES_REV_C4: + write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + _accel_range_scale = (GYROSIM_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * GYROSIM_ONE_G; + return OK; } uint8_t afs_sel; @@ -948,14 +996,17 @@ GYROSIM::set_accel_range(unsigned max_g_in) afs_sel = 3; lsb_per_g = 2048; max_accel_g = 16; + } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 afs_sel = 2; lsb_per_g = 4096; max_accel_g = 8; + } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 afs_sel = 1; lsb_per_g = 8192; max_accel_g = 4; + } else { // 2g - AFS_SEL = 0 afs_sel = 0; lsb_per_g = 16384; @@ -1011,10 +1062,11 @@ GYROSIM::measure() if (x == 99) { x = 0; PX4_INFO("GYROSIM::measure %" PRIu64, hrt_absolute_time()); - } - else { + + } else { x++; } + #endif struct MPUReport mpu_report = {}; @@ -1026,8 +1078,8 @@ GYROSIM::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - // sensor transfer at high clock speed - //set_frequency(GYROSIM_HIGH_BUS_SPEED); + // sensor transfer at high clock speed + //set_frequency(GYROSIM_HIGH_BUS_SPEED); if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { return; } @@ -1045,7 +1097,7 @@ GYROSIM::measure() // transfers and bad register reads. This allows the higher // level code to decide if it should use this sensor based on // whether it has had failures - grb.error_count = arb.error_count = 0; // FIXME + grb.error_count = arb.error_count = 0; // FIXME /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1074,21 +1126,21 @@ GYROSIM::measure() _last_temperature = mpu_report.temp; - arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); + arb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); arb.temperature = _last_temperature; arb.x = mpu_report.accel_x; arb.y = mpu_report.accel_y; arb.z = mpu_report.accel_z; - grb.x_raw = (int16_t)(mpu_report.gyro_x/_gyro_range_scale); - grb.y_raw = (int16_t)(mpu_report.gyro_y/_gyro_range_scale); - grb.z_raw = (int16_t)(mpu_report.gyro_z/_gyro_range_scale); + grb.x_raw = (int16_t)(mpu_report.gyro_x / _gyro_range_scale); + grb.y_raw = (int16_t)(mpu_report.gyro_y / _gyro_range_scale); + grb.z_raw = (int16_t)(mpu_report.gyro_z / _gyro_range_scale); grb.scaling = _gyro_range_scale; grb.range_rad_s = _gyro_range_rad_s; - grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f)*361.0f); + grb.temperature_raw = (int16_t)((mpu_report.temp - 35.0f) * 361.0f); grb.temperature = _last_temperature; grb.x = mpu_report.gyro_x; @@ -1102,6 +1154,7 @@ GYROSIM::measure() /* notify anyone waiting for data */ poll_notify(POLLIN); _gyro->parent_poll_notify(); + if (!(_pub_blocked)) { /* log the time of this report */ perf_begin(_controller_latency_perf); @@ -1135,22 +1188,25 @@ GYROSIM::print_info() void GYROSIM::print_registers() { - char buf[6*13+1]; - int i=0; + char buf[6 * 13 + 1]; + int i = 0; buf[0] = '\0'; PX4_INFO("GYROSIM registers"); - for (uint8_t reg=MPUREG_PRODUCT_ID; reg<=108; reg++) { + + for (uint8_t reg = MPUREG_PRODUCT_ID; reg <= 108; reg++) { uint8_t v = read_reg(reg); - sprintf(&buf[i*6], "%02x:%02x ",(unsigned)reg, (unsigned)v); + sprintf(&buf[i * 6], "%02x:%02x ", (unsigned)reg, (unsigned)v); i++; - if ((i+1) % 13 == 0) { + + if ((i + 1) % 13 == 0) { PX4_INFO("%s", buf); - i=0; + i = 0; buf[i] = '\0'; } } - PX4_INFO("%s",buf); + + PX4_INFO("%s", buf); } @@ -1165,8 +1221,9 @@ GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : GYROSIM_gyro::~GYROSIM_gyro() { - if (_gyro_class_instance != -1) + if (_gyro_class_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance); + } } int @@ -1205,11 +1262,12 @@ GYROSIM_gyro::ioctl(device::file_t *filp, int cmd, unsigned long arg) { switch (cmd) { - case DEVIOCGDEVICEID: - return (int)VDev::ioctl(filp, cmd, arg); - break; - default: - return _parent->gyro_ioctl(filp, cmd, arg); + case DEVIOCGDEVICEID: + return (int)VDev::ioctl(filp, cmd, arg); + break; + + default: + return _parent->gyro_ioctl(filp, cmd, arg); } } @@ -1239,7 +1297,7 @@ int start(enum Rotation rotation) { int fd; - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; @@ -1252,17 +1310,20 @@ start(enum Rotation rotation) /* create the driver */ *g_dev_ptr = new GYROSIM(path_accel, path_gyro, rotation); - if (*g_dev_ptr == nullptr) + if (*g_dev_ptr == nullptr) { goto fail; + } - if (OK != (*g_dev_ptr)->init()) + if (OK != (*g_dev_ptr)->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = px4_open(path_accel, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { px4_close(fd); @@ -1274,8 +1335,8 @@ start(enum Rotation rotation) fail: if (*g_dev_ptr != nullptr) { - delete (*g_dev_ptr); - *g_dev_ptr = nullptr; + delete(*g_dev_ptr); + *g_dev_ptr = nullptr; } PX4_WARN("driver start failed"); @@ -1286,13 +1347,16 @@ int stop() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr != nullptr) { delete *g_dev_ptr; *g_dev_ptr = nullptr; + } else { /* warn, but not an error */ PX4_WARN("already stopped."); } + return 0; } @@ -1350,7 +1414,7 @@ test() PX4_INFO("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); PX4_INFO("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); PX4_INFO("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); + (double)(a_report.range_m_s2 / GYROSIM_ONE_G)); /* do a simple demand read */ sz = px4_read(fd_gyro, &g_report, sizeof(g_report)); @@ -1368,7 +1432,7 @@ test() PX4_INFO("gyro y: \t%d\traw", (int)g_report.y_raw); PX4_INFO("gyro z: \t%d\traw", (int)g_report.z_raw); PX4_INFO("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, - (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); + (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); PX4_INFO("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); PX4_INFO("temp: \t%d\traw 0x%0x", (short)a_report.temperature_raw, (unsigned short)a_report.temperature_raw); @@ -1380,7 +1444,7 @@ test() reset(); PX4_INFO("PASS"); - + return 0; } @@ -1409,11 +1473,11 @@ reset() goto reset_fail; } - px4_close(fd); + px4_close(fd); return 0; reset_fail: - px4_close(fd); + px4_close(fd); return 1; } @@ -1423,7 +1487,8 @@ reset_fail: int info() { - GYROSIM **g_dev_ptr = &g_dev_sim; + GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1442,6 +1507,7 @@ int regdump() { GYROSIM **g_dev_ptr = &g_dev_sim; + if (*g_dev_ptr == nullptr) { PX4_ERR("driver not running"); return 1; @@ -1473,11 +1539,13 @@ gyrosim_main(int argc, char *argv[]) /* jump over start/off/etc and look at options first */ int myoptind = 1; const char *myoptarg = NULL; + while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'R': rotation = (enum Rotation)atoi(myoptarg); break; + default: gyrosim::usage(); return 0; diff --git a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp index d070017a36..b58cccb5bd 100644 --- a/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp +++ b/src/platforms/posix/drivers/tonealrmsim/tone_alarm.cpp @@ -47,16 +47,16 @@ * From Wikibooks: * * PLAY "[string expression]" - * + * * Used to play notes and a score ... The tones are indicated by letters A through G. - * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) + * Accidentals are indicated with a "+" or "#" (for sharp) or "-" (for flat) * immediately after the note letter. See this example: - * + * * PLAY "C C# C C#" * * Whitespaces are ignored inside the string expression. There are also codes that - * set the duration, octave and tempo. They are all case-insensitive. PLAY executes - * the commands or notes the order in which they appear in the string. Any indicators + * set the duration, octave and tempo. They are all case-insensitive. PLAY executes + * the commands or notes the order in which they appear in the string. Any indicators * that change the properties are effective for the notes following that indicator. * * Ln Sets the duration (length) of the notes. The variable n does not indicate an actual duration @@ -66,15 +66,15 @@ * The shorthand notation of length is also provided for a note. For example, "L4 CDE L8 FG L4 AB" * can be shortened to "L4 CDE F8G8 AB". F and G play as eighth notes while others play as quarter notes. * On Sets the current octave. Valid values for n are 0 through 6. An octave begins with C and ends with B. - * Remember that C- is equivalent to B. + * Remember that C- is equivalent to B. * < > Changes the current octave respectively down or up one level. * Nn Plays a specified note in the seven-octave range. Valid values are from 0 to 84. (0 is a pause.) * Cannot use with sharp and flat. Cannot use with the shorthand notation neither. * MN Stand for Music Normal. Note duration is 7/8ths of the length indicated by Ln. It is the default mode. * ML Stand for Music Legato. Note duration is full length of that indicated by Ln. * MS Stand for Music Staccato. Note duration is 3/4ths of the length indicated by Ln. - * Pn Causes a silence (pause) for the length of note indicated (same as Ln). - * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. + * Pn Causes a silence (pause) for the length of note indicated (same as Ln). + * Tn Sets the number of "L4"s per minute (tempo). Valid values are from 32 to 255. The default value is T120. * . When placed after a note, it causes the duration of the note to be 3/2 of the set duration. * This is how to get "dotted" notes. "L4 C#." would play C sharp as a dotted quarter note. * It can be used for a pause as well. @@ -120,14 +120,15 @@ public: virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); virtual ssize_t write(device::file_t *filp, const char *buffer, size_t len); - inline const char *name(int tune) { + inline const char *name(int tune) + { return _tune_names[tune]; } private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes - const char * _default_tunes[TONE_NUMBER_OF_TUNES]; - const char * _tune_names[TONE_NUMBER_OF_TUNES]; + const char *_default_tunes[TONE_NUMBER_OF_TUNES]; + const char *_tune_names[TONE_NUMBER_OF_TUNES]; static const uint8_t _note_tab[]; unsigned _default_tune_number; // number of currently playing default tune (0 for none) @@ -151,8 +152,8 @@ private: // unsigned note_to_divisor(unsigned note); - // Calculate the duration in microseconds of play and silence for a - // note given the current tempo, length and mode and the number of + // Calculate the duration in microseconds of play and silence for a + // note given the current tempo, length and mode and the number of // dots following in the play string. // unsigned note_duration(unsigned &silence, unsigned note_length, unsigned dots); @@ -257,8 +258,9 @@ ToneAlarm::init() ret = VDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } DEVICE_DEBUG("ready"); return OK; @@ -267,7 +269,7 @@ ToneAlarm::init() unsigned ToneAlarm::note_to_divisor(unsigned note) { - const int TONE_ALARM_CLOCK = 120000000ul/4; + const int TONE_ALARM_CLOCK = 120000000ul / 4; // compute the frequency first (Hz) float freq = 880.0f * expf(logf(2.0f) * ((int)note - 46) / 12.0f); @@ -285,25 +287,31 @@ ToneAlarm::note_duration(unsigned &silence, unsigned note_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (note_length == 0) + if (note_length == 0) { note_length = 1; + } + unsigned note_period = whole_note_period / note_length; switch (_note_mode) { case MODE_NORMAL: silence = note_period / 8; break; + case MODE_STACCATO: silence = note_period / 4; break; + default: case MODE_LEGATO: silence = 0; break; } + note_period -= silence; unsigned dot_extension = note_period / 2; + while (dots--) { note_period += dot_extension; dot_extension /= 2; @@ -317,12 +325,14 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) { unsigned whole_note_period = (60 * 1000000 * 4) / _tempo; - if (rest_length == 0) + if (rest_length == 0) { rest_length = 1; + } unsigned rest_period = whole_note_period / rest_length; unsigned dot_extension = rest_period / 2; + while (dots--) { rest_period += dot_extension; dot_extension /= 2; @@ -406,115 +416,155 @@ ToneAlarm::next_note() while (note == 0) { // we always need at least one character from the string int c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_end; + } + _next++; switch (c) { case 'L': // select note length _note_length = next_number(); - if (_note_length < 1) + + if (_note_length < 1) { goto tune_error; + } + break; case 'O': // select octave _octave = next_number(); - if (_octave > 6) + + if (_octave > 6) { _octave = 6; + } + break; case '<': // decrease octave - if (_octave > 0) + if (_octave > 0) { _octave--; + } + break; case '>': // increase octave - if (_octave < 6) + if (_octave < 6) { _octave++; + } + break; case 'M': // select inter-note gap c = next_char(); - if (c == 0) + + if (c == 0) { goto tune_error; + } + _next++; + switch (c) { case 'N': _note_mode = MODE_NORMAL; break; + case 'L': _note_mode = MODE_LEGATO; break; + case 'S': _note_mode = MODE_STACCATO; break; + case 'F': _repeat = false; break; + case 'B': _repeat = true; break; + default: goto tune_error; } + break; case 'P': // pause for a note length stop_note(); - hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(next_number(), next_dots()), - (hrt_callout)next_trampoline, - this); + hrt_call_after(&_note_call, + (hrt_abstime)rest_duration(next_number(), next_dots()), + (hrt_callout)next_trampoline, + this); return; case 'T': { // change tempo - unsigned nt = next_number(); + unsigned nt = next_number(); - if ((nt >= 32) && (nt <= 255)) { - _tempo = nt; - } else { - goto tune_error; + if ((nt >= 32) && (nt <= 255)) { + _tempo = nt; + + } else { + goto tune_error; + } + + break; } - break; - } case 'N': // play an arbitrary note note = next_number(); - if (note > 84) + + if (note > 84) { goto tune_error; + } + if (note == 0) { // this is a rest - pause for the current note length hrt_call_after(&_note_call, - (hrt_abstime)rest_duration(_note_length, next_dots()), - (hrt_callout)next_trampoline, - this); - return; + (hrt_abstime)rest_duration(_note_length, next_dots()), + (hrt_callout)next_trampoline, + this); + return; } + break; case 'A'...'G': // play a note in the current octave note = _note_tab[c - 'A'] + (_octave * 12) + 1; c = next_char(); + switch (c) { case '#': // up a semitone case '+': - if (note < 84) + if (note < 84) { note++; + } + _next++; break; + case '-': // down a semitone - if (note > 1) + if (note > 1) { note--; + } + _next++; break; + default: // 0 / no next char here is OK break; } + // shorthand length notation note_length = next_number(); - if (note_length == 0) + + if (note_length == 0) { note_length = _note_length; + } + break; default: @@ -540,12 +590,15 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); + if (_repeat) { start_tune(_tune); + } else { _tune = nullptr; _default_tune_number = 0; } + return; } @@ -555,6 +608,7 @@ ToneAlarm::next_char() while (isspace(*_next)) { _next++; } + return toupper(*_next); } @@ -566,8 +620,11 @@ ToneAlarm::next_number() for (;;) { c = next_char(); - if (!isdigit(c)) + + if (!isdigit(c)) { return number; + } + _next++; number = (number * 10) + (c - '0'); } @@ -582,6 +639,7 @@ ToneAlarm::next_dots() _next++; dots++; } + return dots; } @@ -615,6 +673,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) _next = nullptr; _repeat = false; _default_tune_number = 0; + } else { /* always interrupt alarms, unless they are repeating and already playing */ if (!(_repeat && _default_tune_number == arg)) { @@ -623,6 +682,7 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) start_tune(_default_tunes[arg]); } } + } else { result = -EINVAL; } @@ -637,8 +697,9 @@ ToneAlarm::ioctl(device::file_t *filp, int cmd, unsigned long arg) // irqrestore(flags); /* give it to the superclass if we didn't like it */ - if (result == -ENOTTY) + if (result == -ENOTTY) { result = VDev::ioctl(filp, cmd, arg); + } return result; } @@ -647,8 +708,9 @@ ssize_t ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) { // sanity-check the buffer for length and nul-termination - if (len > _tune_max) + if (len > _tune_max) { return -EFBIG; + } // if we have an existing user tune, free it if (_user_tune != nullptr) { @@ -665,13 +727,16 @@ ToneAlarm::write(device::file_t *filp, const char *buffer, size_t len) } // if the new tune is empty, we're done - if (buffer[0] == '\0') + if (buffer[0] == '\0') { return OK; + } // allocate a copy of the new tune _user_tune = strndup(buffer, len); - if (_user_tune == nullptr) + + if (_user_tune == nullptr) { return -ENOMEM; + } // and play it start_tune(_user_tune); @@ -725,13 +790,15 @@ play_string(const char *str, bool free_buffer) ret = px4_write(fd, str, strlen(str) + 1); px4_close(fd); - if (free_buffer) + if (free_buffer) { free((void *)str); + } if (ret < 0) { PX4_WARN("play tune"); return 1; } + return ret; } @@ -766,31 +833,38 @@ tone_alarm_main(int argc, char *argv[]) ret = play_tune(TONE_STOP_TUNE); } - else if (!strcmp(argv1, "stop")) + else if (!strcmp(argv1, "stop")) { ret = play_tune(TONE_STOP_TUNE); + } - else if ((tune = strtol(argv1, nullptr, 10)) != 0) + else if ((tune = strtol(argv1, nullptr, 10)) != 0) { ret = play_tune(tune); + } /* If it is a file name then load and play it as a string */ else if (*argv1 == '/') { FILE *fd = fopen(argv1, "r"); int sz; char *buffer; + if (fd == nullptr) { PX4_WARN("couldn't open '%s'", argv1); return 1; } + fseek(fd, 0, SEEK_END); sz = ftell(fd); fseek(fd, 0, SEEK_SET); buffer = (char *)malloc(sz + 1); + if (buffer == nullptr) { PX4_WARN("not enough memory memory"); return 1; } + // FIXME - Make GCC happy if (fread(buffer, sz, 1, fd)) { } + /* terminate the string */ buffer[sz] = 0; ret = play_string(buffer, true); @@ -801,14 +875,15 @@ tone_alarm_main(int argc, char *argv[]) if (*argv1 == 'M') { ret = play_string(argv1, false); } - } - else { + + } else { /* It might be a tune name */ for (tune = 1; tune < TONE_NUMBER_OF_TUNES; tune++) if (!strcmp(g_dev->name(tune), argv1)) { ret = play_tune(tune); return ret; } + PX4_WARN("unrecognized command, try 'start', 'stop', an alarm number or name, or a file name starting with a '/'"); ret = 1; } From d60f201dc86668ba4eb485f732487bac7a295da1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:35:46 +0200 Subject: [PATCH 061/155] POSIX: Fix formatting --- src/platforms/posix/include/crc32.h | 2 +- src/platforms/posix/include/queue.h | 32 ++--- src/platforms/posix/main.cpp | 2 + src/platforms/posix/px4_layer/drv_hrt.c | 1 + src/platforms/posix/px4_layer/lib_crc32.c | 85 ++++++------ .../posix/px4_layer/px4_posix_tasks.cpp | 131 ++++++++++-------- src/platforms/posix/px4_layer/px4_sem.cpp | 10 +- src/platforms/posix/work_queue/hrt_thread.c | 2 +- 8 files changed, 145 insertions(+), 120 deletions(-) diff --git a/src/platforms/posix/include/crc32.h b/src/platforms/posix/include/crc32.h index bf828e3e0e..34e080b1c2 100644 --- a/src/platforms/posix/include/crc32.h +++ b/src/platforms/posix/include/crc32.h @@ -63,7 +63,7 @@ extern "C" { ****************************************************************************/ EXTERN uint32_t crc32part(const uint8_t *src, size_t len, - uint32_t crc32val); + uint32_t crc32val); /**************************************************************************** * Name: crc32 diff --git a/src/platforms/posix/include/queue.h b/src/platforms/posix/include/queue.h index 4d95541e02..30dc264f3f 100644 --- a/src/platforms/posix/include/queue.h +++ b/src/platforms/posix/include/queue.h @@ -69,30 +69,26 @@ * Global Type Declarations ************************************************************************/ -struct sq_entry_s -{ - FAR struct sq_entry_s *flink; +struct sq_entry_s { + FAR struct sq_entry_s *flink; }; typedef struct sq_entry_s sq_entry_t; -struct dq_entry_s -{ - FAR struct dq_entry_s *flink; - FAR struct dq_entry_s *blink; +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; }; typedef struct dq_entry_s dq_entry_t; -struct sq_queue_s -{ - FAR sq_entry_t *head; - FAR sq_entry_t *tail; +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; }; typedef struct sq_queue_s sq_queue_t; -struct dq_queue_s -{ - FAR dq_entry_t *head; - FAR dq_entry_t *tail; +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; }; typedef struct dq_queue_s dq_queue_t; @@ -112,11 +108,11 @@ EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, - sq_queue_t *queue); + sq_queue_t *queue); EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, - dq_queue_t *queue); + dq_queue_t *queue); EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index fd9fdef5c6..26465c266e 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -96,6 +96,7 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) if (exit_on_fail && retval) { exit(retval); } + usleep(65000); } else if (command.compare("help") == 0) { @@ -108,6 +109,7 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) cout << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; } + print_prompt(); } diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index cae3c379da..c925574b42 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -237,6 +237,7 @@ void hrt_init(void) sq_init(&callout_queue); int sem_ret = px4_sem_init(&_hrt_lock, 0, 1); + if (sem_ret) { PX4_ERR("SEM INIT FAIL: %s", strerror(errno)); } diff --git a/src/platforms/posix/px4_layer/lib_crc32.c b/src/platforms/posix/px4_layer/lib_crc32.c index 4ba6fbf6df..c14ebfceeb 100644 --- a/src/platforms/posix/px4_layer/lib_crc32.c +++ b/src/platforms/posix/px4_layer/lib_crc32.c @@ -48,46 +48,45 @@ #include // Needed for Linux -#define FAR +#define FAR /************************************************************************************************ * Private Data ************************************************************************************************/ - -static const uint32_t crc32_tab[] = -{ - 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, - 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, - 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, - 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, - 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, - 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, - 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, - 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, - 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, - 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, - 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, - 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, - 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, - 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, - 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, - 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, - 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, - 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, - 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, - 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, - 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, - 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, - 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, - 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, - 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, - 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, - 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, - 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, - 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, - 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, - 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, - 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d + +static const uint32_t crc32_tab[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d }; /************************************************************************************************ @@ -103,13 +102,13 @@ static const uint32_t crc32_tab[] = uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) { - size_t i; + size_t i; - for (i = 0; i < len; i++) - { - crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); - } - return crc32val; + for (i = 0; i < len; i++) { + crc32val = crc32_tab[(crc32val ^ src[i]) & 0xff] ^ (crc32val >> 8); + } + + return crc32val; } /************************************************************************************************ @@ -122,5 +121,5 @@ uint32_t crc32part(FAR const uint8_t *src, size_t len, uint32_t crc32val) uint32_t crc32(FAR const uint8_t *src, size_t len) { - return crc32part(src, len, 0); + return crc32part(src, len, 0); } diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index f112290cb3..ba476b5070 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -63,8 +63,7 @@ pthread_t _shell_task_id = 0; -struct task_entry -{ +struct task_entry { pthread_t pid; std::string name; bool isused; @@ -73,27 +72,26 @@ struct task_entry static task_entry taskmap[PX4_MAX_TASKS]; -typedef struct -{ +typedef struct { px4_main_t entry; int argc; char *argv[]; - // strings are allocated after the + // strings are allocated after the } pthdata_t; -static void *entry_adapter ( void *ptr ) +static void *entry_adapter(void *ptr) { - pthdata_t *data; - data = (pthdata_t *) ptr; + pthdata_t *data; + data = (pthdata_t *) ptr; data->entry(data->argc, data->argv); free(ptr); PX4_DEBUG("Before px4_task_exit"); - px4_task_exit(0); + px4_task_exit(0); PX4_DEBUG("After px4_task_exit"); return NULL; -} +} void px4_systemreset(bool to_bootloader) @@ -102,7 +100,8 @@ px4_systemreset(bool to_bootloader) exit(0); } -px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, char * const argv[]) +px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, px4_main_t entry, + char *const argv[]) { int rv; int argc = 0; @@ -110,52 +109,62 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int unsigned int len = 0; unsigned long offset; unsigned long structsize; - char * p = (char *)argv; + char *p = (char *)argv; - pthread_t task; + pthread_t task; pthread_attr_t attr; struct sched_param param; // Calculate argc while (p != (char *)0) { p = argv[argc]; - if (p == (char *)0) - break; - ++argc; - len += strlen(p)+1; - } - structsize = sizeof(pthdata_t)+(argc+1)*sizeof(char *); - pthdata_t *taskdata; - - // not safe to pass stack data to the thread creation - taskdata = (pthdata_t *)malloc(structsize+len); - offset = ((unsigned long)taskdata)+structsize; - taskdata->entry = entry; + if (p == (char *)0) { + break; + } + + ++argc; + len += strlen(p) + 1; + } + + structsize = sizeof(pthdata_t) + (argc + 1) * sizeof(char *); + pthdata_t *taskdata; + + // not safe to pass stack data to the thread creation + taskdata = (pthdata_t *)malloc(structsize + len); + offset = ((unsigned long)taskdata) + structsize; + + taskdata->entry = entry; taskdata->argc = argc; - for (i=0; iargv[i] = (char *)offset; strcpy((char *)offset, argv[i]); - offset+=strlen(argv[i])+1; + offset += strlen(argv[i]) + 1; } + // Must add NULL at end of argv taskdata->argv[argc] = (char *)0; PX4_DEBUG("starting task %s", name); rv = pthread_attr_init(&attr); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to init thread attrs"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set inherit sched"); return (rv < 0) ? rv : -rv; } + rv = pthread_attr_setschedpolicy(&attr, scheduler); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched policy"); return (rv < 0) ? rv : -rv; @@ -164,28 +173,31 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int param.sched_priority = priority; rv = pthread_attr_setschedparam(&attr, ¶m); + if (rv != 0) { PX4_WARN("px4_task_spawn_cmd: failed to set sched param"); return (rv < 0) ? rv : -rv; } - rv = pthread_create (&task, &attr, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, &attr, &entry_adapter, (void *) taskdata); + if (rv != 0) { if (rv == EPERM) { //printf("WARNING: NOT RUNING AS ROOT, UNABLE TO RUN REALTIME THREADS\n"); - rv = pthread_create (&task, NULL, &entry_adapter, (void *) taskdata); + rv = pthread_create(&task, NULL, &entry_adapter, (void *) taskdata); + if (rv != 0) { PX4_ERR("px4_task_spawn_cmd: failed to create thread %d %d\n", rv, errno); return (rv < 0) ? rv : -rv; } - } - else { + + } else { return (rv < 0) ? rv : -rv; } } - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { return -ENOSPC; } - return i; + + return i; } int px4_task_delete(px4_task_t id) @@ -205,15 +219,18 @@ int px4_task_delete(px4_task_t id) pthread_t pid; PX4_DEBUG("Called px4_task_delete"); - if (id < PX4_MAX_TASKS && taskmap[id].isused) + if (id < PX4_MAX_TASKS && taskmap[id].isused) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel - if (pthread_self() == pid) { + if (pthread_self() == pid) { taskmap[id].isused = false; pthread_exit(0); + } else { rv = pthread_cancel(pid); } @@ -225,20 +242,21 @@ int px4_task_delete(px4_task_t id) void px4_task_exit(int ret) { - int i; + int i; pthread_t pid = pthread_self(); // Get pthread ID from the opaque ID - for (i=0; i=PX4_MAX_TASKS) { + + if (i >= PX4_MAX_TASKS) { PX4_ERR("px4_task_exit: self task not found!"); - } - else { + + } else { PX4_DEBUG("px4_task_exit: %s", taskmap[i].name.c_str()); } @@ -251,10 +269,12 @@ int px4_task_kill(px4_task_t id, int sig) pthread_t pid; PX4_DEBUG("Called px4_task_kill %d", sig); - if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) + if (id < PX4_MAX_TASKS && taskmap[id].isused && taskmap[id].pid != 0) { pid = taskmap[id].pid; - else + + } else { return -EINVAL; + } // If current thread then exit, otherwise cancel rv = pthread_kill(pid, sig); @@ -268,27 +288,30 @@ void px4_show_tasks() int count = 0; PX4_INFO("Active Tasks:"); - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused) { PX4_INFO(" %-10s %lu", taskmap[idx].name.c_str(), (unsigned long)taskmap[idx].pid); count++; } } - if (count == 0) + + if (count == 0) { PX4_INFO(" No running tasks"); + } } bool px4_task_is_running(const char *taskname) { int idx; - for (idx=0; idx < PX4_MAX_TASKS; idx++) - { + + for (idx = 0; idx < PX4_MAX_TASKS; idx++) { if (taskmap[idx].isused && (strcmp(taskmap[idx].name.c_str(), taskname) == 0)) { return true; } } + return false; } __BEGIN_DECLS @@ -301,14 +324,14 @@ unsigned long px4_getpid() const char *getprogname(); const char *getprogname() { - pthread_t pid = pthread_self(); - for (int i=0; ilock)); s->value--; - if(s->value < 0) { - pthread_cond_wait(&(s->wait), &(s->lock)); + + if (s->value < 0) { + pthread_cond_wait(&(s->wait), &(s->lock)); } + pthread_mutex_unlock(&(s->lock)); return 0; @@ -77,9 +79,11 @@ int px4_sem_post(px4_sem_t *s) { pthread_mutex_lock(&(s->lock)); s->value++; - if(s->value <= 0) { + + if (s->value <= 0) { pthread_cond_signal(&(s->wait)); } + pthread_mutex_unlock(&(s->lock)); return 0; diff --git a/src/platforms/posix/work_queue/hrt_thread.c b/src/platforms/posix/work_queue/hrt_thread.c index 46ca7538c1..98331ca798 100644 --- a/src/platforms/posix/work_queue/hrt_thread.c +++ b/src/platforms/posix/work_queue/hrt_thread.c @@ -274,7 +274,7 @@ void hrt_work_queue_init(void) work_hrtthread, (char *const *)NULL); - + #ifdef __PX4_QURT signal(SIGALRM, _sighandler); #else From 6c8755106e4e33b876bb887a18a5e9b571423e71 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:36:00 +0200 Subject: [PATCH 062/155] ROS: Fix formatting --- src/platforms/ros/geo.cpp | 15 ++- .../ros/nodes/commander/commander.cpp | 40 ++++---- src/platforms/ros/nodes/commander/commander.h | 6 +- .../demo_offboard_attitude_setpoints.cpp | 7 +- .../ros/nodes/manual_input/manual_input.cpp | 5 +- src/platforms/ros/nodes/mavlink/mavlink.cpp | 92 +++++++++++-------- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp | 5 + 7 files changed, 100 insertions(+), 70 deletions(-) diff --git a/src/platforms/ros/geo.cpp b/src/platforms/ros/geo.cpp index 2f277d7f16..d13a5e277a 100644 --- a/src/platforms/ros/geo.cpp +++ b/src/platforms/ros/geo.cpp @@ -182,7 +182,8 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference return ref->timestamp; } -__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat_rad = lat_0 * M_DEG_TO_RAD; @@ -196,12 +197,14 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * return 0; } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, + double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { return map_projection_init_timestamped(ref, lat_0, lon_0, px4::get_time_micros()); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; @@ -213,7 +216,8 @@ __EXPORT int map_projection_reference(const struct map_projection_reference_s *r return 0; } -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) { if (!map_projection_initialized(ref)) { return -1; @@ -235,7 +239,8 @@ __EXPORT int map_projection_project(const struct map_projection_reference_s *ref return 0; } -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) { if (!map_projection_initialized(ref)) { return -1; diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp index 54086cfd4b..38db5dad7f 100644 --- a/src/platforms/ros/nodes/commander/commander.cpp +++ b/src/platforms/ros/nodes/commander/commander.cpp @@ -109,45 +109,45 @@ void Commander::ManualControlInputCallback(const px4::manual_control_setpointCon } void Commander::SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode) + px4::vehicle_control_mode &msg_vehicle_control_mode) { msg_vehicle_control_mode.flag_control_manual_enabled = false; msg_vehicle_control_mode.flag_control_offboard_enabled = true; msg_vehicle_control_mode.flag_control_auto_enabled = false; msg_vehicle_control_mode.flag_control_rates_enabled = !msg_offboard_control_mode.ignore_bodyrate || - !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_attitude || + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_attitude_enabled = !msg_offboard_control_mode.ignore_attitude || - !msg_offboard_control_mode.ignore_position || - !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_acceleration_force; + !msg_offboard_control_mode.ignore_position || + !msg_offboard_control_mode.ignore_velocity || + !msg_offboard_control_mode.ignore_acceleration_force; msg_vehicle_control_mode.flag_control_velocity_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_climb_rate_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_position_enabled = !msg_offboard_control_mode.ignore_position; msg_vehicle_control_mode.flag_control_altitude_enabled = !msg_offboard_control_mode.ignore_velocity || - !msg_offboard_control_mode.ignore_position; + !msg_offboard_control_mode.ignore_position; } void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode) { + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode) +{ // XXX this is a minimal implementation. If more advanced functionalities are // needed consider a full port of the commander - if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) - { + if (msg->offboard_switch == px4::manual_control_setpoint::SWITCH_POS_ON) { SetOffboardControl(_msg_offboard_control_mode, msg_vehicle_control_mode); return; } @@ -155,7 +155,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_offboard_enabled = false; switch (msg->mode_switch) { - case px4::manual_control_setpoint::SWITCH_POS_NONE: + case px4::manual_control_setpoint::SWITCH_POS_NONE: ROS_WARN("Joystick button mapping error, main mode not set"); break; @@ -183,6 +183,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_climb_rate_enabled = true; msg_vehicle_control_mode.flag_control_position_enabled = true; msg_vehicle_control_mode.flag_control_velocity_enabled = true; + } else { msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_ALTCTL; msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_ALTCTL; @@ -194,6 +195,7 @@ void Commander::EvalSwitches(const px4::manual_control_setpointConstPtr &msg, msg_vehicle_control_mode.flag_control_position_enabled = false; msg_vehicle_control_mode.flag_control_velocity_enabled = false; } + break; } @@ -206,20 +208,20 @@ void Commander::OffboardControlModeCallback(const px4::offboard_control_modeCons /* Force system into offboard control mode */ if (!_got_manual_control) { SetOffboardControl(_msg_offboard_control_mode, _msg_vehicle_control_mode); - + _msg_vehicle_status.timestamp = px4::get_time_micros(); _msg_vehicle_status.hil_state = _msg_vehicle_status.HIL_STATE_OFF; _msg_vehicle_status.hil_state = _msg_vehicle_status.VEHICLE_TYPE_QUADROTOR; _msg_vehicle_status.is_rotary_wing = true; _msg_vehicle_status.arming_state = _msg_vehicle_status.ARMING_STATE_ARMED; - + _msg_actuator_armed.armed = true; _msg_actuator_armed.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.timestamp = px4::get_time_micros(); _msg_vehicle_control_mode.flag_armed = true; - + _vehicle_control_mode_pub.publish(_msg_vehicle_control_mode); _actuator_armed_pub.publish(_msg_actuator_armed); diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h index 856144389d..67bee544ab 100644 --- a/src/platforms/ros/nodes/commander/commander.h +++ b/src/platforms/ros/nodes/commander/commander.h @@ -68,14 +68,14 @@ protected: * Set control mode flags based on stick positions (equiv to code in px4 commander) */ void EvalSwitches(const px4::manual_control_setpointConstPtr &msg, - px4::vehicle_status &msg_vehicle_status, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_status &msg_vehicle_status, + px4::vehicle_control_mode &msg_vehicle_control_mode); /** * Sets offboard controll flags in msg_vehicle_control_mode */ void SetOffboardControl(const px4::offboard_control_mode &msg_offboard_control_mode, - px4::vehicle_control_mode &msg_vehicle_control_mode); + px4::vehicle_control_mode &msg_vehicle_control_mode); ros::NodeHandle _n; ros::Subscriber _man_ctrl_sp_sub; diff --git a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp index 328f545c6b..7b02a6f07e 100644 --- a/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp +++ b/src/platforms/ros/nodes/demo_offboard_attitude_setpoints/demo_offboard_attitude_setpoints.cpp @@ -65,15 +65,18 @@ int DemoOffboardAttitudeSetpoints::main() /* Publish example offboard attitude setpoint */ geometry_msgs::PoseStamped pose; - tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , 0.0); + tf::Quaternion q = tf::createQuaternionFromRPY(0.0, 0.1 * (sinf(0.5 * (float)px4::get_time_micros() / 1000000.0f)) , + 0.0); quaternionTFToMsg(q, pose.pose.orientation); _attitude_sp_pub.publish(pose); std_msgs::Float64 thrust; - thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / 1000000.0f)); // just some example throttle input that makes the quad 'jump' + thrust.data = 0.4f + 0.25 * (sinf((float)px4::get_time_micros() / + 1000000.0f)); // just some example throttle input that makes the quad 'jump' _thrust_sp_pub.publish(thrust); } + return 0; } diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp index 8488c518f5..fc8db220c3 100644 --- a/src/platforms/ros/nodes/manual_input/manual_input.cpp +++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp @@ -116,12 +116,14 @@ void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, do if (val + offset > deadzone || val + offset < -deadzone) { out = (float)((val + offset)) * scale; + } else { out = 0.0f; } } -void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) { +void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_control_setpoint &msg_mc_sp) +{ msg_mc_sp.acro_switch = px4::manual_control_setpoint::SWITCH_POS_NONE; if (_buttons_state[MAIN_STATE_MANUAL] != msg->buttons[_param_buttons_map[MAIN_STATE_MANUAL]] == true) { @@ -150,6 +152,7 @@ void ManualInput::MapButtons(const sensor_msgs::JoyConstPtr &msg, px4::manual_co msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; msg_mc_sp.offboard_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; return; + } else if (_buttons_state[MAIN_STATE_OFFBOARD] != msg->buttons[_param_buttons_map[MAIN_STATE_OFFBOARD]] == true) { msg_mc_sp.mode_switch = px4::manual_control_setpoint::SWITCH_POS_MIDDLE; msg_mc_sp.return_switch = px4::manual_control_setpoint::SWITCH_POS_OFF; diff --git a/src/platforms/ros/nodes/mavlink/mavlink.cpp b/src/platforms/ros/nodes/mavlink/mavlink.cpp index 567acc200e..e015266809 100644 --- a/src/platforms/ros/nodes/mavlink/mavlink.cpp +++ b/src/platforms/ros/nodes/mavlink/mavlink.cpp @@ -66,7 +66,8 @@ int main(int argc, char **argv) ros::init(argc, argv, "mavlink"); ros::NodeHandle privateNh("~"); std::string mavlink_fcu_url; - privateNh.param("mavlink_fcu_url", mavlink_fcu_url, std::string("udp://localhost:14565@localhost:14560")); + privateNh.param("mavlink_fcu_url", mavlink_fcu_url, + std::string("udp://localhost:14565@localhost:14560")); Mavlink m(mavlink_fcu_url); ros::spin(); return 0; @@ -76,18 +77,18 @@ void Mavlink::VehicleAttitudeCallback(const vehicle_attitudeConstPtr &msg) { mavlink_message_t msg_m; mavlink_msg_attitude_quaternion_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->q[0], - msg->q[1], - msg->q[2], - msg->q[3], - msg->rollspeed, - msg->pitchspeed, - msg->yawspeed); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->q[0], + msg->q[1], + msg->q[2], + msg->q[3], + msg->rollspeed, + msg->pitchspeed, + msg->yawspeed); _link->send_message(&msg_m); } @@ -95,33 +96,36 @@ void Mavlink::VehicleLocalPositionCallback(const vehicle_local_positionConstPtr { mavlink_message_t msg_m; mavlink_msg_local_position_ned_pack_chan( - _link->get_system_id(), - _link->get_component_id(), - _link->get_channel(), - &msg_m, - get_time_micros() / 1000, - msg->x, - msg->y, - msg->z, - msg->vx, - msg->vy, - msg->vz); + _link->get_system_id(), + _link->get_component_id(), + _link->get_channel(), + &msg_m, + get_time_micros() / 1000, + msg->x, + msg->y, + msg->z, + msg->vx, + msg->vy, + msg->vz); _link->send_message(&msg_m); } -void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) { +void Mavlink::handle_msg(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) +{ (void)sysid; (void)compid; - switch(mmsg->msgid) { - case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: - handle_msg_set_attitude_target(mmsg); - break; - case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: - handle_msg_set_position_target_local_ned(mmsg); - break; - default: - break; + switch (mmsg->msgid) { + case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: + handle_msg_set_attitude_target(mmsg); + break; + + case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: + handle_msg_set_position_target_local_ned(mmsg); + break; + + default: + break; } } @@ -146,10 +150,12 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) /* Message want's us to ignore everything except thrust: only ignore if previously ignored */ _offboard_control_mode.ignore_bodyrate = ignore_bodyrate && _offboard_control_mode.ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude && _offboard_control_mode.ignore_attitude; + } else { _offboard_control_mode.ignore_bodyrate = ignore_bodyrate; _offboard_control_mode.ignore_attitude = ignore_attitude; } + _offboard_control_mode.ignore_position = true; _offboard_control_mode.ignore_velocity = true; _offboard_control_mode.ignore_acceleration_force = true; @@ -162,13 +168,14 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) */ _att_sp.timestamp = get_time_micros(); + if (!ignore_attitude) { mavlink_quaternion_to_euler(set_attitude_target.q, &_att_sp.roll_body, &_att_sp.pitch_body, - &_att_sp.yaw_body); + &_att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])_att_sp.R_body.data()); _att_sp.R_valid = true; } - + if (!_offboard_control_mode.ignore_thrust) { _att_sp.thrust = set_attitude_target.thrust; @@ -178,6 +185,7 @@ void Mavlink::handle_msg_set_attitude_target(const mavlink_message_t *mmsg) for (ssize_t i = 0; i < 4; i++) { _att_sp.q_d[i] = set_attitude_target.q[i]; } + _att_sp.q_d_valid = true; } @@ -200,9 +208,9 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * /* Only accept messages which are intended for this system */ // XXX removed for sitl, makes maybe sense to re-introduce at some point // if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - // set_position_target_local_ned.target_system == 0) && - // (mavlink_system.compid == set_position_target_local_ned.target_component || - // set_position_target_local_ned.target_component == 0)) { + // set_position_target_local_ned.target_system == 0) && + // (mavlink_system.compid == set_position_target_local_ned.target_component || + // set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ offboard_control_mode.ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7); @@ -223,7 +231,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * * gets published only if in offboard mode. We leave that out for now. */ if (is_force_sp && offboard_control_mode.ignore_position && - offboard_control_mode.ignore_velocity) { + offboard_control_mode.ignore_velocity) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ vehicle_force_setpoint force_sp; @@ -233,6 +241,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * //XXX: yaw _force_sp_pub.publish(force_sp); + } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ position_setpoint_triplet pos_sp_triplet; @@ -247,6 +256,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.x = set_position_target_local_ned.x; pos_sp_triplet.current.y = set_position_target_local_ned.y; pos_sp_triplet.current.z = set_position_target_local_ned.z; + } else { pos_sp_triplet.current.position_valid = false; } @@ -257,6 +267,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * pos_sp_triplet.current.vx = set_position_target_local_ned.vx; pos_sp_triplet.current.vy = set_position_target_local_ned.vy; pos_sp_triplet.current.vz = set_position_target_local_ned.vz; + } else { pos_sp_triplet.current.velocity_valid = false; } @@ -292,6 +303,7 @@ void Mavlink::handle_msg_set_position_target_local_ned(const mavlink_message_t * } else { pos_sp_triplet.current.yawspeed_valid = false; } + //XXX handle global pos setpoints (different MAV frames) _pos_sp_triplet_pub.publish(pos_sp_triplet); diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp index 002a112b69..67084c64bb 100644 --- a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp +++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp @@ -221,14 +221,19 @@ void MultirotorMixer::actuatorControlsCallback(const px4::actuator_controls_0 &m // switch mixer if necessary std::string mixer_name; _n.getParamCached("mixer", mixer_name); + if (mixer_name == "x") { _rotors = _config_index[0]; + } else if (mixer_name == "+") { _rotors = _config_index[1]; + } else if (mixer_name == "e") { _rotors = _config_index[2]; + } else if (mixer_name == "w") { _rotors = _config_index[3]; + } else if (mixer_name == "i") { _rotors = _config_index[4]; } From 72757662234d9538ea9f3a3240153fc74afe8975 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:36:15 +0200 Subject: [PATCH 063/155] Platforms: Fix formatting --- src/platforms/common/px4_getopt.c | 48 ++++++++++++++----- .../nuttx/px4_layer/px4_nuttx_tasks.c | 5 +- 2 files changed, 39 insertions(+), 14 deletions(-) diff --git a/src/platforms/common/px4_getopt.c b/src/platforms/common/px4_getopt.c index d7d03ed49f..863b5816b4 100644 --- a/src/platforms/common/px4_getopt.c +++ b/src/platforms/common/px4_getopt.c @@ -44,13 +44,19 @@ static char isvalidopt(char p, const char *options, int *takesarg) { int idx = 0; *takesarg = 0; - while (options[idx] != 0 && p != options[idx]) + + while (options[idx] != 0 && p != options[idx]) { ++idx; - if (options[idx] == 0) + } + + if (options[idx] == 0) { return '?'; - if (options[idx+1] == ':') { + } + + if (options[idx + 1] == ':') { *takesarg = 1; } + return options[idx]; } @@ -67,40 +73,50 @@ static int reorder(int argc, char **argv, const char *options) while (idx < argc && argv[idx] != 0) { if (argv[idx][0] == '-') { c = isvalidopt(argv[idx][1], options, &takesarg); - if (c == '?') + + if (c == '?') { return 1; + } + tmp_argv[tmpidx] = argv[idx]; tmpidx++; + if (takesarg) { - tmp_argv[tmpidx] = argv[idx+1]; + tmp_argv[tmpidx] = argv[idx + 1]; // printf("tmp_argv[%d] = %s\n", tmpidx, tmp_argv[tmpidx]); tmpidx++; idx++; } } + idx++; } // Add non-options to the end idx = 1; + while (idx < argc && argv[idx] != 0) { if (argv[idx][0] == '-') { c = isvalidopt(argv[idx][1], options, &takesarg); - if (c == '?') + + if (c == '?') { return c; + } + if (takesarg) { idx++; } - } - else { + + } else { tmp_argv[tmpidx] = argv[idx]; tmpidx++; } + idx++; } // Reorder argv - for (idx=1; idx < argc; idx++) { + for (idx = 1; idx < argc; idx++) { argv[idx] = tmp_argv[idx]; } @@ -128,24 +144,32 @@ __EXPORT int px4_getopt(int argc, char *argv[], const char *options, int *myopti int takesarg; if (*myoptind == 1) - if (reorder(argc, argv, options) != 0) + if (reorder(argc, argv, options) != 0) { return (int)'?'; + } p = argv[*myoptind]; - if (*myoptarg == 0) + if (*myoptarg == 0) { *myoptarg = argv[*myoptind]; + } if (p && options && myoptind && p[0] == '-') { c = isvalidopt(p[1], options, &takesarg); - if (c == '?') + + if (c == '?') { return (int)c; + } + *myoptind += 1; + if (takesarg) { *myoptarg = argv[*myoptind]; *myoptind += 1; } + return (int)c; } + return -1; } diff --git a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c index 1849374fb2..bb4dea7d4a 100644 --- a/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c +++ b/src/platforms/nuttx/px4_layer/px4_nuttx_tasks.c @@ -70,13 +70,14 @@ px4_systemreset(bool to_bootloader) /* XXX wow, this is evil - write a magic number into backup register zero */ *(uint32_t *)0x40002850 = 0xb007b007; } + up_systemreset(); /* lock up here */ - while(true); + while (true); } -int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) +int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) { int pid; From 59ee7b23eeed756bc8659386769bba917e142ceb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:36:29 +0200 Subject: [PATCH 064/155] Enforce style check on platforms and drivers --- Tools/check_code_style.sh | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index bc59128317..36849d3a19 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -4,15 +4,8 @@ failed=0 for fn in $(find src/examples \ src/systemcmds \ src/include \ - src/drivers/blinkm \ - src/drivers/bma180 \ - src/drivers/pca9685 \ - src/drivers/pca8574 \ - src/drivers/md25 \ - src/drivers/ms5611 \ - src/drivers/stm32 \ - src/drivers/px4io \ - src/drivers/px4fmu \ + src/drivers \ + src/platforms \ src/lib/launchdetection \ src/modules/bottle_drop \ src/modules/dataman \ From 948ff80b8062d438b843c6a9fbb47599881600a3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:46:32 +0200 Subject: [PATCH 065/155] IO firmware: Fix code style --- src/modules/px4iofirmware/adc.c | 9 +- src/modules/px4iofirmware/dsm.c | 50 ++-- src/modules/px4iofirmware/i2c.c | 50 ++-- src/modules/px4iofirmware/mixer.cpp | 99 ++++---- src/modules/px4iofirmware/protocol.h | 18 +- src/modules/px4iofirmware/px4io.c | 45 ++-- src/modules/px4iofirmware/registers.c | 331 +++++++++++++++----------- src/modules/px4iofirmware/safety.c | 4 +- src/modules/px4iofirmware/serial.c | 40 +++- 9 files changed, 395 insertions(+), 251 deletions(-) diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c index d3db26c233..1688d53048 100644 --- a/src/modules/px4iofirmware/adc.c +++ b/src/modules/px4iofirmware/adc.c @@ -96,19 +96,22 @@ adc_init(void) rCR2 |= ADC_CR2_RSTCAL; up_udelay(1); - if (rCR2 & ADC_CR2_RSTCAL) + if (rCR2 & ADC_CR2_RSTCAL) { return -1; + } rCR2 |= ADC_CR2_CAL; up_udelay(100); - if (rCR2 & ADC_CR2_CAL) + if (rCR2 & ADC_CR2_CAL) { return -1; + } + #endif /* * Configure sampling time. - * + * * For electrical protection reasons, we want to be able to have * 10K in series with ADC inputs that leave the board. At 12MHz this * means we need 28.5 cycles of sampling time (per table 43 in the diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index b0e96b1f03..fc6fbf82b8 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -90,8 +90,9 @@ static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value) { - if (raw == 0xffff) + if (raw == 0xffff) { return false; + } *channel = (raw >> shift) & 0xf; @@ -132,18 +133,21 @@ dsm_guess_format(bool reset) unsigned channel, value; /* if the channel decodes, remember the assigned number */ - if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 10, &channel, &value) && (channel < 31)) { cs10 |= (1 << channel); + } - if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) + if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31)) { cs11 |= (1 << channel); + } /* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */ } /* wait until we have seen plenty of frames - 5 should normally be enough */ - if (samples++ < 5) + if (samples++ < 5) { return; + } /* * Iterate the set of sensible sniffed channel sets and see whether @@ -170,11 +174,13 @@ dsm_guess_format(bool reset) for (unsigned i = 0; i < (sizeof(masks) / sizeof(masks[0])); i++) { - if (cs10 == masks[i]) + if (cs10 == masks[i]) { votes10++; + } - if (cs11 == masks[i]) + if (cs11 == masks[i]) { votes11++; + } } if ((votes11 == 1) && (votes10 == 0)) { @@ -210,8 +216,9 @@ dsm_init(const char *device) POWER_SPEKTRUM(true); #endif - if (dsm_fd < 0) + if (dsm_fd < 0) { dsm_fd = open(device, O_RDONLY | O_NONBLOCK); + } if (dsm_fd >= 0) { @@ -251,13 +258,14 @@ void dsm_bind(uint16_t cmd, int pulses) { #if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) - #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; - if (dsm_fd < 0) + if (dsm_fd < 0) { return; + } switch (cmd) { @@ -297,6 +305,7 @@ dsm_bind(uint16_t cmd, int pulses) up_udelay(120); stm32_gpiowrite(usart1RxAsOutp, true); } + break; case dsm_bind_reinit_uart: @@ -306,6 +315,7 @@ dsm_bind(uint16_t cmd, int pulses) break; } + #endif } @@ -329,8 +339,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * If we have lost signal for at least a second, reset the * format guessing heuristic. */ - if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) + if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0)) { dsm_guess_format(true); + } /* we have received something we think is a dsm_frame */ dsm_last_frame_time = frame_time; @@ -358,20 +369,24 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; - if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) + if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value)) { continue; + } /* ignore channels out of range */ - if (channel >= PX4IO_RC_INPUT_CHANNELS) + if (channel >= PX4IO_RC_INPUT_CHANNELS) { continue; + } /* update the decoded channel count */ - if (channel >= *num_values) + if (channel >= *num_values) { *num_values = channel + 1; + } /* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */ - if (dsm_channel_shift == 10) + if (dsm_channel_shift == 10) { value *= 2; + } /* * Spektrum scaling is special. There are these basic considerations @@ -421,8 +436,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * lines, so if we get a channel count of 13, we'll return 12 (the last * data index that is stable). */ - if (*num_values == 13) + if (*num_values == 13) { *num_values = 12; + } if (dsm_channel_shift == 11) { /* Set the 11-bit data indicator */ @@ -482,6 +498,7 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* if the read failed for any reason, just give up here */ if (ret < 1) { return false; + } else { *n_bytes = ret; *bytes = &dsm_frame[dsm_partial_frame_count]; @@ -497,8 +514,9 @@ dsm_input(uint16_t *values, uint16_t *num_values, uint8_t *n_bytes, uint8_t **by /* * If we don't have a full dsm frame, return */ - if (dsm_partial_frame_count < DSM_FRAME_SIZE) + if (dsm_partial_frame_count < DSM_FRAME_SIZE) { return false; + } /* * Great, it looks like we might have a dsm frame. Go ahead and diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 6d1d1fc2dd..74692a2675 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -120,7 +120,7 @@ interface_init(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -128,8 +128,11 @@ interface_init(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -163,7 +166,7 @@ i2c_reset(void) rCR1 = 0; /* set for DMA operation */ - rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN; + rCR2 |= I2C_CR2_ITEVFEN | I2C_CR2_ITERREN | I2C_CR2_DMAEN; /* set the frequency value in CR2 */ rCR2 &= ~I2C_CR2_FREQ_MASK; @@ -171,8 +174,11 @@ i2c_reset(void) /* set divisor and risetime for fast mode */ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25); - if (result < 1) + + if (result < 1) { result = 1; + } + result = 3; rCCR &= ~I2C_CCR_CCR_MASK; rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result; @@ -203,13 +209,16 @@ i2c_interrupt(int irq, FAR void *context) case DIR_TX: i2c_tx_complete(); break; + case DIR_RX: i2c_rx_complete(); break; + default: /* not currently transferring - must be a new txn */ break; } + direction = DIR_NONE; } @@ -232,8 +241,9 @@ i2c_interrupt(int irq, FAR void *context) } /* clear any errors that might need it (this handles AF as well */ - if (sr1 & I2C_SR1_ERRORMASK) + if (sr1 & I2C_SR1_ERRORMASK) { rSR1 = 0; + } return 0; } @@ -248,13 +258,13 @@ i2c_rx_setup(void) */ rx_len = 0; stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf), - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_32BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_32BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); - stm32_dmastart(rx_dma, NULL, NULL, false); + stm32_dmastart(rx_dma, NULL, NULL, false); } static void @@ -269,8 +279,10 @@ i2c_rx_complete(void) /* work out how many registers are being written */ unsigned count = (rx_len - 2) / 2; + if (count > 0) { registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count); + } else { /* no registers written, must be an address cycle */ uint16_t *regs; @@ -278,9 +290,11 @@ i2c_rx_complete(void) /* work out which registers are being addressed */ int ret = registers_get(selected_page, selected_offset, ®s, ®_count); + if (ret == 0) { tx_buf = (uint8_t *)regs; tx_len = reg_count * 2; + } else { tx_buf = junk_buf; tx_len = sizeof(junk_buf); @@ -306,16 +320,16 @@ i2c_tx_setup(void) /* * Note that we configure DMA in circular mode; this means that a too-long * transfer will copy the buffer more than once, but that avoids us having - * to deal with bailing out of a transaction while the master is still + * to deal with bailing out of a transaction while the master is still * babbling at us. */ stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len, - DMA_CCR_DIR | - DMA_CCR_CIRC | - DMA_CCR_MINC | - DMA_CCR_PSIZE_8BITS | - DMA_CCR_MSIZE_8BITS | - DMA_CCR_PRIMED); + DMA_CCR_DIR | + DMA_CCR_CIRC | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS | + DMA_CCR_PRIMED); stm32_dmastart(tx_dma, NULL, NULL, false); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b608d5fc93..b15ec09610 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -97,12 +97,13 @@ mixer_tick(void) /* check that we are receiving fresh data from the FMU */ if ((system_state.fmu_data_received_time == 0) || - hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; @@ -122,7 +123,7 @@ mixer_tick(void) /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; @@ -130,28 +131,29 @@ mixer_tick(void) } else { if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* mix from FMU controls */ source = MIX_FMU; } - if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && - /* do not enter manual override if we asked for termination failsafe and FMU is lost */ - !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { - /* if allowed, mix from RC inputs directly */ + /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; - } else if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && - !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + + } else if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { /* if allowed, mix from RC inputs directly up to available rc channels */ source = MIX_OVERRIDE_FMU_OK; @@ -166,31 +168,32 @@ mixer_tick(void) * */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); should_arm_nothrottle = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ( /* if we have requested flight termination style failsafe (noreturn) */ + if ( /* if we have requested flight termination style failsafe (noreturn) */ (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && @@ -213,6 +216,7 @@ mixer_tick(void) */ if (source == MIX_FAILSAFE) { r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; + } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); } @@ -244,7 +248,8 @@ mixer_tick(void) in_mixer = false; /* the pwm limit call takes care of out of band errors */ - pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); + pwm_limit_calc(should_arm, should_arm_nothrottle, mixed, r_setup_pwm_reverse, r_page_servo_disarmed, + r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); /* clamp unused outputs to zero */ for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) { @@ -291,6 +296,7 @@ mixer_tick(void) if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); } @@ -324,10 +330,11 @@ mixer_callback(uintptr_t handle, switch (source) { case MIX_FMU: - if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS ) { + if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_OVERRIDE: @@ -335,17 +342,21 @@ mixer_callback(uintptr_t handle, control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; } + return -1; case MIX_OVERRIDE_FMU_OK: + /* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) { control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]); break; + } else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) { control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]); break; } + return -1; case MIX_FAILSAFE: @@ -357,15 +368,15 @@ mixer_callback(uintptr_t handle, /* apply trim offsets for override channels */ if (source == MIX_OVERRIDE || source == MIX_OVERRIDE_FMU_OK) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_ROLL) { + control_index == actuator_controls_s::INDEX_ROLL) { control += REG_TO_FLOAT(r_setup_trim_roll); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_PITCH) { + control_index == actuator_controls_s::INDEX_PITCH) { control += REG_TO_FLOAT(r_setup_trim_pitch); } else if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_YAW) { + control_index == actuator_controls_s::INDEX_YAW) { control += REG_TO_FLOAT(r_setup_trim_yaw); } } @@ -373,6 +384,7 @@ mixer_callback(uintptr_t handle, /* limit output */ if (control > 1.0f) { control = 1.0f; + } else if (control < -1.0f) { control = -1.0f; } @@ -380,7 +392,7 @@ mixer_callback(uintptr_t handle, /* motor spinup phase - lock throttle to zero */ if ((pwm_limit.state == PWM_LIMIT_STATE_RAMP) || (should_arm_nothrottle && !should_arm)) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* limit the throttle output to zero during motor spinup, * as the motors cannot follow any demand yet */ @@ -391,7 +403,7 @@ mixer_callback(uintptr_t handle, /* only safety off, but not armed - set throttle as invalid */ if (should_arm_nothrottle && !should_arm) { if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { + control_index == actuator_controls_s::INDEX_THROTTLE) { /* mark the throttle as invalid */ control = NAN_VALUE; } @@ -414,7 +426,7 @@ mixer_handle_text(const void *buffer, size_t length) { /* do not allow a mixer change while safety off and FMU armed */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { return 1; } @@ -444,7 +456,7 @@ mixer_handle_text(const void *buffer, size_t length) mixer_group.reset(); mixer_text_length = 0; - /* FALLTHROUGH */ + /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: isr_debug(2, "append %d", length); @@ -470,6 +482,7 @@ mixer_handle_text(const void *buffer, size_t length) /* only set mixer ok if no residual is left over */ if (resid == 0) { r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + } else { /* not yet reached the end of the mixer, set as not ok */ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; @@ -497,13 +510,13 @@ mixer_handle_text(const void *buffer, size_t length) static void mixer_set_failsafe() { - /* + /* * Check if a custom failsafe value has been written, * or if the mixer is not ok and bail out. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 673da0e44d..1267b48853 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -46,7 +46,7 @@ * * The first two bytes of each write select a page and offset address * respectively. Subsequent reads and writes increment the offset within - * the page. + * the page. * * Some pages are read- or write-only. * @@ -56,7 +56,7 @@ * Writes to unimplemented registers are ignored. Reads from unimplemented * registers return undefined values. * - * As convention, values that would be floating point in other parts of + * As convention, values that would be floating point in other parts of * the PX4 system are expressed as signed integer values scaled by 10000, * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and * SIGNED_TO_REG macros to convert between register representation and @@ -66,7 +66,7 @@ * readable pages to be densely packed. Page numbers do not need to be * packed. * - * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, + * Definitions marked [1] are only valid on PX4IOv1 boards. Likewise, * [2] denotes definitions specific to the PX4IOv2 board. */ @@ -218,14 +218,14 @@ enum { /* DSM bind states */ dsm_bind_send_pulses, dsm_bind_reinit_uart }; - /* 8 */ +/* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ #define PX4IO_P_SETUP_REBOOT_BL 10 /* reboot IO into bootloader */ #define PX4IO_REBOOT_BL_MAGIC 14662 /* required argument for reboot (random) */ #define PX4IO_P_SETUP_CRC 11 /* get CRC of IO firmware */ - /* storage space of 12 occupied by CRC */ +/* storage space of 12 occupied by CRC */ #define PX4IO_P_SETUP_FORCE_SAFETY_OFF 12 /* force safety switch into 'armed' (PWM enabled) state - this is a non-data write and hence index 12 can safely be used. */ @@ -339,8 +339,7 @@ struct IOPacket { #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) #define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) -static const uint8_t crc8_tab[256] __attribute__((unused)) = -{ +static const uint8_t crc8_tab[256] __attribute__((unused)) = { 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, @@ -383,8 +382,9 @@ crc_packet(struct IOPacket *pkt) uint8_t *p = (uint8_t *)pkt; uint8_t c = 0; - while (p < end) - c = crc8_tab[c ^ *(p++)]; + while (p < end) { + c = crc8_tab[c ^ * (p++)]; + } return c; } diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 94f1fc11ef..1ee50d5a7f 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -97,11 +97,12 @@ isr_debug(uint8_t level, const char *fmt, ...) if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { return; } + va_list ap; va_start(ap, fmt); vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap); va_end(ap); - msg_next_in = (msg_next_in+1) % NUM_MSG; + msg_next_in = (msg_next_in + 1) % NUM_MSG; msg_counter++; } @@ -113,11 +114,14 @@ show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; - if (n > NUM_MSG) n = NUM_MSG; + + if (n > NUM_MSG) { n = NUM_MSG; } + last_msg_counter = msg_counter; + while (n--) { debug("%s", msg[msg_next_out]); - msg_next_out = (msg_next_out+1) % NUM_MSG; + msg_next_out = (msg_next_out + 1) % NUM_MSG; } } } @@ -135,7 +139,7 @@ ring_blink(void) #ifdef GPIO_LED4 if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + /* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { LED_RING(1); return; } @@ -151,7 +155,7 @@ ring_blink(void) if (brightness_counter < max_brightness) { - bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1); + bool on = ((on_counter * 100) / brightness_counter + 1) <= ((brightness * 100) / max_brightness + 1); // XXX once led is PWM driven, // remove the ! in the line below @@ -195,7 +199,7 @@ static uint64_t reboot_time; */ void schedule_reboot(uint32_t time_delta_usec) { - reboot_time = hrt_absolute_time() + time_delta_usec; + reboot_time = hrt_absolute_time() + time_delta_usec; } /** @@ -203,9 +207,9 @@ void schedule_reboot(uint32_t time_delta_usec) */ static void check_reboot(void) { - if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { - up_systemreset(); - } + if (reboot_time != 0 && hrt_absolute_time() > reboot_time) { + up_systemreset(); + } } static void @@ -215,12 +219,14 @@ calculate_fw_crc(void) #define APP_LOAD_ADDRESS 0x08001000 // compute CRC of the current firmware uint32_t sum = 0; + for (unsigned p = 0; p < APP_SIZE_MAX; p += 4) { uint32_t bytes = *(uint32_t *)(p + APP_LOAD_ADDRESS); sum = crc32part((uint8_t *)&bytes, sizeof(bytes), sum); } + r_page_setup[PX4IO_P_SETUP_CRC] = sum & 0xFFFF; - r_page_setup[PX4IO_P_SETUP_CRC+1] = sum >> 16; + r_page_setup[PX4IO_P_SETUP_CRC + 1] = sum >> 16; } int @@ -308,7 +314,7 @@ user_start(int argc, char *argv[]) * allocations. We don't want him to be able to * get past that point. This needs to be clearly * documented in the dev guide. - * + * */ if (minfo.mxordblk < 600) { @@ -320,10 +326,12 @@ user_start(int argc, char *argv[]) if (phase) { LED_AMBER(true); LED_BLUE(false); + } else { LED_AMBER(false); LED_BLUE(true); } + up_udelay(250000); phase = !phase; @@ -338,7 +346,8 @@ user_start(int argc, char *argv[]) */ uint64_t last_debug_time = 0; - uint64_t last_heartbeat_time = 0; + uint64_t last_heartbeat_time = 0; + for (;;) { /* track the rate at which the loop is running */ @@ -354,14 +363,14 @@ user_start(int argc, char *argv[]) controls_tick(); perf_end(controls_perf); - if ((hrt_absolute_time() - last_heartbeat_time) > 250*1000) { - last_heartbeat_time = hrt_absolute_time(); - heartbeat_blink(); - } + if ((hrt_absolute_time() - last_heartbeat_time) > 250 * 1000) { + last_heartbeat_time = hrt_absolute_time(); + heartbeat_blink(); + } ring_blink(); - check_reboot(); + check_reboot(); /* check for debug activity (default: none) */ show_debug_messages(); @@ -371,7 +380,7 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8adc91cda9..46852003c8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -112,15 +112,14 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; * * Raw RC input */ -uint16_t r_page_raw_rc_input[] = -{ +uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, [PX4IO_P_RAW_RC_FLAGS] = 0, [PX4IO_P_RAW_RC_NRSSI] = 0, [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ...(PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; /** @@ -130,7 +129,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ...(PX4IO_P_RC_BASE + PX4IO_RC_MAPPED_CONTROL_CHANNELS)] = 0 }; /** @@ -146,8 +145,7 @@ uint16_t r_page_scratch[32]; * * Setup registers */ -volatile uint16_t r_page_setup[] = -{ +volatile uint16_t r_page_setup[] = { #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 /* default to RSSI ADC functionality */ [PX4IO_P_SETUP_FEATURES] = PX4IO_P_SETUP_FEATURES_ADC_RSSI, @@ -171,7 +169,7 @@ volatile uint16_t r_page_setup[] = #endif [PX4IO_P_SETUP_SET_DEBUG] = 0, [PX4IO_P_SETUP_REBOOT_BL] = 0, - [PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0, + [PX4IO_P_SETUP_CRC ...(PX4IO_P_SETUP_CRC + 1)] = 0, [PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0, [PX4IO_P_SETUP_PWM_REVERSE] = 0, [PX4IO_P_SETUP_TRIM_ROLL] = 0, @@ -181,23 +179,23 @@ volatile uint16_t r_page_setup[] = #ifdef CONFIG_ARCH_BOARD_PX4IO_V2 #define PX4IO_P_SETUP_FEATURES_VALID (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | \ - PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ - PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ - PX4IO_P_SETUP_FEATURES_PWM_RSSI) + PX4IO_P_SETUP_FEATURES_SBUS2_OUT | \ + PX4IO_P_SETUP_FEATURES_ADC_RSSI | \ + PX4IO_P_SETUP_FEATURES_PWM_RSSI) #else #define PX4IO_P_SETUP_FEATURES_VALID 0 #endif #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ - PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ - PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ - PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ - PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ + PX4IO_P_SETUP_ARMING_LOCKDOWN | \ + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -230,7 +228,7 @@ uint16_t r_page_rc_input_config[PX4IO_RC_INPUT_CHANNELS * PX4IO_P_RC_CONFIG_STR * PAGE 105 * * Failsafe servo PWM values - * + * * Disable pulses as default. */ uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 }; @@ -265,7 +263,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num switch (page) { - /* handle bulk controls input */ + /* handle bulk controls input */ case PX4IO_PAGE_CONTROLS: /* copy channel data */ @@ -282,10 +280,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - + break; - /* handle raw PWM input */ + /* handle raw PWM input */ case PX4IO_PAGE_DIRECT_PWM: /* copy channel data */ @@ -306,7 +304,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num break; - /* handle setup for servo failsafe values */ + /* handle setup for servo failsafe values */ case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ @@ -316,8 +314,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values < PWM_LOWEST_MIN) { r_page_servo_failsafe[offset] = PWM_LOWEST_MIN; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX; + } else { r_page_servo_failsafe[offset] = *values; } @@ -329,6 +329,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; case PX4IO_PAGE_CONTROL_MIN_PWM: @@ -340,8 +341,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MIN) { r_page_servo_control_min[offset] = PWM_HIGHEST_MIN; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_control_min[offset] = PWM_LOWEST_MIN; + } else { r_page_servo_control_min[offset] = *values; } @@ -350,8 +353,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - + case PX4IO_PAGE_CONTROL_MAX_PWM: /* copy channel data */ @@ -361,8 +365,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* ignore 0 */ } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_control_max[offset] = PWM_HIGHEST_MAX; + } else if (*values < PWM_LOWEST_MAX) { r_page_servo_control_max[offset] = PWM_LOWEST_MAX; + } else { r_page_servo_control_max[offset] = *values; } @@ -371,10 +377,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num num_values--; values++; } + break; - case PX4IO_PAGE_DISARMED_PWM: - { + case PX4IO_PAGE_DISARMED_PWM: { /* flag for all outputs */ bool all_disarmed_off = true; @@ -384,12 +390,15 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (*values == 0) { /* 0 means disabling always PWM */ r_page_servo_disarmed[offset] = 0; + } else if (*values < PWM_LOWEST_MIN) { r_page_servo_disarmed[offset] = PWM_LOWEST_MIN; all_disarmed_off = false; + } else if (*values > PWM_HIGHEST_MAX) { r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX; all_disarmed_off = false; + } else { r_page_servo_disarmed[offset] = *values; all_disarmed_off = false; @@ -403,6 +412,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num if (all_disarmed_off) { /* disable PWM output if disarmed */ r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE); + } else { /* enable PWM output always */ r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; @@ -410,7 +420,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; - /* handle text going to the mixer parser */ + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: /* do not change the mixer if FMU is armed and IO's safety is off * this state defines an active system. This check is done in the @@ -419,19 +429,25 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num return mixer_handle_text(values, num_values * sizeof(*values)); default: + /* avoid offset wrap */ - if ((offset + num_values) > 255) + if ((offset + num_values) > 255) { num_values = 255 - offset; + } /* iterate individual registers, set each in turn */ while (num_values--) { - if (registers_set_one(page, offset, *values)) + if (registers_set_one(page, offset, *values)) { return -1; + } + offset++; values++; } + break; } + return 0; } @@ -448,19 +464,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_STATUS_FLAGS: - /* + + /* * Allow FMU override of arming state (to allow in-air restores), * but only if the arming state is not in sync on the IO side. */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { r_status_flags = value; } + break; default: /* just ignore writes to other registers in this page */ break; } + break; case PX4IO_PAGE_SETUP: @@ -472,36 +491,37 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* some of the options conflict - give S.BUS out precedence, then ADC RSSI, then PWM RSSI */ /* switch S.Bus output pin as needed */ - #ifdef ENABLE_SBUS_OUT +#ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); /* disable the conflicting options with SBUS 1 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with SBUS 2 */ if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } - #endif + +#endif /* disable the conflicting options with ADC RSSI */ if (value & (PX4IO_P_SETUP_FEATURES_ADC_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* disable the conflicting options with PWM RSSI (without effect here, but for completeness) */ if (value & (PX4IO_P_SETUP_FEATURES_PWM_RSSI)) { value &= ~(PX4IO_P_SETUP_FEATURES_ADC_RSSI | - PX4IO_P_SETUP_FEATURES_SBUS1_OUT | - PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + PX4IO_P_SETUP_FEATURES_SBUS1_OUT | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); } /* apply changes */ @@ -551,9 +571,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate); break; @@ -561,13 +583,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value < 25) { value = 25; } + if (value > 400) { value = 400; } + pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; @@ -598,10 +623,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; } - // we schedule a reboot rather than rebooting - // immediately to allow the IO board to ACK - // the reboot command - schedule_reboot(100000); + // we schedule a reboot rather than rebooting + // immediately to allow the IO board to ACK + // the reboot command + schedule_reboot(100000); break; case PX4IO_P_SETUP_DSM: @@ -612,18 +637,21 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } + break; case PX4IO_P_SETUP_FORCE_SAFETY_OFF: if (value == PX4IO_FORCE_SAFETY_MAGIC) { r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } + break; case PX4IO_P_SETUP_RC_THR_FAILSAFE_US: if (value > 650 && value < 2350) { r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value; } + break; case PX4IO_P_SETUP_PWM_REVERSE: @@ -639,93 +667,103 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) default: return -1; } + break; case PX4IO_PAGE_RC_CONFIG: { - /** - * do not allow a RC config change while safety is off - */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - break; - } - - unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; - unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; - uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - - if (channel >= PX4IO_RC_INPUT_CHANNELS) - return -1; - - /* disable the channel until we have a chance to sanity-check it */ - conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - switch (index) { - - case PX4IO_P_RC_CONFIG_MIN: - case PX4IO_P_RC_CONFIG_CENTER: - case PX4IO_P_RC_CONFIG_MAX: - case PX4IO_P_RC_CONFIG_DEADZONE: - case PX4IO_P_RC_CONFIG_ASSIGNMENT: - conf[index] = value; - break; - - case PX4IO_P_RC_CONFIG_OPTIONS: - value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; - - /* clear any existing RC disabled flag */ - r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); - - /* set all options except the enabled option */ - conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - - /* should the channel be enabled? */ - /* this option is normally set last */ - if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - uint8_t count = 0; - bool disabled = false; - - /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { - count++; - } - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { - count++; - } - /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { - count++; - } - - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { - disabled = true; - } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && - (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { - count++; - } - - /* sanity checks pass, enable channel */ - if (count) { - isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; - } else if (!disabled) { - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; - } + /** + * do not allow a RC config change while safety is off + */ + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { + break; } - break; - /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; + unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; + uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; + + if (channel >= PX4IO_RC_INPUT_CHANNELS) { + return -1; + } + + /* disable the channel until we have a chance to sanity-check it */ + conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + switch (index) { + + case PX4IO_P_RC_CONFIG_MIN: + case PX4IO_P_RC_CONFIG_CENTER: + case PX4IO_P_RC_CONFIG_MAX: + case PX4IO_P_RC_CONFIG_DEADZONE: + case PX4IO_P_RC_CONFIG_ASSIGNMENT: + conf[index] = value; + break; + + case PX4IO_P_RC_CONFIG_OPTIONS: + value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + + /* set all options except the enabled option */ + conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + + /* should the channel be enabled? */ + /* this option is normally set last */ + if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + bool disabled = false; + + /* assert min..center..max ordering */ + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + + /* assert deadzone is sane */ + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { + disabled = true; + + } else if ((conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) && + (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] != PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH)) { + count++; + } + + /* sanity checks pass, enable channel */ + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + + } else if (!disabled) { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } + } + + break; + /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ + + } + + break; + /* case PX4IO_RC_PAGE_CONFIG */ } - break; - /* case PX4IO_RC_PAGE_CONFIG */ - } case PX4IO_PAGE_TEST: switch (offset) { @@ -733,11 +771,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) LED_AMBER(value & 1); break; } + break; default: return -1; } + return 0; } @@ -787,6 +827,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val * */ unsigned counts = adc_measure(ADC_VBATT); + if (counts != 0xffff) { unsigned mV = (166460 + (counts * 45934)) / 10000; unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000; @@ -806,6 +847,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val configuration for their sensor */ unsigned counts = adc_measure(ADC_IBATT); + if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } @@ -815,6 +857,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VSERVO */ { unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { // use 3:1 scaling on 3.3V ADC input unsigned mV = counts * 9900 / 4096; @@ -826,6 +869,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_VRSSI */ { unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { // use 1:1 scaling on 3.3V ADC input unsigned mV = counts * 3300 / 4096; @@ -858,8 +902,10 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); + } SELECT_PAGE(r_page_scratch); break; @@ -872,15 +918,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONFIG: SELECT_PAGE(r_page_config); break; + case PX4IO_PAGE_ACTUATORS: SELECT_PAGE(r_page_actuators); break; + case PX4IO_PAGE_SERVOS: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_RAW_RC_INPUT: SELECT_PAGE(r_page_raw_rc_input); break; + case PX4IO_PAGE_RC_INPUT: SELECT_PAGE(r_page_rc_input); break; @@ -889,24 +939,31 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_SETUP: SELECT_PAGE(r_page_setup); break; + case PX4IO_PAGE_CONTROLS: SELECT_PAGE(r_page_controls); break; + case PX4IO_PAGE_RC_CONFIG: SELECT_PAGE(r_page_rc_input_config); break; + case PX4IO_PAGE_DIRECT_PWM: SELECT_PAGE(r_page_servos); break; + case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: SELECT_PAGE(r_page_servo_control_min); break; + case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_DISARMED_PWM: SELECT_PAGE(r_page_servo_disarmed); break; @@ -918,12 +975,13 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #undef SELECT_PAGE #undef COPY_PAGE -last_page = page; -last_offset = offset; + last_page = page; + last_offset = offset; /* if the offset is at or beyond the end of the page, we have no data */ - if (offset >= *num_values) + if (offset >= *num_values) { return -1; + } /* correct the data pointer and count for the offset */ *values += offset; @@ -943,8 +1001,10 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); - if (mask == 0) + + if (mask == 0) { continue; + } /* all channels in the group must be either default or alt-rate */ uint32_t alt = map & mask; @@ -956,18 +1016,23 @@ pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; return; } + } else { /* set it - errors here are unexpected */ if (alt != 0) { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } + } else { - if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) + if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK) { r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR; + } } } } } + r_setup_pwm_rates = map; r_setup_pwm_defaultrate = defaultrate; r_setup_pwm_altrate = altrate; diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 06f8733a52..f4ab74a45a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * length in all cases of the if/else struct below. */ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { + (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; @@ -149,6 +149,7 @@ safety_check_button(void *arg) } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_FMU_ARMED; + } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) { pattern = LED_PATTERN_FMU_OK_TO_ARM; @@ -176,6 +177,7 @@ failsafe_blink(void *arg) /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; + } else { failsafe = false; } diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c index e9adc8cd69..300ec0b784 100644 --- a/src/modules/px4iofirmware/serial.c +++ b/src/modules/px4iofirmware/serial.c @@ -131,14 +131,19 @@ interface_init(void) rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; #if 0 /* keep this for signal integrity testing */ + for (;;) { while (!(rSR & USART_SR_TXE)) ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) ; + rDR = 0xa0; } + #endif /* configure RX DMA and return to listening state */ @@ -153,6 +158,7 @@ rx_handle_packet(void) /* check packet CRC */ uint8_t crc = dma_packet.crc; dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { perf_count(pc_crcerr); @@ -170,11 +176,13 @@ rx_handle_packet(void) if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { dma_packet.count_code = PKT_CODE_SUCCESS; } + return; - } + } if (PKT_CODE(dma_packet) == PKT_CODE_READ) { @@ -185,17 +193,22 @@ rx_handle_packet(void) if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { perf_count(pc_regerr); dma_packet.count_code = PKT_CODE_ERROR; + } else { /* constrain reply to requested size */ - if (count > PKT_MAX_REGS) + if (count > PKT_MAX_REGS) { count = PKT_MAX_REGS; - if (count > PKT_COUNT(dma_packet)) + } + + if (count > PKT_COUNT(dma_packet)) { count = PKT_COUNT(dma_packet); + } /* copy reply registers into DMA buffer */ memcpy((void *)&dma_packet.regs[0], registers, count * 2); dma_packet.count_code = count | PKT_CODE_SUCCESS; } + return; } @@ -250,16 +263,22 @@ serial_interrupt(int irq, void *context) (void)rDR; /* required to clear any of the interrupt status that brought us here */ if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ - USART_SR_NE | /* noise error - we have lost a byte due to noise */ - USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ perf_count(pc_errors); - if (sr & USART_SR_ORE) + + if (sr & USART_SR_ORE) { perf_count(pc_ore); - if (sr & USART_SR_NE) + } + + if (sr & USART_SR_NE) { perf_count(pc_ne); - if (sr & USART_SR_FE) + } + + if (sr & USART_SR_FE) { perf_count(pc_fe); + } /* send a line break - this will abort transmission/reception on the other end */ rCR1 |= USART_CR1_SBK; @@ -270,7 +289,7 @@ serial_interrupt(int irq, void *context) if (sr & USART_SR_IDLE) { - /* + /* * If we saw an error, don't bother looking at the packet - it should have * been aborted by the sender and will definitely be bad. Get the DMA reconfigured * ready for their retry. @@ -284,10 +303,11 @@ serial_interrupt(int irq, void *context) /* * The sender has stopped sending - this is probably the end of a packet. - * Check the received length against the length in the header to see if + * Check the received length against the length in the header to see if * we have something that looks like a packet. */ unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { /* it was too short - possibly truncated */ From fc29fed26000a180c9cdd6a9666aed908ca9a43f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:47:57 +0200 Subject: [PATCH 066/155] Land detector: Fix code style --- .../land_detector/FixedwingLandDetector.cpp | 28 ++++++++++--------- src/modules/land_detector/LandDetector.cpp | 14 +++++----- src/modules/land_detector/LandDetector.h | 3 +- .../land_detector/MulticopterLandDetector.cpp | 9 +++--- .../land_detector/land_detector_main.cpp | 2 ++ 5 files changed, 31 insertions(+), 25 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 7cfe65bb7f..f8066241c5 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -49,18 +49,18 @@ FixedwingLandDetector::FixedwingLandDetector() : LandDetector(), _paramHandle(), _params(), _vehicleLocalPositionSub(-1), - _vehicleLocalPosition({}), - _airspeedSub(-1), - _vehicleStatusSub(-1), - _armingSub(-1), - _airspeed{}, - _vehicleStatus{}, - _arming{}, - _parameterSub(-1), - _velocity_xy_filtered(0.0f), - _velocity_z_filtered(0.0f), - _airspeed_filtered(0.0f), - _landDetectTrigger(0) + _vehicleLocalPosition( {}), + _airspeedSub(-1), + _vehicleStatusSub(-1), + _armingSub(-1), + _airspeed{}, + _vehicleStatus{}, + _arming{}, + _parameterSub(-1), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f), + _landDetectTrigger(0) { _paramHandle.maxVelocity = param_find("LNDFW_VEL_XY_MAX"); _paramHandle.maxClimbRate = param_find("LNDFW_VEL_Z_MAX"); @@ -101,10 +101,12 @@ bool FixedwingLandDetector::update() if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * - _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); + if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; } + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); if (PX4_ISFINITE(val)) { diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index ae6a1b5037..3040e23407 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/src/modules/land_detector/LandDetector.cpp @@ -47,12 +47,11 @@ LandDetector::LandDetector() : _landDetectedPub(0), - _landDetected({0, false}), - _arming_time(0), - _taskShouldExit(false), - _taskIsRunning(false), - _work{} -{ + _landDetected( {0, false}), + _arming_time(0), + _taskShouldExit(false), + _taskIsRunning(false), +_work{} { // ctor } @@ -111,7 +110,8 @@ void LandDetector::cycle() } if (!_taskShouldExit) { - work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); + work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, + USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE)); } } diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h index d28863b499..b86e431150 100644 --- a/src/modules/land_detector/LandDetector.h +++ b/src/modules/land_detector/LandDetector.h @@ -99,7 +99,8 @@ protected: static constexpr uint64_t LAND_DETECTOR_TRIGGER_TIME = 2000000; /**< usec that landing conditions have to hold before triggering a land */ - static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ + static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = + 1000000; /**< time interval in which wider acceptance thresholds are used after arming */ protected: orb_advert_t _landDetectedPub; /**< publisher for position in local frame */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 1d0a6b92dc..fa636e496c 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -99,13 +99,14 @@ bool MulticopterLandDetector::update() if (!_arming.armed) { _arming_time = 0; return true; + } else if (_arming_time == 0) { _arming_time = hrt_absolute_time(); } // return status based on armed state if no position lock is available if (_vehicleGlobalPosition.timestamp == 0 || - hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { + hrt_elapsed_time(&_vehicleGlobalPosition.timestamp) > 500000) { // no position lock - not landed if armed return !_arming.armed; @@ -129,14 +130,14 @@ bool MulticopterLandDetector::update() // check if we are moving horizontally bool horizontalMovement = sqrtf(_vehicleGlobalPosition.vel_n * _vehicleGlobalPosition.vel_n + _vehicleGlobalPosition.vel_e * _vehicleGlobalPosition.vel_e) > _params.maxVelocity - && _vehicleStatus.condition_global_position_valid; + && _vehicleStatus.condition_global_position_valid; // next look if all rotation angles are not moving float maxRotationScaled = _params.maxRotation * armThresholdFactor; bool rotating = (fabsf(_vehicleAttitude.rollspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || - (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); + (fabsf(_vehicleAttitude.pitchspeed) > maxRotationScaled) || + (fabsf(_vehicleAttitude.yawspeed) > maxRotationScaled); // check if thrust output is minimal (about half of default) bool minimalThrust = _actuators.control[3] <= _params.maxThrottle; diff --git a/src/modules/land_detector/land_detector_main.cpp b/src/modules/land_detector/land_detector_main.cpp index c4d0e5194d..0aa6e1fad2 100644 --- a/src/modules/land_detector/land_detector_main.cpp +++ b/src/modules/land_detector/land_detector_main.cpp @@ -152,6 +152,7 @@ static int land_detector_start(const char *mode) return 1; } } + printf("\n"); } @@ -176,6 +177,7 @@ int land_detector_main(int argc, char *argv[]) warnx("land_detector start failed"); return 1; } + return 0; } From af1fe6ab9ba6f1a21ffac05d6ee17d88bf298c26 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:48:23 +0200 Subject: [PATCH 067/155] Geo lib: Fix code style --- src/lib/geo/geo.c | 67 +++++++++++++++++------- src/lib/geo/geo.h | 40 ++++++++------ src/lib/geo_lookup/geo_mag_declination.c | 26 ++++----- 3 files changed, 84 insertions(+), 49 deletions(-) diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 30b9d9ec28..a4b1599a9d 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -83,16 +83,19 @@ __EXPORT uint64_t map_projection_timestamp(const struct map_projection_reference return ref->timestamp; } -__EXPORT int map_projection_global_init(double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_global_init(double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { if (strcmp("commander", getprogname()) == 0) { return map_projection_init_timestamped(&mp_ref, lat_0, lon_0, timestamp); + } else { return -1; } } -__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init_timestamped(struct map_projection_reference_s *ref, double lat_0, double lon_0, + uint64_t timestamp) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { ref->lat_rad = lat_0 * M_DEG_TO_RAD; @@ -106,7 +109,8 @@ __EXPORT int map_projection_init_timestamped(struct map_projection_reference_s * return 0; } -__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 +__EXPORT int map_projection_init(struct map_projection_reference_s *ref, double lat_0, + double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 { return map_projection_init_timestamped(ref, lat_0, lon_0, hrt_absolute_time()); } @@ -116,7 +120,8 @@ __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lo return map_projection_reference(&mp_ref, ref_lat_rad, ref_lon_rad); } -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad) +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad) { if (!map_projection_initialized(ref)) { return -1; @@ -134,7 +139,8 @@ __EXPORT int map_projection_global_project(double lat, double lon, float *x, flo } -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y) +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y) { if (!map_projection_initialized(ref)) { return -1; @@ -161,7 +167,8 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub return map_projection_reproject(&mp_ref, x, y, lat, lon); } -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon) { if (!map_projection_initialized(ref)) { return -1; @@ -212,14 +219,16 @@ __EXPORT int globallocalconverter_init(double lat_0, double lon_0, float alt_0, { if (strcmp("commander", getprogname()) == 0) { gl_ref.alt = alt_0; - if (!map_projection_global_init(lat_0, lon_0, timestamp)) - { + + if (!map_projection_global_init(lat_0, lon_0, timestamp)) { gl_ref.init_done = true; return 0; + } else { gl_ref.init_done = false; return -1; } + } else { return -1; } @@ -260,8 +269,7 @@ __EXPORT int globallocalconverter_getref(double *lat_0, double *lon_0, float *al return -1; } - if (map_projection_global_getref(lat_0, lon_0)) - { + if (map_projection_global_getref(lat_0, lon_0)) { return -1; } @@ -283,7 +291,8 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); + double a = sin(d_lat / (double)2.0) * sin(d_lat / (double)2.0) + sin(d_lon / (double)2.0) * sin(d_lon / + (double)2.0) * cos(lat_now_rad) * cos(lat_next_rad); double c = (double)2.0 * atan2(sqrt(a), sqrt((double)1.0 - a)); return CONSTANTS_RADIUS_OF_EARTH * c; @@ -299,14 +308,16 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , + cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); theta = _wrap_pi(theta); return theta; } -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -316,11 +327,13 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ - *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); + *v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos( + d_lon)); *v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad); } -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e) +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -335,7 +348,8 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d *v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad); } -__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res) +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -346,7 +360,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa // Additional functions - @author Doug Weibel -__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current // position is right of the track and negative if left of the track as seen from a point on the track line @@ -397,7 +412,8 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d } -__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current @@ -436,10 +452,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do in_sector = false; // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } + if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start + && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } + if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start + || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { @@ -479,6 +497,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; @@ -539,6 +558,7 @@ __EXPORT float _wrap_pi(float bearing) } int c = 0; + while (bearing >= M_PI_F) { bearing -= M_TWOPI_F; @@ -548,6 +568,7 @@ __EXPORT float _wrap_pi(float bearing) } c = 0; + while (bearing < -M_PI_F) { bearing += M_TWOPI_F; @@ -567,6 +588,7 @@ __EXPORT float _wrap_2pi(float bearing) } int c = 0; + while (bearing >= M_TWOPI_F) { bearing -= M_TWOPI_F; @@ -576,6 +598,7 @@ __EXPORT float _wrap_2pi(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += M_TWOPI_F; @@ -595,6 +618,7 @@ __EXPORT float _wrap_180(float bearing) } int c = 0; + while (bearing >= 180.0f) { bearing -= 360.0f; @@ -604,6 +628,7 @@ __EXPORT float _wrap_180(float bearing) } c = 0; + while (bearing < -180.0f) { bearing += 360.0f; @@ -623,6 +648,7 @@ __EXPORT float _wrap_360(float bearing) } int c = 0; + while (bearing >= 360.0f) { bearing -= 360.0f; @@ -632,6 +658,7 @@ __EXPORT float _wrap_360(float bearing) } c = 0; + while (bearing < 0.0f) { bearing += 360.0f; diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index ca3587b934..f77a8b58a4 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -113,7 +113,8 @@ __EXPORT int map_projection_global_reference(double *ref_lat_rad, double *ref_lo * Writes the reference values of the projection given by the argument to ref_lat and ref_lon * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, double *ref_lon_rad); +__EXPORT int map_projection_reference(const struct map_projection_reference_s *ref, double *ref_lat_rad, + double *ref_lon_rad); /** * Initializes the global map transformation. @@ -158,15 +159,16 @@ __EXPORT int map_projection_init(struct map_projection_reference_s *ref, double __EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y); - /* Transforms a point in the geographic coordinate system to the local - * azimuthal equidistant plane using the projection given by the argument - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - * @return 0 if map_projection_init was called before, -1 else - */ -__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y); +/* Transforms a point in the geographic coordinate system to the local + * azimuthal equidistant plane using the projection given by the argument +* @param x north +* @param y east +* @param lat in degrees (47.1234567°, not 471234567°) +* @param lon in degrees (8.1234567°, not 81234567°) +* @return 0 if map_projection_init was called before, -1 else +*/ +__EXPORT int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, + float *y); /** * Transforms a point in the local azimuthal equidistant plane to the @@ -190,7 +192,8 @@ __EXPORT int map_projection_global_reproject(float x, float y, double *lat, doub * @param lon in degrees (8.1234567°, not 81234567°) * @return 0 if map_projection_init was called before, -1 else */ -__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon); +__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat, + double *lon); /** * Get reference position of the global map projection @@ -243,15 +246,20 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); +__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, + float *v_e); -__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e); +__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, + float *v_n, float *v_e); -__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res); +__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, + double *lon_res); -__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); +__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_start, double lon_start, double lat_end, double lon_end); -__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, +__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, + double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep); /* diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index c41d526061..58a3a351d9 100644 --- a/src/lib/geo_lookup/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -35,7 +35,7 @@ * @file geo_mag_declination.c * * Calculation / lookup table for earth magnetic field declination. -* +* * Lookup table from Scott Ferguson * * XXX Lookup table currently too coarse in resolution (only full degrees) @@ -55,18 +55,18 @@ static const int8_t declination_table[13][37] = \ { { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, - { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, - { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, - { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, - { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, - { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, - { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, - { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, - { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, - { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, - { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, - { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, - { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, }; static float get_lookup_table_val(unsigned lat, unsigned lon); From c19c8a35b259a98cf360ed6c6bb7c1d635df19bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:51:18 +0200 Subject: [PATCH 068/155] uORB: Update code style --- src/modules/uORB/Subscription.hpp | 34 +++++---- src/modules/uORB/uORB.cpp | 1 + src/modules/uORB/uORBMain.cpp | 123 +++++++++++++++--------------- src/modules/uORB/uORBUtils.cpp | 49 ++++++------ src/modules/uORB/uORBUtils.hpp | 24 +++--- 5 files changed, 121 insertions(+), 110 deletions(-) diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 31842ed715..49fc9918a1 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -65,9 +65,10 @@ public: * between updates */ SubscriptionBase(const struct orb_metadata *meta, - unsigned interval=0) : + unsigned interval = 0) : _meta(meta), - _handle() { + _handle() + { setHandle(orb_subscribe(getMeta())); orb_set_interval(getHandle(), interval); } @@ -75,7 +76,8 @@ public: /** * Check if there is a new update. * */ - bool updated() { + bool updated() + { bool isUpdated = false; orb_check(_handle, &isUpdated); return isUpdated; @@ -85,7 +87,8 @@ public: * Update the struct * @param data The uORB message struct we are updating. */ - void update(void * data) { + void update(void *data) + { if (updated()) { orb_copy(_meta, _handle, data); } @@ -94,7 +97,8 @@ public: /** * Deconstructor */ - virtual ~SubscriptionBase() { + virtual ~SubscriptionBase() + { orb_unsubscribe(_handle); } // accessors @@ -108,9 +112,9 @@ protected: int _handle; private: // forbid copy - SubscriptionBase(const SubscriptionBase& other); + SubscriptionBase(const SubscriptionBase &other); // forbid assignment - SubscriptionBase& operator = (const SubscriptionBase &); + SubscriptionBase &operator = (const SubscriptionBase &); }; /** @@ -139,11 +143,12 @@ public: * that this should be appended to. */ SubscriptionNode(const struct orb_metadata *meta, - unsigned interval=0, - List * list=nullptr) : + unsigned interval = 0, + List *list = nullptr) : SubscriptionBase(meta, interval), - _interval(interval) { - if (list != nullptr) list->add(this); + _interval(interval) + { + if (list != nullptr) { list->add(this); } } /** @@ -179,8 +184,8 @@ public: * list during construction */ Subscription(const struct orb_metadata *meta, - unsigned interval=0, - List * list=nullptr); + unsigned interval = 0, + List *list = nullptr); /** * Deconstructor */ @@ -190,7 +195,8 @@ public: /** * Create an update function that uses the embedded struct. */ - void update() { + void update() + { SubscriptionBase::update(getDataVoidPtr()); } diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 5a13a864c1..6d6f084d24 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -269,6 +269,7 @@ int orb_exists(const struct orb_metadata *meta, int instance) int orb_group_count(const struct orb_metadata *meta) { unsigned group_count = 0; + while (!uORB::Manager::get_instance()->orb_exists(meta, group_count++)) {}; return group_count; diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 8b2930eaef..360b95a724 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -45,83 +45,86 @@ extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static uORB::DeviceMaster *g_dev = nullptr; static void usage() { - warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); + warnx("Usage: uorb 'start', 'test', 'latency_test' or 'status'"); } int uorb_main(int argc, char *argv[]) { - if (argc < 2) { - usage(); - return -EINVAL; - } + if (argc < 2) { + usage(); + return -EINVAL; + } - /* - * Start/load the driver. - * - * XXX it would be nice to have a wrapper for this... - */ - if (!strcmp(argv[1], "start")) { + /* + * Start/load the driver. + * + * XXX it would be nice to have a wrapper for this... + */ + if (!strcmp(argv[1], "start")) { - if (g_dev != nullptr) { - warnx("already loaded"); - /* user wanted to start uorb, its already running, no error */ - return 0; - } + if (g_dev != nullptr) { + warnx("already loaded"); + /* user wanted to start uorb, its already running, no error */ + return 0; + } - /* create the driver */ - g_dev = new uORB::DeviceMaster(uORB::PUBSUB); + /* create the driver */ + g_dev = new uORB::DeviceMaster(uORB::PUBSUB); - if (g_dev == nullptr) { - warnx("driver alloc failed"); - return -ENOMEM; - } + if (g_dev == nullptr) { + warnx("driver alloc failed"); + return -ENOMEM; + } - if (OK != g_dev->init()) { - warnx("driver init failed"); - delete g_dev; - g_dev = nullptr; - return -EIO; - } + if (OK != g_dev->init()) { + warnx("driver init failed"); + delete g_dev; + g_dev = nullptr; + return -EIO; + } - return OK; - } + return OK; + } #ifndef __PX4_QURT - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) - { - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - return t.test(); - } - /* - * Test the latency. - */ - if (!strcmp(argv[1], "latency_test")) { + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + return t.test(); + } + + /* + * Test the latency. + */ + if (!strcmp(argv[1], "latency_test")) { + + uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); + + if (argc > 2 && !strcmp(argv[2], "medium")) { + return t.latency_test(ORB_ID(orb_test_medium), true); + + } else if (argc > 2 && !strcmp(argv[2], "large")) { + return t.latency_test(ORB_ID(orb_test_large), true); + + } else { + return t.latency_test(ORB_ID(orb_test), true); + } + } - uORBTest::UnitTest &t = uORBTest::UnitTest::instance(); - if (argc > 2 && !strcmp(argv[2], "medium")) { - return t.latency_test(ORB_ID(orb_test_medium), true); - } else if (argc > 2 && !strcmp(argv[2], "large")) { - return t.latency_test(ORB_ID(orb_test_large), true); - } else { - return t.latency_test(ORB_ID(orb_test), true); - } - } #endif - /* - * Print driver information. - */ - if (!strcmp(argv[1], "status")) - { - return OK; - } + /* + * Print driver information. + */ + if (!strcmp(argv[1], "status")) { + return OK; + } - usage(); - return -EINVAL; + usage(); + return -EINVAL; } diff --git a/src/modules/uORB/uORBUtils.cpp b/src/modules/uORB/uORBUtils.cpp index a18fa1453a..4b4e76c170 100644 --- a/src/modules/uORB/uORBUtils.cpp +++ b/src/modules/uORB/uORBUtils.cpp @@ -37,45 +37,46 @@ int uORB::Utils::node_mkpath ( - char *buf, - Flavor f, - const struct orb_metadata *meta, - int *instance + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance ) { - unsigned len; + unsigned len; - unsigned index = 0; + unsigned index = 0; - if (instance != nullptr) { - index = *instance; - } + if (instance != nullptr) { + index = *instance; + } - len = snprintf(buf, orb_maxpath, "/%s/%s%d", - (f == PUBSUB) ? "obj" : "param", - meta->o_name, index); + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); - if (len >= orb_maxpath) { - return -ENAMETOOLONG; - } + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } - return OK; + return OK; } //----------------------------------------------------------------------------- //----------------------------------------------------------------------------- int uORB::Utils::node_mkpath(char *buf, Flavor f, - const char *orbMsgName ) + const char *orbMsgName) { - unsigned len; + unsigned len; - unsigned index = 0; + unsigned index = 0; - len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", - orbMsgName, index ); + len = snprintf(buf, orb_maxpath, "/%s/%s%d", (f == PUBSUB) ? "obj" : "param", + orbMsgName, index); - if (len >= orb_maxpath) - return -ENAMETOOLONG; + if (len >= orb_maxpath) { + return -ENAMETOOLONG; + } - return OK; + return OK; } diff --git a/src/modules/uORB/uORBUtils.hpp b/src/modules/uORB/uORBUtils.hpp index 522d255b26..4593f1a95e 100644 --- a/src/modules/uORB/uORBUtils.hpp +++ b/src/modules/uORB/uORBUtils.hpp @@ -38,24 +38,24 @@ namespace uORB { - class Utils; +class Utils; } class uORB::Utils { public: - static int node_mkpath - ( - char *buf, - Flavor f, - const struct orb_metadata *meta, - int *instance = nullptr - ); + static int node_mkpath + ( + char *buf, + Flavor f, + const struct orb_metadata *meta, + int *instance = nullptr + ); - /** - * same as above except this generators the path based on the string. - */ - static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); + /** + * same as above except this generators the path based on the string. + */ + static int node_mkpath(char *buf, Flavor f, const char *orbMsgName); }; From 96142427aa81dc5da25a895e9c538882696a7d60 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:51:26 +0200 Subject: [PATCH 069/155] sensors: Update code style --- src/modules/sensors/sensors.cpp | 156 ++++++++++++++++++++------------ 1 file changed, 99 insertions(+), 57 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c14afb322f..eb52ce8296 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -167,7 +167,8 @@ public: int start(); private: - static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ + static const unsigned _rc_max_chan_count = + input_rc_s::RC_INPUT_MAX_CHANNELS; /**< maximum number of r/c channels we handle */ /** * Get and limit value for specified RC function. Returns NAN if not mapped. @@ -227,7 +228,7 @@ private: orb_advert_t _battery_pub; /**< battery status */ orb_advert_t _airspeed_pub; /**< airspeed */ orb_advert_t _diff_pres_pub; /**< differential_pressure */ - + perf_counter_t _loop_perf; /**< loop performance counter */ struct rc_channels_s _rc; /**< r/c channel data */ @@ -366,7 +367,7 @@ private: int init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount); + uint32_t *priorities, uint32_t *errcount); /** * Update our local parameter cache. @@ -489,10 +490,10 @@ Sensors::Sensors() : _armed(false), /* subscriptions */ - _gyro_sub{-1, -1, -1}, - _accel_sub{-1, -1, -1}, - _mag_sub{-1, -1, -1}, - _baro_sub{-1, -1, -1}, + _gyro_sub{ -1, -1, -1}, + _accel_sub{ -1, -1, -1}, + _mag_sub{ -1, -1, -1}, + _baro_sub{ -1, -1, -1}, _gyro_count(0), _accel_count(0), _mag_count(0), @@ -588,7 +589,8 @@ Sensors::Sensors() : /* RC to parameter mapping for changing parameters with RC */ for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) { char name[rc_parameter_map_s::PARAM_ID_LEN]; - snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", + i + 1); // shifted by 1 because param name starts at 1 _parameter_handles.rc_map_param[i] = param_find(name); } @@ -641,7 +643,7 @@ Sensors::Sensors() : (void)param_find("PWM_AUX_MAX"); (void)param_find("PWM_AUX_DISARMED"); (void)param_find("TRIG_MODE"); - + /* fetch initial parameter values */ parameters_update(); } @@ -825,19 +827,20 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("%s", paramerr); + } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ - #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 _parameters.battery_voltage_scaling = 0.0082f; - #elif CONFIG_ARCH_BOARD_AEROCORE +#elif CONFIG_ARCH_BOARD_AEROCORE _parameters.battery_voltage_scaling = 0.0063f; - #elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 _parameters.battery_voltage_scaling = 0.00459340659f; - #else +#else /* ensure a missing default trips a low voltage lockdown */ _parameters.battery_voltage_scaling = 0.00001f; - #endif +#endif } /* scaling of ADC ticks to battery current */ @@ -1098,9 +1101,11 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[i * 3 + 2] = gyro_report.z_raw; raw.gyro_timestamp[i] = gyro_report.timestamp; + if (i == 0) { raw.timestamp = gyro_report.timestamp; } + raw.gyro_errcount[i] = gyro_report.error_count; raw.gyro_temp[i] = gyro_report.temperature; } @@ -1183,8 +1188,8 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar[0] * 1e2f, raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, - calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, - raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar[0] * 1e2f, + raw.baro_pres_mbar[0] * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; @@ -1265,7 +1270,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1297,15 +1302,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); + } else { config_ok = true; } } + break; } } @@ -1331,7 +1340,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1363,15 +1372,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); + } else { config_ok = true; } } + break; } } @@ -1404,7 +1417,7 @@ Sensors::parameter_update_poll(bool forced) bool config_ok = false; - /* run through all stored calibrations */ + /* run through all stored calibrations */ for (unsigned i = 0; i < SENSOR_COUNT_MAX; i++) { /* initially status is ok per config */ failed = false; @@ -1444,6 +1457,7 @@ Sensors::parameter_update_poll(bool forced) /* reset param to -1 to indicate internal mag */ int32_t minus_one = MAG_ROT_VAL_INTERNAL; param_set_no_notification(param_find(str), &minus_one); + } else { int32_t mag_rot; @@ -1488,15 +1502,19 @@ Sensors::parameter_update_poll(bool forced) if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { /* apply new scaling and offsets */ res = px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&gscale); + if (res) { warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); + } else { config_ok = true; } } + break; } } @@ -1764,6 +1782,7 @@ Sensors::set_params_from_rc() } float rc_val = get_rc_value((rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, * maybe we need to introduce a more aggressive limit here */ if (rc_val > _param_rc_values[i] + FLT_EPSILON || rc_val < _param_rc_values[i] - FLT_EPSILON) { @@ -1895,24 +1914,30 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.y = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); - manual.x = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); - manual.r = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); - manual.z = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); - manual.flaps = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); - manual.aux1 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); - manual.aux2 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); - manual.aux3 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); - manual.aux4 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); - manual.aux5 = get_rc_value (rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); + manual.y = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0); + manual.x = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0); + manual.r = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0); + manual.z = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0); + manual.flaps = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0); + manual.aux1 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0); + manual.aux2 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0); + manual.aux3 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0); + manual.aux4 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0); + manual.aux5 = get_rc_value(rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.posctl_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); - manual.return_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); - manual.loiter_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); - manual.acro_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); - manual.offboard_switch = get_rc_sw2pos_position (rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv); + manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, + _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); + manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, + _parameters.rc_posctl_inv); + manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, + _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, + _parameters.rc_loiter_inv); + manual.acro_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, + _parameters.rc_acro_inv); + manual.offboard_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD, + _parameters.rc_offboard_th, _parameters.rc_offboard_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub != nullptr) { @@ -1964,7 +1989,7 @@ Sensors::task_main_trampoline(int argc, char *argv[]) int Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, - uint32_t *priorities, uint32_t *errcount) + uint32_t *priorities, uint32_t *errcount) { unsigned group_count = orb_group_count(meta); @@ -1975,7 +2000,7 @@ Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs, for (unsigned i = 0; i < group_count; i++) { if (subs[i] < 0) { subs[i] = orb_subscribe_multi(meta, i); - orb_priority(subs[i], (int32_t*)&priorities[i]); + orb_priority(subs[i], (int32_t *)&priorities[i]); } } @@ -1988,27 +2013,40 @@ Sensors::task_main() /* start individual sensors */ int ret = 0; + do { /* create a scope to handle exit with break */ ret = accel_init(); - if (ret) break; + + if (ret) { break; } + ret = gyro_init(); - if (ret) break; + + if (ret) { break; } + ret = mag_init(); - if (ret) break; + + if (ret) { break; } + ret = baro_init(); - if (ret) break; + + if (ret) { break; } + ret = adc_init(); - if (ret) break; + + if (ret) { break; } + break; } while (0); if (ret) { warnx("sensor initialization failed"); _sensors_task = -1; - if (_fd_adc >=0) { + + if (_fd_adc >= 0) { px4_close(_fd_adc); _fd_adc = -1; } + return; } @@ -2016,28 +2054,31 @@ Sensors::task_main() /* ensure no overflows can occur */ static_assert((sizeof(raw.gyro_timestamp) / sizeof(raw.gyro_timestamp[0])) >= SENSOR_COUNT_MAX, - "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); + "SENSOR_COUNT_MAX larger than sensor_combined datastructure fields. Overflow would occur"); /* * do subscriptions */ unsigned gcount_prev = _gyro_count; + unsigned mcount_prev = _mag_count; + unsigned acount_prev = _accel_count; + unsigned bcount_prev = _baro_count; _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], - &raw.gyro_priority[0], &raw.gyro_errcount[0]); + &raw.gyro_priority[0], &raw.gyro_errcount[0]); _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], - &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], - &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], - &raw.baro_priority[0], &raw.baro_errcount[0]); + &raw.baro_priority[0], &raw.baro_errcount[0]); if (gcount_prev != _gyro_count || mcount_prev != _mag_count || @@ -2130,6 +2171,7 @@ Sensors::task_main() /* if the secondary failed as well, go to the tertiary */ if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { fds[0].fd = _gyro_sub[2]; + } else { fds[0].fd = _gyro_sub[1]; } @@ -2150,16 +2192,16 @@ Sensors::task_main() */ if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) { _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], - &raw.gyro_priority[0], &raw.gyro_errcount[0]); + &raw.gyro_priority[0], &raw.gyro_errcount[0]); _mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0], - &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); + &raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]); _accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0], - &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); + &raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]); _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], - &raw.baro_priority[0], &raw.baro_errcount[0]); + &raw.baro_priority[0], &raw.baro_errcount[0]); _last_config_update = hrt_absolute_time(); @@ -2190,11 +2232,11 @@ Sensors::start() /* start the task */ _sensors_task = px4_task_spawn_cmd("sensors", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2000, - (px4_main_t)&Sensors::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2000, + (px4_main_t)&Sensors::task_main_trampoline, + nullptr); /* wait until the task is up and running or has failed */ while (_sensors_task > 0 && _task_should_exit) { From 752d091eccb941a2a90806cf14709dc0e6bf43dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:51:36 +0200 Subject: [PATCH 070/155] muorb: Update code style --- src/modules/muorb/adsp/px4muorb.hpp | 20 +++---- src/modules/muorb/adsp/uORBFastRpcChannel.cpp | 6 ++ .../krait-stubs/px4muorb_KraitRpcWrapper.hpp | 56 +++++++++---------- .../muorb/krait/uORBKraitFastRpcChannel.cpp | 4 +- 4 files changed, 46 insertions(+), 40 deletions(-) diff --git a/src/modules/muorb/adsp/px4muorb.hpp b/src/modules/muorb/adsp/px4muorb.hpp index d509819570..1ec6b45566 100644 --- a/src/modules/muorb/adsp/px4muorb.hpp +++ b/src/modules/muorb/adsp/px4muorb.hpp @@ -37,22 +37,22 @@ extern "C" { -int px4muorb_orb_initialize() __EXPORT; + int px4muorb_orb_initialize() __EXPORT; -int px4muorb_add_subscriber(const char *name) __EXPORT; + int px4muorb_add_subscriber(const char *name) __EXPORT; -int px4muorb_remove_subscriber(const char *name) __EXPORT; + int px4muorb_remove_subscriber(const char *name) __EXPORT; -int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; + int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes) __EXPORT; -int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT; + int px4muorb_is_subscriber_present(const char *topic_name, int *status) __EXPORT; -int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, - int *bytes_returned) __EXPORT; + int px4muorb_receive_msg(int *msg_type, char *topic_name, int topic_name_len, uint8_t *data, int data_len_in_bytes, + int *bytes_returned) __EXPORT; -int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes, - int *length_in_bytes, int *topic_count) __EXPORT; + int px4muorb_receive_bulk_data(uint8_t *_BulkTransferBuffer, int max_size_in_bytes, + int *length_in_bytes, int *topic_count) __EXPORT; -int px4muorb_unblock_recieve_msg(void) __EXPORT; + int px4muorb_unblock_recieve_msg(void) __EXPORT; } diff --git a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp index ffbf2ea049..82a3301322 100644 --- a/src/modules/muorb/adsp/uORBFastRpcChannel.cpp +++ b/src/modules/muorb/adsp/uORBFastRpcChannel.cpp @@ -183,6 +183,7 @@ int16_t uORB::FastRpcChannel::send_message(const char *messageName, int32_t leng } if ((unsigned long)DataQSize() < _min_q) { _min_q = (unsigned long)DataQSize(); } + if ((unsigned long)DataQSize() > _max_q) { _max_q = (unsigned long)DataQSize(); } _count++; @@ -367,6 +368,7 @@ int16_t uORB::FastRpcChannel::get_data if (rc != 0) { topic_name[0] = '\0'; } + /* PX4_DEBUG("GetData: %30s: t1: %lu t2: %lu t3: %lu", topic_name, (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); @@ -464,9 +466,13 @@ int16_t uORB::FastRpcChannel::get_bulk_data hrt_abstime t3 = hrt_absolute_time(); if ((unsigned long)(t3 - t1) > _get_bulk_max) { _get_bulk_max = (unsigned long)(t3 - t1); } + if ((unsigned long)(t3 - t1) < _get_bulk_min) { _get_bulk_min = (unsigned long)(t3 - t1); } + if ((unsigned long)(*topic_count) > _bulk_topic_count_max) { _bulk_topic_count_max = (unsigned long)(*topic_count); } + if ((unsigned long)(*topic_count) < _bulk_topic_count_min) { _bulk_topic_count_min = (unsigned long)(*topic_count); } + if ((unsigned long)(t3 - check_time) > 10000000) { //PX4_DEBUG("GetData: t1: %lu t2: %lu t3: %lu", (unsigned long)t1, (unsigned long)t2, (unsigned long)t3); //PX4_DEBUG(".... dt1: %7lu dt2: %7lu Q: %d", (unsigned long)(t2 - t1), (unsigned long)(t3 - t2), DataQSize()); diff --git a/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp b/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp index d6feab2a47..f6b64b8e54 100644 --- a/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp +++ b/src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp @@ -36,41 +36,41 @@ namespace px4muorb { - class KraitRpcWrapper; +class KraitRpcWrapper; } class px4muorb::KraitRpcWrapper { public: - /** - * Constructor - */ - KraitRpcWrapper() {} - - /** - * destructor - */ - ~KraitRpcWrapper() {} + /** + * Constructor + */ + KraitRpcWrapper() {} - /** - * Initiatizes the rpc channel px4 muorb - */ - bool Initialize() { return true; } + /** + * destructor + */ + ~KraitRpcWrapper() {} - /** - * Terminate to clean up the resources. This should be called at program exit - */ - bool Terminate() { return true; } + /** + * Initiatizes the rpc channel px4 muorb + */ + bool Initialize() { return true; } - /** - * Muorb related functions to pub/sub of orb topic from krait to adsp - */ - int32_t AddSubscriber( const char* topic ) { return 1; } - int32_t RemoveSubscriber( const char* topic ) { return 1; } - int32_t SendData( const char* topic, int32_t length_in_bytes, const uint8_t* data ) { return 1; } - int32_t ReceiveData( int32_t* msg_type, char** topic, int32_t* length_in_bytes, uint8_t** data ) { return 1; } - int32_t IsSubscriberPresent( const char* topic, int32_t* status ) { return 1; } - int32_t ReceiveBulkData( uint8_t** bulk_data, int32_t* length_in_bytes, int32_t* topic_count ) { return 1; } - int32_t UnblockReceiveData() { return 1; } + /** + * Terminate to clean up the resources. This should be called at program exit + */ + bool Terminate() { return true; } + + /** + * Muorb related functions to pub/sub of orb topic from krait to adsp + */ + int32_t AddSubscriber(const char *topic) { return 1; } + int32_t RemoveSubscriber(const char *topic) { return 1; } + int32_t SendData(const char *topic, int32_t length_in_bytes, const uint8_t *data) { return 1; } + int32_t ReceiveData(int32_t *msg_type, char **topic, int32_t *length_in_bytes, uint8_t **data) { return 1; } + int32_t IsSubscriberPresent(const char *topic, int32_t *status) { return 1; } + int32_t ReceiveBulkData(uint8_t **bulk_data, int32_t *length_in_bytes, int32_t *topic_count) { return 1; } + int32_t UnblockReceiveData() { return 1; } }; #endif // _px4muorb_KraitWrapper_hpp_ diff --git a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp index 165705ee19..6e53f0bbfd 100644 --- a/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp +++ b/src/modules/muorb/krait/uORBKraitFastRpcChannel.cpp @@ -139,11 +139,11 @@ int16_t uORB::KraitFastRpcChannel::send_message(const char *messageName, int32_t if ((t3 - t2) > _snd_msg_max) { _snd_msg_max = (t3 - t2); } _snd_msg_avg = ((double)((_snd_msg_avg * (_snd_msg_count - 1)) + - (unsigned long)(t3 - t2))) / (double)(_snd_msg_count); + (unsigned long)(t3 - t2))) / (double)(_snd_msg_count); } _overall_snd_avg = ((double)((_overall_snd_avg * (_overall_snd_count - 1)) + - (unsigned long)(t4 - t1))) / (double)(_overall_snd_count); + (unsigned long)(t4 - t1))) / (double)(_overall_snd_count); if ((t4 - _log_check_time) > _log_check_interval) { /* From 2b8e981a7d7fb9e7b381df345d9b1a65b6ed4629 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:51:47 +0200 Subject: [PATCH 071/155] conversion lib: Update code style --- src/lib/conversion/rotation.cpp | 329 +++++++++++++++++--------------- src/lib/conversion/rotation.h | 2 +- 2 files changed, 180 insertions(+), 151 deletions(-) diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index ca82ddd5e8..f56513e209 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -55,154 +55,183 @@ get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix) __EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z) { - float tmp; - switch (rot) { - case ROTATION_NONE: - case ROTATION_MAX: - return; - case ROTATION_YAW_45: { - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_90: { - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_YAW_135: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_YAW_180: - x = -x; y = -y; - return; - case ROTATION_YAW_225: { - tmp = HALF_SQRT_2*(y - x); - y = -HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_YAW_270: { - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_YAW_315: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; - return; - } - case ROTATION_ROLL_180: { - y = -y; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_45: { - tmp = HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_90: { - tmp = x; x = y; y = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_135: { - tmp = HALF_SQRT_2*(y - x); - y = HALF_SQRT_2*(y + x); - x = tmp; z = -z; - return; - } - case ROTATION_PITCH_180: { - x = -x; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_225: { - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(y - x); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_270: { - tmp = x; x = -y; y = -tmp; z = -z; - return; - } - case ROTATION_ROLL_180_YAW_315: { - tmp = HALF_SQRT_2*(x - y); - y = -HALF_SQRT_2*(x + y); - x = tmp; z = -z; - return; - } - case ROTATION_ROLL_90: { - tmp = z; z = y; y = -tmp; - return; - } - case ROTATION_ROLL_90_YAW_45: { - tmp = z; z = y; y = -tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_90_YAW_90: { - tmp = z; z = y; y = -tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_90_YAW_135: { - tmp = z; z = y; y = -tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270: { - tmp = z; z = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_45: { - tmp = z; z = -y; y = tmp; - tmp = HALF_SQRT_2*(x - y); - y = HALF_SQRT_2*(x + y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_90: { - tmp = z; z = -y; y = tmp; - tmp = x; x = -y; y = tmp; - return; - } - case ROTATION_ROLL_270_YAW_135: { - tmp = z; z = -y; y = tmp; - tmp = -HALF_SQRT_2*(x + y); - y = HALF_SQRT_2*(x - y); - x = tmp; - return; - } - case ROTATION_ROLL_270_YAW_270: { - tmp = z; z = -y; y = tmp; - tmp = x; x = y; y = -tmp; - return; - } - case ROTATION_PITCH_90: { - tmp = z; z = -x; x = tmp; - return; - } - case ROTATION_PITCH_270: { - tmp = z; z = x; x = -tmp; - return; - } - case ROTATION_ROLL_180_PITCH_270: { - tmp = z; z = x; x = tmp; - y = -y; - return; - } - case ROTATION_PITCH_90_YAW_180: { - tmp = x; x = z; z = tmp; - y = -y; - return; - } - } + float tmp; + + switch (rot) { + case ROTATION_NONE: + case ROTATION_MAX: + return; + + case ROTATION_YAW_45: { + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_90: { + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_YAW_135: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_YAW_180: + x = -x; y = -y; + return; + + case ROTATION_YAW_225: { + tmp = HALF_SQRT_2 * (y - x); + y = -HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_YAW_270: { + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_YAW_315: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; + return; + } + + case ROTATION_ROLL_180: { + y = -y; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_45: { + tmp = HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_90: { + tmp = x; x = y; y = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_135: { + tmp = HALF_SQRT_2 * (y - x); + y = HALF_SQRT_2 * (y + x); + x = tmp; z = -z; + return; + } + + case ROTATION_PITCH_180: { + x = -x; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_225: { + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (y - x); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_270: { + tmp = x; x = -y; y = -tmp; z = -z; + return; + } + + case ROTATION_ROLL_180_YAW_315: { + tmp = HALF_SQRT_2 * (x - y); + y = -HALF_SQRT_2 * (x + y); + x = tmp; z = -z; + return; + } + + case ROTATION_ROLL_90: { + tmp = z; z = y; y = -tmp; + return; + } + + case ROTATION_ROLL_90_YAW_45: { + tmp = z; z = y; y = -tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_90: { + tmp = z; z = y; y = -tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_90_YAW_135: { + tmp = z; z = y; y = -tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270: { + tmp = z; z = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_45: { + tmp = z; z = -y; y = tmp; + tmp = HALF_SQRT_2 * (x - y); + y = HALF_SQRT_2 * (x + y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_90: { + tmp = z; z = -y; y = tmp; + tmp = x; x = -y; y = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_135: { + tmp = z; z = -y; y = tmp; + tmp = -HALF_SQRT_2 * (x + y); + y = HALF_SQRT_2 * (x - y); + x = tmp; + return; + } + + case ROTATION_ROLL_270_YAW_270: { + tmp = z; z = -y; y = tmp; + tmp = x; x = y; y = -tmp; + return; + } + + case ROTATION_PITCH_90: { + tmp = z; z = -x; x = tmp; + return; + } + + case ROTATION_PITCH_270: { + tmp = z; z = x; x = -tmp; + return; + } + + case ROTATION_ROLL_180_PITCH_270: { + tmp = z; z = x; x = tmp; + y = -y; + return; + } + + case ROTATION_PITCH_90_YAW_180: { + tmp = x; x = z; z = tmp; + y = -y; + return; + } + } } diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 8128551e51..280b9cad0c 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -122,7 +122,7 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3, 3> *rot_matrix); /** From 571c4aebac6357b11b003ba44f507fc39b463fab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:51:57 +0200 Subject: [PATCH 072/155] rc lib: Update code style --- src/lib/rc/sumd.c | 170 +++++++++++++++++++++++++++++----------------- src/lib/rc/sumd.h | 6 +- 2 files changed, 111 insertions(+), 65 deletions(-) diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.c index cea7790ec1..1d99f1d433 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.c @@ -95,9 +95,11 @@ uint16_t sumd_crc16(uint16_t crc, uint8_t value) { int i; crc ^= (uint16_t)value << 8; + for (i = 0; i < 8; i++) { crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); } + return crc; } @@ -108,15 +110,17 @@ uint8_t sumd_crc8(uint8_t crc, uint8_t value) } int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels, - uint16_t max_chan_count) + uint16_t max_chan_count) { int ret = 1; + switch (_decode_state) { case SUMD_DECODE_STATE_UNSYNCED: - if(_debug) - printf( " SUMD_DECODE_STATE_UNSYNCED \n") ; - + if (_debug) { + printf(" SUMD_DECODE_STATE_UNSYNCED \n") ; + } + if (byte == SUMD_HEADER_ID) { _rxpacket.header = byte; _sumd = true; @@ -127,9 +131,11 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe _crc16 = sumd_crc16(_crc16, byte); _crc8 = sumd_crc8(_crc8, byte); _decode_state = SUMD_DECODE_STATE_GOT_HEADER; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_HEADER: %x \n", byte) ; + } + } else { ret = 3; } @@ -139,17 +145,23 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_HEADER: if (byte == SUMD_ID_SUMD || byte == SUMD_ID_SUMH) { _rxpacket.status = byte; + if (byte == SUMD_ID_SUMH) { _sumd = false; } + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _decode_state = SUMD_DECODE_STATE_GOT_STATE; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_STATE: %x \n", byte) ; + } } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; @@ -160,50 +172,65 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_STATE: if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { _rxpacket.length = byte; + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _rxlen++; _decode_state = SUMD_DECODE_STATE_GOT_LEN; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_LEN: %x (%d) \n", byte, byte) ; + } + } else { _decode_state = SUMD_DECODE_STATE_UNSYNCED; } + break; case SUMD_DECODE_STATE_GOT_LEN: _rxpacket.sumd_data[_rxlen] = byte; + if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); + } else { - _crc8 = sumd_crc8(_crc8, byte); + _crc8 = sumd_crc8(_crc8, byte); } + _rxlen++; - - if (_rxlen <= ((_rxpacket.length*2) )) { - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen-2, byte) ; + + if (_rxlen <= ((_rxpacket.length * 2))) { + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ; + } } else { _decode_state = SUMD_DECODE_STATE_GOT_DATA; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_DATA -- finish --\n") ; + } + } break; case SUMD_DECODE_STATE_GOT_DATA: _rxpacket.crc16_high = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; + } + if (_sumd) { _decode_state = SUMD_DECODE_STATE_GOT_CRC; + } else { _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_1; } @@ -212,91 +239,108 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: _rxpacket.crc16_low = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; + } + _decode_state = SUMD_DECODE_STATE_GOT_CRC16_BYTE_2; break; case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: _rxpacket.telemetry = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; + } + _decode_state = SUMD_DECODE_STATE_GOT_CRC; break; - + case SUMD_DECODE_STATE_GOT_CRC: if (_sumd) { _rxpacket.crc16_low = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; - - if (_crc16 == (uint16_t)(_rxpacket.crc16_high<<8)+_rxpacket.crc16_low) { + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; + } + + if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { _crcOK = true; } + } else { _rxpacket.crc8 = byte; - if(_debug) - printf( " SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; - + + if (_debug) { + printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; + } + if (_crc8 == _rxpacket.crc8) { _crcOK = true; } } - + if (_crcOK) { - if(_debug) - printf( " CRC - OK \n") ; + if (_debug) { + printf(" CRC - OK \n") ; + } if (_sumd) { - if(_debug) - printf( " Got valid SUMD Packet\n") ; - + if (_debug) { + printf(" Got valid SUMD Packet\n") ; + } + } else { - if(_debug) - printf( " Got valid SUMH Packet\n") ; - + if (_debug) { + printf(" Got valid SUMH Packet\n") ; + } + + } + + if (_debug) { + printf(" RXLEN: %d [Chans: %d] \n\n", _rxlen - 1, (_rxlen - 1) / 2) ; } - - if(_debug) - printf( " RXLEN: %d [Chans: %d] \n\n", _rxlen-1, (_rxlen-1)/2) ; ret = 0; unsigned i; uint8_t _cnt = *rx_count + 1; *rx_count = _cnt; - + *rssi = 100; /* received Channels */ if ((uint16_t)_rxpacket.length > max_chan_count) { _rxpacket.length = (uint8_t) max_chan_count; } + *channel_count = (uint16_t)_rxpacket.length; /* decode the actual packet */ /* reorder first 4 channels */ /* ch1 = roll -> sumd = ch2 */ - channels[0] = (uint16_t)((_rxpacket.sumd_data[1*2+1]<<8) | _rxpacket.sumd_data[1*2+2])>>3; + channels[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3; /* ch2 = pitch -> sumd = ch2 */ - channels[1] = (uint16_t)((_rxpacket.sumd_data[2*2+1]<<8) | _rxpacket.sumd_data[2*2+2])>>3; + channels[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3; /* ch3 = throttle -> sumd = ch2 */ - channels[2] = (uint16_t)((_rxpacket.sumd_data[0*2+1]<<8) | _rxpacket.sumd_data[0*2+2])>>3; + channels[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3; /* ch4 = yaw -> sumd = ch2 */ - channels[3] = (uint16_t)((_rxpacket.sumd_data[3*2+1]<<8) | _rxpacket.sumd_data[3*2+2])>>3; + channels[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; /* we start at channel 5(index 4) */ unsigned chan_index = 4; for (i = 4; i < _rxpacket.length; i++) { - if(_debug) - printf( "ch[%d] : %x %x [ %x %d ]\n", i+1, _rxpacket.sumd_data[i*2+1], _rxpacket.sumd_data[i*2+2], ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3, ((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3); - - channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i*2+1]<<8) | _rxpacket.sumd_data[i*2+2])>>3; + if (_debug) { + printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2], + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, + ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); + } + + channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; @@ -306,8 +350,10 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe } else { /* decoding failed */ ret = 4; - if(_debug) - printf( " CRC - fail \n") ; + + if (_debug) { + printf(" CRC - fail \n") ; + } } diff --git a/src/lib/rc/sumd.h b/src/lib/rc/sumd.h index f4793edfc8..ba9e830b91 100644 --- a/src/lib/rc/sumd.h +++ b/src/lib/rc/sumd.h @@ -60,7 +60,7 @@ typedef struct { uint8_t header; ///< 0xA8 for a valid packet uint8_t status; ///< 0x01 valid and live SUMD data frame / 0x00 = SUMH / 0x81 = Failsafe uint8_t length; ///< Channels - uint8_t sumd_data[SUMD_MAX_CHANNELS*2]; ///< ChannelData (High Byte/ Low Byte) + uint8_t sumd_data[SUMD_MAX_CHANNELS * 2]; ///< ChannelData (High Byte/ Low Byte) uint8_t crc16_high; ///< High Byte of 16 Bit CRC uint8_t crc16_low; ///< Low Byte of 16 Bit CRC uint8_t telemetry; ///< Telemetry request @@ -102,7 +102,7 @@ __EXPORT int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_ uint16_t *channels, uint16_t max_chan_count); */ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, - uint16_t *channels, uint16_t max_chan_count); + uint16_t *channels, uint16_t max_chan_count); + - __END_DECLS From 6342f4e58692231f3c52f52bf37298d5ecf97b48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:52:12 +0200 Subject: [PATCH 073/155] Q Estimator: Fix code style --- .../attitude_estimator_q_main.cpp | 75 ++++++++++++------- 1 file changed, 48 insertions(+), 27 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a00aa4749b..926e2dc592 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -78,12 +78,14 @@ using math::Quaternion; class AttitudeEstimatorQ; -namespace attitude_estimator_q { +namespace attitude_estimator_q +{ AttitudeEstimatorQ *instance; } -class AttitudeEstimatorQ { +class AttitudeEstimatorQ +{ public: /** * Constructor @@ -189,7 +191,7 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _lp_pitch_rate(250.0f, 20.0f) { _voter_mag.set_timeout(200000); - + _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); @@ -233,11 +235,11 @@ int AttitudeEstimatorQ::start() /* start the task */ _control_task = px4_task_spawn_cmd("attitude_estimator_q", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 2100, - (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 2100, + (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); @@ -287,6 +289,7 @@ void AttitudeEstimatorQ::task_main() // Poll error, sleep and try again usleep(10000); continue; + } else if (ret == 0) { // Poll timeout, do nothing continue; @@ -296,6 +299,7 @@ void AttitudeEstimatorQ::task_main() // Update sensors sensor_combined_s sensors; + if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) { // Feed validator with recent sensor data @@ -309,6 +313,7 @@ void AttitudeEstimatorQ::task_main() for (unsigned j = 0; j < 3; j++) { if (sensors.gyro_integral_dt[i] > 0) { gyro[j] = (double)sensors.gyro_integral_rad[i * 3 + j] / (sensors.gyro_integral_dt[i] / 1e6); + } else { /* fall back to angular rate */ gyro[j] = sensors.gyro_rad_s[i * 3 + j]; @@ -317,10 +322,11 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); } int best_gyro, best_accel, best_mag; @@ -339,26 +345,28 @@ void AttitudeEstimatorQ::task_main() _data_good = true; if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + _voter_accel.failover_count() > 0 || + _voter_mag.failover_count() > 0)) { _failsafe = true; mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || - _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { + _voter_accel.get_vibration_factor(curr_time) > _vibration_warning_threshold || + _voter_mag.get_vibration_factor(curr_time) > _vibration_warning_threshold)) { if (_vibration_warning_timestamp == 0) { _vibration_warning_timestamp = curr_time; + } else if (hrt_elapsed_time(&_vibration_warning_timestamp) > 10000000) { _vibration_warning = true; mavlink_and_console_log_critical(_mavlink_fd, "HIGH VIBRATION! g: %d a: %d m: %d", - (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), - (int)(100 * _voter_accel.get_vibration_factor(curr_time)), - (int)(100 * _voter_mag.get_vibration_factor(curr_time))); + (int)(100 * _voter_gyro.get_vibration_factor(curr_time)), + (int)(100 * _voter_accel.get_vibration_factor(curr_time)), + (int)(100 * _voter_mag.get_vibration_factor(curr_time))); } + } else { _vibration_warning_timestamp = 0; } @@ -366,8 +374,10 @@ void AttitudeEstimatorQ::task_main() bool gpos_updated; orb_check(_global_pos_sub, &gpos_updated); + if (gpos_updated) { orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos); + if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) { /* set magnetic declination automatically */ _mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon)); @@ -385,6 +395,7 @@ void AttitudeEstimatorQ::task_main() /* calculate acceleration in body frame */ _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt); } + _vel_prev_t = _gpos.timestamp; _vel_prev = vel; } @@ -444,17 +455,21 @@ void AttitudeEstimatorQ::task_main() if (_att_pub == nullptr) { _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + } else { orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); } } } -void AttitudeEstimatorQ::update_parameters(bool force) { +void AttitudeEstimatorQ::update_parameters(bool force) +{ bool updated = force; + if (!updated) { orb_check(_params_sub, &updated); } + if (updated) { parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); @@ -476,7 +491,8 @@ void AttitudeEstimatorQ::update_parameters(bool force) { } } -bool AttitudeEstimatorQ::init() { +bool AttitudeEstimatorQ::init() +{ // Rotation matrix can be easily constructed from acceleration and mag field vectors // 'k' is Earth Z axis (Down) unit vector in body frame Vector<3> k = -_accel; @@ -500,9 +516,10 @@ bool AttitudeEstimatorQ::init() { _q.normalize(); if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && - PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && - _q.length() > 0.95f && _q.length() < 1.05f) { + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) && + _q.length() > 0.95f && _q.length() < 1.05f) { _inited = true; + } else { _inited = false; } @@ -510,7 +527,8 @@ bool AttitudeEstimatorQ::init() { return _inited; } -bool AttitudeEstimatorQ::update(float dt) { +bool AttitudeEstimatorQ::update(float dt) +{ if (!_inited) { if (!_data_good) { @@ -537,18 +555,20 @@ bool AttitudeEstimatorQ::update(float dt) { // Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f)); // Optimized version with dropped zeros Vector<3> k( - 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), - 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), - (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) + 2.0f * (_q(1) * _q(3) - _q(0) * _q(2)), + 2.0f * (_q(2) * _q(3) + _q(0) * _q(1)), + (_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3)) ); corr += (k % (_accel - _pos_acc).normalized()) * _w_accel; // Gyro bias estimation _gyro_bias += corr * (_w_gyro_bias * dt); + for (int i = 0; i < 3; i++) { _gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max); } + _rates = _gyro + _gyro_bias; // Feed forward gyro @@ -561,7 +581,7 @@ bool AttitudeEstimatorQ::update(float dt) { _q.normalize(); if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) && - PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { + PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) { // Reset quaternion to last good state _q = q_last; _rates.zero(); @@ -573,7 +593,8 @@ bool AttitudeEstimatorQ::update(float dt) { } -int attitude_estimator_q_main(int argc, char *argv[]) { +int attitude_estimator_q_main(int argc, char *argv[]) +{ if (argc < 1) { warnx("usage: attitude_estimator_q {start|stop|status}"); return 1; From 87ea6f54b70e22ef4bf131380aa892c56e793313 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:52:29 +0200 Subject: [PATCH 074/155] MAVLink simulator: Update code style --- src/modules/simulator/simulator_mavlink.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a930d78c80..f99dd20450 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -769,7 +769,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) return OK; } -int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink) +int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t *flow_mavlink) { uint64_t timestamp = hrt_absolute_time(); @@ -794,10 +794,10 @@ int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink) flow.quality = flow_mavlink->quality; if (_flow_pub == nullptr) { - _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); + _flow_pub = orb_advertise(ORB_ID(optical_flow), &flow); } else { - orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); + orb_publish(ORB_ID(optical_flow), _flow_pub, &flow); } } From 16b6b3caa2e152888bc942dbe723cfab3320d526 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 15:04:12 +0200 Subject: [PATCH 075/155] Enable enforcement --- Tools/check_code_style.sh | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 36849d3a19..d911f15fa0 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -6,7 +6,22 @@ for fn in $(find src/examples \ src/include \ src/drivers \ src/platforms \ + src/firmware \ src/lib/launchdetection \ + src/lib/geo \ + src/lib/geo_lookup \ + src/lib/conversion \ + src/lib/rc \ + src/lib/version \ + src/modules/attitude_estimator_q \ + src/modules/gpio_led \ + src/modules/land_detector \ + src/modules/muorb \ + src/modules/px4iofirmware \ + src/modules/param \ + src/modules/sensors \ + src/modules/simulator \ + src/modules/uORB \ src/modules/bottle_drop \ src/modules/dataman \ src/modules/fixedwing_backside \ From 468bac71c2997f17b04546f03b50f84c54adddce Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 19 Oct 2015 10:05:04 -0700 Subject: [PATCH 076/155] Fixed PRI64 to PRId64 in generate_listener.py Signed-off-by: Mark Charlebois --- Tools/generate_listener.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 1f240ddbf6..625eda7037 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -119,8 +119,8 @@ print(""" #define PRIu64 "llu" #endif -#ifndef PRI64 -#define PRI64 "lld" +#ifndef PRId64 +#define PRId64 "lld" #endif """) @@ -184,7 +184,7 @@ for index,m in enumerate(messages[1:]): print("\t\t\t}") print("\t\t\tprintf(\"\\n\");") elif item[0] == "int64": - print("\t\t\tprintf(\"%s: %%\" PRI64 \"\\n\",container.%s);" % (item[1], item[1])) + print("\t\t\tprintf(\"%s: %%\" PRId64 \"\\n\",container.%s);" % (item[1], item[1])) elif item[0] == "int32": print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1])) elif item[0] == "uint32": From 9288b8fa7094d91efa233b4db5fd13258f88aa34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 21:48:33 +0200 Subject: [PATCH 077/155] Add posix_sitl_default target for consistency --- Makefile | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Makefile b/Makefile index 635b0c3e9f..b896e51391 100644 --- a/Makefile +++ b/Makefile @@ -127,6 +127,8 @@ qurt_eagle_travis: posix: posix_sitl_simple +posix_sitl_default: posix_sitl_simple + ros: ros_sitl_simple run_sitl_quad: posix From a554333841f49e8cb1ba6fd407637cca7f99fe82 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 22:38:16 +0200 Subject: [PATCH 078/155] 22 state estimator: Fix min/max #3022 --- .../estimator_22states.cpp | 21 +++++++++---------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index b920cb9458..0e7b459b72 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -43,7 +43,6 @@ #include #include #include -#include #ifndef M_PI_F #define M_PI_F static_cast(M_PI) @@ -1851,7 +1850,7 @@ void AttPosEKF::FuseOptFlow() Vector3f relVelSensor; // Perform sequential fusion of optical flow measurements only with valid tilt and height - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); float heightAboveGndEst = flowStates[1] - statesAtFlowTime[9]; bool validTilt = Tnb.z.z > 0.71f; if (validTilt) @@ -2111,7 +2110,7 @@ void AttPosEKF::OpticalFlowEKF() } else { return; } - distanceTravelledSq = std::min(distanceTravelledSq, 100.0f); + distanceTravelledSq = fmin(distanceTravelledSq, 100.0f); Popt[1][1] += (distanceTravelledSq * sq(gndHgtSigma)); } @@ -2151,7 +2150,7 @@ void AttPosEKF::OpticalFlowEKF() varInnovRng = 1.0f/SK_RNG[1]; // constrain terrain height to be below the vehicle - flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtRngTime[9]) * SK_RNG[2]; @@ -2171,7 +2170,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = std::max(flowStates[1], statesAtRngTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtRngTime[9] + minFlowRng); // correct the covariance matrix float nextPopt[2][2]; @@ -2180,8 +2179,8 @@ void AttPosEKF::OpticalFlowEKF() nextPopt[1][0] = -Popt[1][0]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); nextPopt[1][1] = -Popt[1][1]*((Popt[1][1]*SK_RNG[1]*SK_RNG[2]) * SK_RNG[2] - 1.0f); // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = std::max(nextPopt[0][0],0.0f); - Popt[1][1] = std::max(nextPopt[1][1],0.0f); + Popt[0][0] = fmax(nextPopt[0][0],0.0f); + Popt[1][1] = fmax(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } @@ -2220,7 +2219,7 @@ void AttPosEKF::OpticalFlowEKF() vel.z = statesAtFlowTime[6]; // constrain terrain height to be below the vehicle - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); // estimate range to centre of image range = (flowStates[1] - statesAtFlowTime[9]) / Tnb_flow.z.z; @@ -2288,7 +2287,7 @@ void AttPosEKF::OpticalFlowEKF() } // constrain the states flowStates[0] = ConstrainFloat(flowStates[0], 0.1f, 10.0f); - flowStates[1] = std::max(flowStates[1], statesAtFlowTime[9] + minFlowRng); + flowStates[1] = fmax(flowStates[1], statesAtFlowTime[9] + minFlowRng); // correct the covariance matrix for (uint8_t i = 0; i < 2 ; i++) { @@ -2304,8 +2303,8 @@ void AttPosEKF::OpticalFlowEKF() } // prevent the state variances from becoming negative and maintain symmetry - Popt[0][0] = std::max(nextPopt[0][0],0.0f); - Popt[1][1] = std::max(nextPopt[1][1],0.0f); + Popt[0][0] = fmax(nextPopt[0][0],0.0f); + Popt[1][1] = fmax(nextPopt[1][1],0.0f); Popt[0][1] = 0.5f * (nextPopt[0][1] + nextPopt[1][0]); Popt[1][0] = Popt[0][1]; } From 92d0347957c75d650b3fce8c47e1134f61f83279 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 22:39:57 +0200 Subject: [PATCH 079/155] Prevent Git-gate from Qt creator local files --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.gitignore b/.gitignore index 1a6e278539..14183ed177 100644 --- a/.gitignore +++ b/.gitignore @@ -55,3 +55,5 @@ src/platforms/posix-arm/px4_messages/ src/platforms/qurt/px4_messages/ ROMFS/*/*/rc.autostart rootfs/ +*.autosave +CMakeLists.txt.user From 79b4bd60b2fa18e073fe3aa95ab135f94855a3b7 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 19 Oct 2015 14:46:18 -0700 Subject: [PATCH 080/155] Fixed clang builds on Linux The link needed --start-group and --end-group for clang on Linux for posix builds. Signed-off-by: Mark Charlebois --- src/firmware/posix/CMakeLists.txt | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 7c30f1dfa6..2c7074ed33 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -10,7 +10,7 @@ add_executable(mainapp apps.h ) -if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") +if (NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} @@ -18,17 +18,10 @@ if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") -Wl,--end-group ) else() - if (APPLE) - target_link_libraries(mainapp - ${module_libraries} - pthread m - ) - else() - target_link_libraries(mainapp - ${module_libraries} - pthread m rt - ) - endif() + target_link_libraries(mainapp + ${module_libraries} + pthread m + ) endif() # vim: set noet ft=cmake fenc=utf-8 ff=unix : From 2cdd6c60550210344be7475b995b80062e30d1a8 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 19 Oct 2015 14:58:15 -0700 Subject: [PATCH 081/155] Corrected logic for gcc build on Apple Signed-off-by: Mark Charlebois --- src/firmware/posix/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 2c7074ed33..7cef387962 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -10,7 +10,7 @@ add_executable(mainapp apps.h ) -if (NOT APPLE) +if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang" OR NOT APPLE) target_link_libraries(mainapp -Wl,--start-group ${module_libraries} From 938798f4db481766d44dbf7a1a8efcabed3fa23a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 19 Oct 2015 10:10:01 -0700 Subject: [PATCH 082/155] Removed qurt patch for Eigen and updated eigen commit The patches for Hexagon support are in the eigen repo so the patch can now be removed. Signed-off-by: Mark Charlebois --- cmake/qurt/px4_impl_qurt.cmake | 9 +-------- cmake/toolchains/Toolchain-posix-clang-native.cmake | 5 +++++ src/lib/eigen | 2 +- 3 files changed, 7 insertions(+), 9 deletions(-) diff --git a/cmake/qurt/px4_impl_qurt.cmake b/cmake/qurt/px4_impl_qurt.cmake index 49bd83f53b..8f9f3dc5d3 100644 --- a/cmake/qurt/px4_impl_qurt.cmake +++ b/cmake/qurt/px4_impl_qurt.cmake @@ -222,14 +222,7 @@ function(px4_os_prebuild_targets) ONE_VALUE OUT BOARD THREADS REQUIRED OUT BOARD ARGN ${ARGN}) - add_custom_target(git_eigen_patched - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}/src/lib/eigen - COMMAND git checkout . - COMMAND git checkout master - COMMAND patch -p1 -i ${CMAKE_SOURCE_DIR}/cmake/qurt/qurt_eigen.patch - DEPENDS git_eigen) - add_custom_target(${OUT} DEPENDS git_dspal git_eigen_patched) - add_custom_target(ALL DEPENDS git_eigen_patched) + add_custom_target(${OUT} DEPENDS git_dspal git_eigen) endfunction() diff --git a/cmake/toolchains/Toolchain-posix-clang-native.cmake b/cmake/toolchains/Toolchain-posix-clang-native.cmake index 0945b2b1f5..948928150f 100644 --- a/cmake/toolchains/Toolchain-posix-clang-native.cmake +++ b/cmake/toolchains/Toolchain-posix-clang-native.cmake @@ -53,6 +53,8 @@ set(C_WARNINGS set(C_FLAGS -std=gnu99 -fno-common + -fsanitize=address + -fno-omit-frame-pointer ) #============================================================================= @@ -67,6 +69,8 @@ set(CXX_FLAGS -fno-rtti -std=gnu++0x -fno-threadsafe-statics + -fsanitize=address + -fno-omit-frame-pointer -DCONFIG_WCHAR_BUILTIN -D__CUSTOM_FILE_IO__ ) @@ -75,6 +79,7 @@ set(CXX_FLAGS # ld flags # set(LD_FLAGS + -fsanitize=address -Wl,--warn-common -Wl,--gc-sections ) diff --git a/src/lib/eigen b/src/lib/eigen index 240599e148..eb7213abbf 160000 --- a/src/lib/eigen +++ b/src/lib/eigen @@ -1 +1 @@ -Subproject commit 240599e1488da540c2ae8753dea0907f6cfac8ee +Subproject commit eb7213abbf38a987a18ed5071172d9d26cc38306 From 43834e3fafa09ba38fe6f995cadcf191e666250a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 00:20:17 +0200 Subject: [PATCH 083/155] Att Q estimator: Lowpass output for all three rates with adjusted lowpass frequencies --- .../attitude_estimator_q/attitude_estimator_q_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 926e2dc592..a9ffbe0c42 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -160,6 +160,7 @@ private: /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; math::LowPassFilter2p _lp_pitch_rate; + math::LowPassFilter2p _lp_yaw_rate; hrt_abstime _vel_prev_t = 0; @@ -187,8 +188,9 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _voter_gyro(3), _voter_accel(3), _voter_mag(3), - _lp_roll_rate(250.0f, 20.0f), - _lp_pitch_rate(250.0f, 20.0f) + _lp_roll_rate(250.0f, 18.0f), + _lp_pitch_rate(250.0f, 18.0f), + _lp_yaw_rate(250, 10.0f) { _voter_mag.set_timeout(200000); @@ -434,7 +436,7 @@ void AttitudeEstimatorQ::task_main() */ att.rollspeed = _lp_roll_rate.apply(_rates(0)); att.pitchspeed = _lp_pitch_rate.apply(_rates(1)); - att.yawspeed = _rates(2); + att.yawspeed = _lp_yaw_rate.apply(_rates(2)); for (int i = 0; i < 3; i++) { att.g_comp[i] = _accel(i) - _pos_acc(i); From a57f8e2ce30a3dce4bdbd38dac8e8389762f81c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 00:21:12 +0200 Subject: [PATCH 084/155] Adjust F450 attitude gains --- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 55e5237945..5e7ba1bda5 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -15,13 +15,12 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then - # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.16 + param set MC_ROLLRATE_P 0.15 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.16 + param set MC_PITCHRATE_P 0.15 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 From b9e461b0283ecc22a07c4090e445c6564427cc53 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Tue, 20 Oct 2015 10:48:10 -0400 Subject: [PATCH 085/155] Control lib update. --- src/drivers/roboclaw/RoboClaw.cpp | 4 +- src/modules/controllib/block/Block.hpp | 32 ++++--- src/modules/controllib/block/BlockParam.cpp | 19 ++-- src/modules/controllib/blocks.cpp | 5 ++ src/modules/controllib/blocks.hpp | 30 ++++--- src/modules/controllib/uorb/blocks.cpp | 27 +++--- src/modules/controllib/uorb/blocks.hpp | 8 +- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 10 +-- src/modules/uORB/Publication.cpp | 55 +++++++++--- src/modules/uORB/Publication.hpp | 80 ++++++++--------- src/modules/uORB/Subscription.cpp | 90 +++++++++++++++---- src/modules/uORB/Subscription.hpp | 66 +++++--------- 12 files changed, 255 insertions(+), 171 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index a3d2f96a20..f9116f1d45 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -299,8 +299,8 @@ int RoboClaw::update() // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorDutyCycle(MOTOR_1, _actuators.control[CH_VOLTAGE_LEFT]); - setMotorDutyCycle(MOTOR_2, _actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1, _actuators.get().control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]); } return 0; diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index bb2a0e2657..e74258b523 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -58,8 +58,8 @@ static const uint16_t maxPublicationsPerBlock = 100; static const uint8_t blockNameLengthMax = 80; // forward declaration -class SuperBlock; class BlockParamBase; +class SuperBlock; /** */ @@ -81,9 +81,9 @@ public: protected: // accessors SuperBlock *getParent() { return _parent; } - List & getSubscriptions() { return _subscriptions; } - List & getPublications() { return _publications; } - List & getParams() { return _params; } + List &getSubscriptions() { return _subscriptions; } + List &getPublications() { return _publications; } + List &getParams() { return _params; } // attributes const char *_name; SuperBlock *_parent; @@ -94,8 +94,8 @@ protected: private: /* this class has pointer data members and should not be copied (private constructor) */ - Block(const control::Block&); - Block operator=(const control::Block&); + Block(const control::Block &); + Block operator=(const control::Block &); }; class __EXPORT SuperBlock : @@ -106,28 +106,32 @@ public: // methods SuperBlock(SuperBlock *parent, const char *name) : Block(parent, name), - _children() { + _children() + { } virtual ~SuperBlock() {}; virtual void setDt(float dt); - virtual void updateParams() { + virtual void updateParams() + { Block::updateParams(); - if (getChildren().getHead() != NULL) updateChildParams(); + if (getChildren().getHead() != NULL) { updateChildParams(); } } - virtual void updateSubscriptions() { + virtual void updateSubscriptions() + { Block::updateSubscriptions(); - if (getChildren().getHead() != NULL) updateChildSubscriptions(); + if (getChildren().getHead() != NULL) { updateChildSubscriptions(); } } - virtual void updatePublications() { + virtual void updatePublications() + { Block::updatePublications(); - if (getChildren().getHead() != NULL) updateChildPublications(); + if (getChildren().getHead() != NULL) { updateChildPublications(); } } protected: // methods - List & getChildren() { return _children; } + List &getChildren() { return _children; } void updateChildParams(); void updateChildSubscriptions(); void updateChildPublications(); diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 80f1bae1b0..b560d7999e 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -65,6 +65,7 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref } else if (parent_prefix) { snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name); + } else { strncpy(fullname, name, blockNameLengthMax); } @@ -74,15 +75,17 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref _handle = param_find(fullname); - if (_handle == PARAM_INVALID) + if (_handle == PARAM_INVALID) { printf("error finding param: %s\n", fullname); + } }; template BlockParam::BlockParam(Block *block, const char *name, - bool parent_prefix) : + bool parent_prefix) : BlockParamBase(block, name, parent_prefix), - _val() { + _val() +{ update(); } @@ -93,13 +96,15 @@ template void BlockParam::set(T val) { _val = val; } template -void BlockParam::update() { - if (_handle != PARAM_INVALID) param_get(_handle, &_val); +void BlockParam::update() +{ + if (_handle != PARAM_INVALID) { param_get(_handle, &_val); } } template -void BlockParam::commit() { - if (_handle != PARAM_INVALID) param_set(_handle, &_val); +void BlockParam::commit() +{ + if (_handle != PARAM_INVALID) { param_set(_handle, &_val); } } template diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 6a0b615910..1aa60e63c8 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -126,6 +126,7 @@ float BlockLowPass::update(float input) if (!isfinite(getState())) { setState(input); } + float b = 2 * float(M_PI) * getFCut() * getDt(); float a = b / (1 + b); setState(a * input + (1 - a)*getState()); @@ -209,6 +210,7 @@ float BlockLowPass2::update(float input) if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) { _lp.set_cutoff_frequency(_fs, getFCutParam()); } + _state = _lp.apply(input); return _state; } @@ -340,8 +342,10 @@ int blockIntegralTrapTest() float BlockDerivative::update(float input) { float output; + if (_initialized) { output = _lowPass.update((input - getU()) / getDt()); + } else { // if this is the first call to update // we have no valid derivative @@ -351,6 +355,7 @@ float BlockDerivative::update(float input) output = 0.0f; _initialized = true; } + setU(input); return output; } diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index 786bfc06d3..2b571de590 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -317,7 +317,8 @@ public: _kP(this, "") // only one param, no need to name {}; virtual ~BlockP() {}; - float update(float input) { + float update(float input) + { return getKP() * input; } // accessors @@ -343,7 +344,8 @@ public: _kI(this, "I") {}; virtual ~BlockPI() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKI() * getIntegral().update(input); } @@ -374,7 +376,8 @@ public: _kD(this, "D") {}; virtual ~BlockPD() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKD() * getDerivative().update(input); } @@ -407,7 +410,8 @@ public: _kD(this, "D") {}; virtual ~BlockPID() {}; - float update(float input) { + float update(float input) + { return getKP() * input + getKI() * getIntegral().update(input) + getKD() * getDerivative().update(input); @@ -440,11 +444,13 @@ public: SuperBlock(parent, name), _trim(this, "TRIM"), _limit(this, ""), - _val(0) { + _val(0) + { update(0); }; virtual ~BlockOutput() {}; - void update(float input) { + void update(float input) + { _val = _limit.update(input + getTrim()); } // accessors @@ -472,13 +478,15 @@ public: const char *name) : Block(parent, name), _min(this, "MIN"), - _max(this, "MAX") { + _max(this, "MAX") + { // seed should be initialized somewhere // in main program for all calls to rand // XXX currently in nuttx if you seed to 0, rand breaks }; virtual ~BlockRandUniform() {}; - float update() { + float update() + { static float rand_max = MAX_RAND; float rand_val = rand(); float bounds = getMax() - getMin(); @@ -503,13 +511,15 @@ public: const char *name) : Block(parent, name), _mean(this, "MEAN"), - _stdDev(this, "DEV") { + _stdDev(this, "DEV") + { // seed should be initialized somewhere // in main program for all calls to rand // XXX currently in nuttx if you seed to 0, rand breaks }; virtual ~BlockRandGauss() {}; - float update() { + float update() + { static float V1, V2, S; static int phase = 0; float X; diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 4f651777ba..3a99617e4d 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -53,10 +53,11 @@ BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *nam BlockWaypointGuidance::~BlockWaypointGuidance() {}; -void BlockWaypointGuidance::update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - position_setpoint_s &missionCmd, - position_setpoint_s &lastMissionCmd) +void BlockWaypointGuidance::update( + const vehicle_global_position_s &pos, + const vehicle_attitude_s &att, + const position_setpoint_s &missionCmd, + const position_setpoint_s &lastMissionCmd) { // heading to waypoint @@ -83,16 +84,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos, BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // subscriptions - _att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()), - _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()), - _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()), - _pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()), - _missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()), - _manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()), - _status(ORB_ID(vehicle_status), 20, &getSubscriptions()), - _param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz + _att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()), + _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()), + _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()), + _pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()), + _missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()), + _manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()), + _status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()), + _param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz // publications - _actuators(ORB_ID(actuator_controls_0), &getPublications()) + _actuators(ORB_ID(actuator_controls_0), -1, &getPublications()) { } diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index e3c0811116..7766b67f6b 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -80,10 +80,10 @@ private: public: BlockWaypointGuidance(SuperBlock *parent, const char *name); virtual ~BlockWaypointGuidance(); - void update(vehicle_global_position_s &pos, - vehicle_attitude_s &att, - position_setpoint_s &missionCmd, - position_setpoint_s &lastMissionCmd); + void update(const vehicle_global_position_s &pos, + const vehicle_attitude_s &att, + const position_setpoint_s &missionCmd, + const position_setpoint_s &lastMissionCmd); float getPsiCmd() { return _psiCmd; } }; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 49e1d735e0..6de6d9d8f0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -53,7 +53,7 @@ mTecs::mTecs() : _airspeedMin(this, "FW_AIRSPD_MIN", false), /* Publications */ #if defined(__PX4_NUTTX) - _status(ORB_ID(tecs_status), &getPublications()), + _status(ORB_ID(tecs_status), -1, &getPublications()), #endif // defined(__PX4_NUTTX) /* control blocks */ _controlTotalEnergy(this, "THR"), @@ -111,8 +111,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti #if defined(__PX4_NUTTX) /* Write part of the status message */ - _status.altitudeSp = altitudeSp; - _status.altitude_filtered = altitudeFiltered; + _status.get().altitudeSp = altitudeSp; + _status.get().altitude_filtered = altitudeFiltered; #endif // defined(__PX4_NUTTX) @@ -149,8 +149,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng #if defined(__PX4_NUTTX) /* Write part of the status message */ - _status.airspeedSp = airspeedSp; - _status.airspeed_filtered = airspeedFiltered; + _status.get().airspeedSp = airspeedSp; + _status.get().airspeed_filtered = airspeedFiltered; #endif // defined(__PX4_NUTTX) diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index bd8ecf13b4..e979e3b4f4 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -50,37 +50,63 @@ #include "topics/encoders.h" #include "topics/tecs_status.h" #include "topics/rc_channels.h" +#include "topics/filtered_bottom_flow.h" + +#include namespace uORB { -template -Publication::Publication( - const struct orb_metadata *meta, - List *list) : - T(), // initialize data structure to zero - PublicationNode(meta, list) +PublicationBase::PublicationBase(const struct orb_metadata *meta, + int priority) : + _meta(meta), + _priority(priority), + _instance(), + _handle(nullptr) { } -template -Publication::~Publication() {} - -template -void *Publication::getDataVoidPtr() +void PublicationBase::update(void *data) { - return (void *)(T *)(this); + if (_handle != nullptr) { + int ret = orb_publish(getMeta(), getHandle(), data); + + if (ret != PX4_OK) { warnx("publish fail"); } + + } else { + orb_advert_t handle; + + if (_priority > 0) { + handle = orb_advertise_multi( + getMeta(), data, + &_instance, _priority); + + } else { + handle = orb_advertise(getMeta(), data); + } + + if (int64_t(handle) != PX4_ERROR) { + setHandle(handle); + + } else { + warnx("advert fail"); + } + } } +PublicationBase::~PublicationBase() +{ +} PublicationNode::PublicationNode(const struct orb_metadata *meta, + int priority, List *list) : - PublicationBase(meta) + PublicationBase(meta, priority) { if (list != nullptr) { list->add(this); } } - +// explicit template instantiation template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; @@ -94,5 +120,6 @@ template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index f8af00e96d..37658721ca 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -42,13 +42,13 @@ #include #include - +#include namespace uORB { /** - * Base publication warapper class, used in list traversal + * Base publication wrapper class, used in list traversal * of various publications. */ class __EXPORT PublicationBase @@ -58,50 +58,40 @@ public: /** * Constructor * - * - * @param meta The uORB metadata (usually from the ORB_ID() - * macro) for the topic. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub/sub, 0-based, -1 means + * don't publish as multi */ - PublicationBase(const struct orb_metadata *meta) : - _meta(meta), - _handle(nullptr) - { - } + PublicationBase(const struct orb_metadata *meta, + int priority = -1); /** * Update the struct * @param data The uORB message struct we are updating. */ - void update(void *data) - { - if (_handle != nullptr) { - orb_publish(getMeta(), getHandle(), data); - - } else { - setHandle(orb_advertise(getMeta(), data)); - } - } + void update(void *data); /** * Deconstructor */ - virtual ~PublicationBase() - { - } + virtual ~PublicationBase(); + // accessors const struct orb_metadata *getMeta() { return _meta; } orb_advert_t getHandle() { return _handle; } protected: + // disallow copy + PublicationBase(const PublicationBase &other); + // disallow assignment + PublicationBase &operator=(const PublicationBase &other); // accessors void setHandle(orb_advert_t handle) { _handle = handle; } // attributes const struct orb_metadata *_meta; + int _priority; + int _instance; orb_advert_t _handle; -private: - // forbid copy - PublicationBase(const PublicationBase &) : _meta(), _handle() {}; - // forbid assignment - PublicationBase &operator = (const PublicationBase &); }; /** @@ -121,13 +111,14 @@ public: /** * Constructor * - * - * @param meta The uORB metadata (usually from the ORB_ID() - * macro) for the topic. - * @param list A pointer to a list of subscriptions - * that this should be appended to. + * @param meta The uORB metadata (usually from + * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub, 0-based. + * @param list A list interface for adding to + * list during construction */ PublicationNode(const struct orb_metadata *meta, + int priority = -1, List *list = nullptr); /** @@ -142,7 +133,6 @@ public: */ template class __EXPORT Publication : - public T, // this must be first! public PublicationNode { public: @@ -151,33 +141,37 @@ public: * * @param meta The uORB metadata (usually from * the ORB_ID() macro) for the topic. + * @param priority The priority for multi pub, 0-based. * @param list A list interface for adding to * list during construction */ Publication(const struct orb_metadata *meta, - List *list = nullptr); + int priority = -1, + List *list = nullptr) : + PublicationNode(meta, priority, list), + _data() + { + } /** * Deconstructor **/ - virtual ~Publication(); + virtual ~Publication() {}; /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr(); + * This function gets the T struct + * */ + T &get() { return _data; } /** * Create an update function that uses the embedded struct. */ void update() { - PublicationBase::update(getDataVoidPtr()); + PublicationBase::update((void *)(&_data)); } +private: + T _data; }; } // namespace uORB diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 3554b497d7..a85e8da02d 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -54,37 +54,89 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/rc_channels.h" +#include "topics/battery_status.h" +#include "topics/optical_flow.h" +#include "topics/distance_sensor.h" +#include "topics/home_position.h" #include "topics/vehicle_control_mode.h" #include "topics/actuator_armed.h" +#include "topics/att_pos_mocap.h" +#include "topics/vision_position_estimate.h" + +#include namespace uORB { -template -Subscription::Subscription( - const struct orb_metadata *meta, - unsigned interval, - List *list) : - T(), // initialize data structure to zero - SubscriptionNode(meta, interval, list) +SubscriptionBase::SubscriptionBase(const struct orb_metadata *meta, + unsigned interval, unsigned instance) : + _meta(meta), + _instance(instance), + _handle() +{ + if (_instance > 0) { + _handle = orb_subscribe_multi( + getMeta(), instance); + + } else { + _handle = orb_subscribe(getMeta()); + } + + if (_handle < 0) { warnx("sub failed"); } + + orb_set_interval(getHandle(), interval); +} + +bool SubscriptionBase::updated() +{ + bool isUpdated = false; + int ret = orb_check(_handle, &isUpdated); + + if (ret != PX4_OK) { warnx("orb check failed"); } + + return isUpdated; +} + +void SubscriptionBase::update(void *data) +{ + if (updated()) { + int ret = orb_copy(_meta, _handle, data); + + if (ret != PX4_OK) { warnx("orb copy failed"); } + } +} + +SubscriptionBase::~SubscriptionBase() +{ + int ret = orb_unsubscribe(_handle); + + if (ret != PX4_OK) { warnx("orb unsubscribe failed"); } +} + +template +Subscription::Subscription(const struct orb_metadata *meta, + unsigned interval, + int instance, + List *list) : + SubscriptionNode(meta, interval, instance, list), + _data() // initialize data structure to zero { } -template -Subscription::~Subscription() {} - -template -void *Subscription::getDataVoidPtr() +template +Subscription::~Subscription() { - return (void *)(T *)(this); } -template -T Subscription::getData() +template +void Subscription::update() { - return T(*this); + SubscriptionBase::update((void *)(&_data)); } +template +const T &Subscription::get() { return _data; } + template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; @@ -104,5 +156,11 @@ template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; +template class __EXPORT Subscription; } // namespace uORB diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 49fc9918a1..0136cfcacc 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -42,6 +42,7 @@ #include #include +#include namespace uORB { @@ -60,47 +61,29 @@ public: * * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. - * * @param interval The minimum interval in milliseconds * between updates + * @param instance The instance for multi sub. */ SubscriptionBase(const struct orb_metadata *meta, - unsigned interval = 0) : - _meta(meta), - _handle() - { - setHandle(orb_subscribe(getMeta())); - orb_set_interval(getHandle(), interval); - } + unsigned interval = 0, unsigned instance = 0); /** * Check if there is a new update. * */ - bool updated() - { - bool isUpdated = false; - orb_check(_handle, &isUpdated); - return isUpdated; - } + bool updated(); /** * Update the struct * @param data The uORB message struct we are updating. */ - void update(void *data) - { - if (updated()) { - orb_copy(_meta, _handle, data); - } - } + void update(void *data); /** * Deconstructor */ - virtual ~SubscriptionBase() - { - orb_unsubscribe(_handle); - } + virtual ~SubscriptionBase(); + // accessors const struct orb_metadata *getMeta() { return _meta; } int getHandle() { return _handle; } @@ -109,12 +92,13 @@ protected: void setHandle(int handle) { _handle = handle; } // attributes const struct orb_metadata *_meta; + int _instance; int _handle; private: - // forbid copy + // disallow copy SubscriptionBase(const SubscriptionBase &other); - // forbid assignment - SubscriptionBase &operator = (const SubscriptionBase &); + // disallow assignment + SubscriptionBase &operator=(const SubscriptionBase &other); }; /** @@ -123,7 +107,7 @@ private: typedef SubscriptionBase SubscriptionTiny; /** - * The publication base class as a list node. + * The subscription base class as a list node. */ class __EXPORT SubscriptionNode : @@ -134,18 +118,19 @@ public: /** * Constructor * - * * @param meta The uORB metadata (usually from the ORB_ID() * macro) for the topic. * @param interval The minimum interval in milliseconds * between updates + * @param instance The instance for multi sub. * @param list A pointer to a list of subscriptions * that this should be appended to. */ SubscriptionNode(const struct orb_metadata *meta, unsigned interval = 0, + int instance = 0, List *list = nullptr) : - SubscriptionBase(meta, interval), + SubscriptionBase(meta, interval, instance), _interval(interval) { if (list != nullptr) { list->add(this); } @@ -169,7 +154,6 @@ protected: */ template class __EXPORT Subscription : - public T, // this must be first! public SubscriptionNode { public: @@ -185,7 +169,9 @@ public: */ Subscription(const struct orb_metadata *meta, unsigned interval = 0, + int instance = 0, List *list = nullptr); + /** * Deconstructor */ @@ -195,20 +181,14 @@ public: /** * Create an update function that uses the embedded struct. */ - void update() - { - SubscriptionBase::update(getDataVoidPtr()); - } + void update(); /* - * XXX - * This function gets the T struct, assuming - * the struct is the first base class, this - * should use dynamic cast, but doesn't - * seem to be available - */ - void *getDataVoidPtr(); - T getData(); + * This function gets the T struct data + * */ + const T &get(); +private: + T _data; }; } // namespace uORB From 853672286199facd09a483f2414630e6271d5636 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Tue, 20 Oct 2015 13:36:25 -0400 Subject: [PATCH 086/155] Firmware rename for px4io to avoid using board label in name. --- ROMFS/px4fmu_common/init.d/rcS | 6 +++--- ROMFS/px4fmu_test/init.d/rcS | 6 +++--- src/drivers/px4io/px4io.cpp | 4 ++-- src/firmware/nuttx/CMakeLists.txt | 8 ++++---- src/modules/px4iofirmware/CMakeLists.txt | 2 +- 5 files changed, 13 insertions(+), 13 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f14dd3015d..45128dae52 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -260,11 +260,11 @@ then # # Check if PX4IO present and update firmware if needed # - if [ -f /etc/extras/px4io-v2_default.bin ] + if [ -f /etc/extras/px4io-v2.bin ] then - set IO_FILE /etc/extras/px4io-v2_default.bin + set IO_FILE /etc/extras/px4io-v2.bin else - set IO_FILE /etc/extras/px4io-v1_default.bin + set IO_FILE /etc/extras/px4io-v1.bin fi if px4io checkcrc ${IO_FILE} diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 6d72ecc6c7..0df6c29071 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -55,11 +55,11 @@ else echo "[param] FAILED loading $PARAM_FILE" fi -if [ -f /etc/extras/px4io-v2_default.bin ] +if [ -f /etc/extras/px4io-v2.bin ] then - set io_file /etc/extras/px4io-v2_default.bin + set io_file /etc/extras/px4io-v2.bin else - set io_file /etc/extras/px4io-v1_default.bin + set io_file /etc/extras/px4io-v1.bin fi if px4io start diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3c7c0d2994..9a6581437e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -3468,12 +3468,12 @@ px4io_main(int argc, char *argv[]) } else { #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) - fn[0] = "/etc/extras/px4io-v1_default.bin"; + fn[0] = "/etc/extras/px4io-v1.bin"; fn[1] = "/fs/microsd/px4io1.bin"; fn[2] = "/fs/microsd/px4io.bin"; fn[3] = nullptr; #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) - fn[0] = "/etc/extras/px4io-v2_default.bin"; + fn[0] = "/etc/extras/px4io-v2.bin"; fn[1] = "/fs/microsd/px4io2.bin"; fn[2] = "/fs/microsd/px4io.bin"; fn[3] = nullptr; diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 6304ab2642..4632383bff 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -7,7 +7,7 @@ px4_nuttx_generate_builtin_commands( px4_nuttx_add_romfs(OUT romfs ROOT ROMFS/px4fmu_common - EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL}.bin + EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin ) add_dependencies(romfs fw_io) @@ -81,21 +81,21 @@ if(NOT ${BOARD} STREQUAL "sim") add_custom_target(debug_io COMMAND ${GDB} - ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} DEPENDS firmware_nuttx ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit ) add_custom_target(debug_io_tui COMMAND ${GDBTUI} - ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} DEPENDS firmware_nuttx ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit ) add_custom_target(debug_io_ddd COMMAND ${DDD} --debugger ${GDB} - ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}_${LABEL} + ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board} DEPENDS firmware_nuttx ${CMAKE_CURRENT_BINARY_DIR}/.gdbinit ) diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 7b4c5652c6..3319bec6a2 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -103,7 +103,7 @@ elseif(${config_io_board} STREQUAL "px4io-v2") ) endif() -set(fw_io_name ${config_io_board}_${LABEL}) +set(fw_io_name ${config_io_board}) add_executable(${fw_io_name} ${srcs}) From f0ad724fda8813cb2fcb9af454295e0e15d83c82 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Tue, 20 Oct 2015 13:48:40 -0400 Subject: [PATCH 087/155] Update to fixedwing backside. --- src/modules/fixedwing_backside/fixedwing.cpp | 110 ++++++++++--------- 1 file changed, 59 insertions(+), 51 deletions(-) diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 7984bb12af..39b624f24a 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -142,7 +142,7 @@ void BlockMultiModeBacksideAutopilot::update() // store old position command before update if new command sent if (_missionCmd.updated()) { - _lastMissionCmd = _missionCmd.getData(); + _lastMissionCmd = _missionCmd.get(); } // check for new updates @@ -152,15 +152,17 @@ void BlockMultiModeBacksideAutopilot::update() updateSubscriptions(); // default all output to zero unless handled by mode - for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) { - _actuators.control[i] = 0.0f; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + _actuators.get().control[i] = 0.0f; } // only update guidance in auto mode - if (_status.main_state == MAIN_STATE_AUTO) { + if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? // update guidance - _guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current); + _guide.update(_pos.get(), _att.get(), + _missionCmd.get().current, + _lastMissionCmd.current); } // XXX handle STABILIZED (loiter on spot) as well @@ -168,32 +170,32 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.main_state == MAIN_STATE_AUTO) { + if (_status.get().main_state == vehicle_status_s::MAIN_STATE_AUTO) { // calculate velocity, XXX should be airspeed, // but using ground speed for now for the purpose // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.get().vel_n * _pos.get().vel_n + + _pos.get().vel_e * _pos.get().vel_e + + _pos.get().vel_d * _pos.get().vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); // altitude hold - float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt); + float dThrottle = _h2Thr.update(_missionCmd.get().current.alt - _pos.get().alt); // heading hold - float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); + float psiError = _wrap_pi(_guide.getPsiCmd() - _att.get().yaw); float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); - float pCmd = _phi2P.update(phiCmd - _att.roll); + float pCmd = _phi2P.update(phiCmd - _att.get().roll); // velocity hold // negative sign because nose over to increase speed float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch); // yaw rate cmd float rCmd = 0; @@ -203,14 +205,16 @@ void BlockMultiModeBacksideAutopilot::update() float outputScale = velocityRatio * velocityRatio; // this term scales the output based on the dynamic pressure change from trim _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed, + _att.get().rollspeed, + _att.get().pitchspeed, + _att.get().yawspeed, outputScale); // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - _actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.get().control[CH_THR] = dThrottle + _trimThr.get(); // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once @@ -218,28 +222,29 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. // do not limit in HIL - if (_status.hil_state != HIL_STATE_ON) { + if (_status.get().hil_state != _vehicle_status::HIL_STATE_ON) { /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; + _actuators.get().control[CH_THR] = + (_actuators.get().control[CH_THR] < _manual.get().z) ? + _actuators.get().control[CH_THR] : _manual.get().z; } - } else if (_status.main_state == MAIN_STATE_MANUAL) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; + } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_MANUAL) { + _actuators.get().control[CH_AIL] = _manual.get().y; + _actuators.get().control[CH_ELV] = _manual.get().x; + _actuators.get().control[CH_RDR] = _manual.get().r; + _actuators.get().control[CH_THR] = _manual.get().z; - } else if (_status.main_state == MAIN_STATE_ALTCTL || - _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { + } else if (_status.get().main_state == vehicle_status_s::MAIN_STATE_ALTCTL || + _status.get().main_state == vehicle_status_s::MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vel_n * _pos.vel_n + - _pos.vel_e * _pos.vel_e + - _pos.vel_d * _pos.vel_d)); + _pos.get().vel_n * _pos.get().vel_n + + _pos.get().vel_e * _pos.get().vel_e + + _pos.get().vel_d * _pos.get().vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding @@ -248,33 +253,35 @@ void BlockMultiModeBacksideAutopilot::update() //_crMax.get()*_manual.pitch - _pos.vz); // roll channel -> bank angle - float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.roll); + float phiCmd = _phiLimit.update(_manual.get().y * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.get().roll); // throttle channel -> velocity // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.throttle * + float vCmd = _vLimit.update(_manual.get().z * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin()); float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); + float qCmd = _theta2Q.update(thetaCmd - _att.get().pitch); // yaw rate cmd float rCmd = 0; // stabilization _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _att.get().rollspeed, + _att.get().pitchspeed, + _att.get().yawspeed); // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + _actuators.get().control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); // currently using manual throttle // XXX if you enable this watch out, vz might be very noisy //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); - _actuators.control[CH_THR] = _manual.throttle; + _actuators.get().control[CH_THR] = _manual.get().z; // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once @@ -282,10 +289,11 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (_status.hil_state != HIL_STATE_ON) { + if (_status.get().hil_state != vehicle_status_s::HIL_STATE_ON) { /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; + _actuators.get().control[CH_THR] = + (_actuators.get().control[CH_THR] < _manual.get().z) ? + _actuators.get().control[CH_THR] : _manual.get().z; } // body rates controller, disabled for now @@ -294,13 +302,13 @@ void BlockMultiModeBacksideAutopilot::update() } else if ( 0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _stabilization.update(_manual.get().y, _manual.get().x, _manual.get().r, + _att.get().rollspeed, _att.get().pitchspeed, _att.get().yawspeed); - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; + _actuators.get().control[CH_AIL] = _stabilization.getAileron(); + _actuators.get().control[CH_ELV] = _stabilization.getElevator(); + _actuators.get().control[CH_RDR] = _stabilization.getRudder(); + _actuators.get().control[CH_THR] = _manual.get().z; } // update all publications @@ -311,8 +319,8 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() { // send one last publication when destroyed, setting // all output to zero - for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { - _actuators.control[i] = 0.0f; + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) { + _actuators.get().control[i] = 0.0f; } updatePublications(); From ceffa3fadbb8b11fd86035f0e654ab546db5a44d Mon Sep 17 00:00:00 2001 From: Jonathan Lee Date: Tue, 20 Oct 2015 10:34:11 -0700 Subject: [PATCH 088/155] uORB Devices NuttX: Fix Build under GCC 4.9.x Remove "algorithm.h" from included files; similar bug and solution as seen in #3022 --- src/modules/uORB/uORBDevices_nuttx.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/uORB/uORBDevices_nuttx.cpp b/src/modules/uORB/uORBDevices_nuttx.cpp index 5d0ce3b99e..55f9d353f3 100644 --- a/src/modules/uORB/uORBDevices_nuttx.cpp +++ b/src/modules/uORB/uORBDevices_nuttx.cpp @@ -36,7 +36,6 @@ #include #include #include -#include #include "uORBDevices_nuttx.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" @@ -684,4 +683,3 @@ uORB::DeviceNode *uORB::DeviceMaster::GetDeviceNode(const char *nodepath) return rc; } - From 3ac5b3b84efcda2543a1e33b6c0c2173e8bd7d8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 22 Oct 2015 00:25:37 +0200 Subject: [PATCH 089/155] Controllib: Enforce code style --- Tools/check_code_style.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index d911f15fa0..06c6bea1cc 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -28,6 +28,7 @@ for fn in $(find src/examples \ src/modules/segway \ src/modules/unit_test \ src/modules/systemlib \ + src/modules/controllib \ -path './Build' -prune -o \ -path './mavlink' -prune -o \ -path './NuttX' -prune -o \ From 0e967d08747999e6fea195d26f947a65c20dfc53 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 22 Oct 2015 10:07:22 -0700 Subject: [PATCH 090/155] qurt: Change to Hexagon 7.2.10 compiler Switched to 7.2.10 since the Linux version of this is available. Added -DHAS_C9X flag to fix isses with undefined math functions when using hexagon-clang++. Signed-off-by: Mark Charlebois --- cmake/common/px4_base.cmake | 2 +- cmake/configs/qurt_eagle_hello.cmake | 2 +- cmake/configs/qurt_eagle_hil.cmake | 2 +- cmake/configs/qurt_eagle_muorb.cmake | 2 +- cmake/configs/qurt_eagle_release.cmake | 2 +- cmake/configs/qurt_eagle_test.cmake | 2 +- cmake/configs/qurt_eagle_travis.cmake | 2 +- ...agon-7.4.cmake => Toolchain-hexagon-7.2.10.cmake} | 5 +++-- src/platforms/px4_defines.h | 12 ------------ 9 files changed, 10 insertions(+), 21 deletions(-) rename cmake/toolchains/{Toolchain-hexagon-7.4.cmake => Toolchain-hexagon-7.2.10.cmake} (98%) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 5c9b8aefdf..5c8ea5787a 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -492,7 +492,6 @@ function(px4_add_common_flags) -Wpointer-arith -Wmissing-declarations -Wno-unused-parameter - -Wno-varargs -Werror=format-security -Werror=array-bounds -Wfatal-errors @@ -517,6 +516,7 @@ function(px4_add_common_flags) if (NOT ${OS} STREQUAL "qurt") list(APPEND warnings -Wno-unused-const-variable + -Wno-varargs ) endif() else() diff --git a/cmake/configs/qurt_eagle_hello.cmake b/cmake/configs/qurt_eagle_hello.cmake index c127216681..4328a3d4aa 100644 --- a/cmake/configs/qurt_eagle_hello.cmake +++ b/cmake/configs/qurt_eagle_hello.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake index 0b48820254..4222fff249 100644 --- a/cmake/configs/qurt_eagle_hil.cmake +++ b/cmake/configs/qurt_eagle_hil.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_muorb.cmake b/cmake/configs/qurt_eagle_muorb.cmake index 492225aa40..d73d7becff 100644 --- a/cmake/configs/qurt_eagle_muorb.cmake +++ b/cmake/configs/qurt_eagle_muorb.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_release.cmake b/cmake/configs/qurt_eagle_release.cmake index 494416ed4f..945e9da199 100644 --- a/cmake/configs/qurt_eagle_release.cmake +++ b/cmake/configs/qurt_eagle_release.cmake @@ -20,7 +20,7 @@ set(target_libraries ) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list # diff --git a/cmake/configs/qurt_eagle_test.cmake b/cmake/configs/qurt_eagle_test.cmake index b2935f1fda..2e6a370950 100644 --- a/cmake/configs/qurt_eagle_test.cmake +++ b/cmake/configs/qurt_eagle_test.cmake @@ -1,6 +1,6 @@ include(qurt/px4_impl_qurt) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake index 3b4672a115..b9fda6ec48 100644 --- a/cmake/configs/qurt_eagle_travis.cmake +++ b/cmake/configs/qurt_eagle_travis.cmake @@ -3,7 +3,7 @@ include(qurt/px4_impl_qurt) # Run a full link with build stubs to make sure qurt target isn't broken set(QURT_ENABLE_STUBS "1") -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.4.cmake) +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake) set(config_module_list drivers/device diff --git a/cmake/toolchains/Toolchain-hexagon-7.4.cmake b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake similarity index 98% rename from cmake/toolchains/Toolchain-hexagon-7.4.cmake rename to cmake/toolchains/Toolchain-hexagon-7.2.10.cmake index 9c4286044d..a3930a066e 100644 --- a/cmake/toolchains/Toolchain-hexagon-7.4.cmake +++ b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake @@ -35,7 +35,7 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) include(common/px4_base) if(NOT HEXAGON_TOOLS_ROOT) - set(HEXAGON_TOOLS_ROOT /opt/7.4/Tools) + set(HEXAGON_TOOLS_ROOT /opt/HEXAGON_Tools/7.2.10/Tools) endif() macro (list2string out in) @@ -53,7 +53,7 @@ set(HEXAGON_LIB_DIR ${HEXAGON_TOOLS_ROOT}/gnu/hexagon/lib) set(HEXAGON_ISS_DIR ${HEXAGON_TOOLS_ROOT}/lib/iss) set(TOOLSLIB ${HEXAGON_TOOLS_ROOT}/target/hexagon/lib/${V_ARCH}/G0) -# Use the HexagonTools compiler (6.4.05) +# Use the HexagonTools compiler (7.2.10) set(CMAKE_C_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang) set(CMAKE_CXX_COMPILER ${HEXAGON_BIN}/${CROSSDEV}clang++) @@ -82,6 +82,7 @@ set(ARCHCPUFLAGS add_definitions( -D_PID_T -D_UID_T -D_TIMER_T -Dnoreturn_function= + -D_HAS_C9X -D__EXPORT= -Drestrict= -D_DEBUG diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index b80532b099..20667f220f 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -224,8 +224,6 @@ __END_DECLS #endif -#if defined(__PX4_QURT) - #define PX4_ROOTFSDIR #define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" @@ -234,16 +232,6 @@ __END_DECLS // Missing math.h defines #define PX4_ISFINITE(x) __builtin_isfinite(x) -// FIXME - these are missing for clang++ but not for clang -#if defined(__cplusplus) -#define isfinite(x) true -#define isnan(x) false -#define isinf(x) false -#define fminf(x, y) ((x) > (y) ? y : x) -#endif - -#endif - /* *Defines for all platforms */ From 49e0fc967ac976713248724dac5f0579da32bb81 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 22 Oct 2015 10:12:32 -0700 Subject: [PATCH 091/155] qurt: Added back missing ifdef section in px4_defines.h Signed-off-by: Mark Charlebois --- src/platforms/px4_defines.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 20667f220f..06bddf2eb6 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -224,6 +224,8 @@ __END_DECLS #endif +#if defined(__PX4_QURT) + #define PX4_ROOTFSDIR #define DEFAULT_PARAM_FILE "/fs/eeprom/parameters" @@ -232,6 +234,8 @@ __END_DECLS // Missing math.h defines #define PX4_ISFINITE(x) __builtin_isfinite(x) +#endif + /* *Defines for all platforms */ From 6de5a36518adb723c6604ad416f40e35482b039d Mon Sep 17 00:00:00 2001 From: gcarmix Date: Thu, 22 Oct 2015 21:16:55 +0200 Subject: [PATCH 092/155] Driver for SRF02 range finder adapted from Maxbotix driver --- src/drivers/srf02/module.mk | 42 ++ src/drivers/srf02/srf02.cpp | 989 ++++++++++++++++++++++++++++++++++++ 2 files changed, 1031 insertions(+) create mode 100644 src/drivers/srf02/module.mk create mode 100644 src/drivers/srf02/srf02.cpp diff --git a/src/drivers/srf02/module.mk b/src/drivers/srf02/module.mk new file mode 100644 index 0000000000..a7bbf0225f --- /dev/null +++ b/src/drivers/srf02/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Makefile to build the Maxbotix Sonar driver. +# + +MODULE_COMMAND = srf02 + +SRCS = srf02.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp new file mode 100644 index 0000000000..014d430d2b --- /dev/null +++ b/src/drivers/srf02/srf02.cpp @@ -0,0 +1,989 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file srf02.cpp + * + * Driver for the SRF02 sonar range finders connected via I2C, + * adapted from the Maxbotix (mb12xx) driver. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include + +/* Configuration Constants */ +#define SRF02_MAX_RANGEFINDERS 4 +#define SRF02_BUS PX4_I2C_BUS_EXPANSION +#define SRF02_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ +#define SRF02_DEVICE_PATH "/dev/srf02" + +/* SRF02 Registers addresses */ + +#define SRF02_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define SRF02_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define SRF02_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define SRF02_MIN_DISTANCE (0.20f) +#define SRF02_MAX_DISTANCE (7.65f) + +#define SRF02_CONVERSION_INTERVAL 100000 /* 60ms for one sonar */ +#define TICKS_BETWEEN_SUCCESIVE_FIRES 100000 /* 30ms between each sonar measurement (watch out for interference!) */ + + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class SRF02 : public device::I2C +{ +public: + SRF02(int bus = SRF02_BUS, int address = SRF02_BASEADDR); + virtual ~SRF02(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + RingBuffer *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + int _class_instance; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + uint8_t _cycle_counter; /* counter in cycle to change i2c adresses */ + int _cycling_rate; /* */ + uint8_t _index_counter; /* temporary sonar i2c address */ + std::vector addr_ind; /* temp sonar i2c address vector */ + std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults SRF02_MIN_DISTANCE + * and SRF02_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int srf02_main(int argc, char *argv[]); + +SRF02::SRF02(int bus, int address) : + I2C("SRF02", SRF02_DEVICE_PATH, bus, address, 100000), + _min_distance(SRF02_MIN_DISTANCE), + _max_distance(SRF02_MAX_DISTANCE), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _class_instance(-1), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "srf02_read")), + _comms_errors(perf_alloc(PC_COUNT, "srf02_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "srf02_buffer_overflows")), + _cycle_counter(0), /* initialising counter for cycling function to zero */ + _cycling_rate(0), /* initialising cycling rate (which can differ depending on one sonar or multiple) */ + _index_counter(0) /* initialising temp sonar i2c address to zero */ + +{ + /* enable debug() calls */ + _debug_enabled = true; + + /* work_cancel in the dtor will explode if we don't do this... */ + memset(&_work, 0, sizeof(_work)); +} + +SRF02::~SRF02() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) { + delete _reports; + } + + if (_class_instance != -1) { + unregister_class_devname(RANGE_FINDER_BASE_DEVICE_PATH, _class_instance); + } + + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_buffer_overflows); +} + +int +SRF02::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) { + goto out; + } + + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + + _index_counter = SRF02_BASEADDR; /* set temp sonar i2c address to base adress */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + + if (_reports == nullptr) { + goto out; + } + + _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); + + if (_class_instance == CLASS_DEVICE_PRIMARY) { + /* get a publish handle on the range finder topic */ + struct range_finder_report rf_report = {}; + + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + + if (_range_finder_topic < 0) { + log("failed to create sensor_range_finder object. Did you start uOrb?"); + } + } + + // XXX we should find out why we need to wait 200 ms here + usleep(200000); + + /* check for connected rangefinders on each i2c port: + We start from i2c base address (0x70 = 112) and count downwards + So second iteration it uses i2c address 111, third iteration 110 and so on*/ + for (unsigned counter = 0; counter <= SRF02_MAX_RANGEFINDERS; counter++) { + _index_counter = SRF02_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ + set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ + int ret2 = measure(); + + if (ret2 == 0) { /* sonar is present -> store address_index in array */ + addr_ind.push_back(_index_counter); + debug("sonar added"); + _latest_sonar_measurements.push_back(200); + } + } + + _index_counter = SRF02_BASEADDR; + set_address(_index_counter); /* set i2c port back to base adress for rest of driver */ + + /* if only one sonar detected, no special timing is required between firing, so use default */ + if (addr_ind.size() == 1) { + _cycling_rate = SRF02_CONVERSION_INTERVAL; + + } else { + _cycling_rate = TICKS_BETWEEN_SUCCESIVE_FIRES; + } + + /* show the connected sonars in terminal */ + for (unsigned i = 0; i < addr_ind.size(); i++) { + log("sonar %d with address %d added", (i + 1), addr_ind[i]); + } + + debug("Number of sonars connected: %d", addr_ind.size()); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +SRF02::probe() +{ + return measure(); +} + +void +SRF02::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +SRF02::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +SRF02::get_minimum_distance() +{ + return _min_distance; +} + +float +SRF02::get_maximum_distance() +{ + return _max_distance; +} + +int +SRF02::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(_cycling_rate); + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + + } + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + int ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(_cycling_rate)) { + return -EINVAL; + } + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) { + start(); + } + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) { + return SENSOR_POLLRATE_MANUAL; + } + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 1) || (arg > 100)) { + return -EINVAL; + } + + irqstate_t flags = irqsave(); + + if (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } + + irqrestore(flags); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + + case RANGEFINDERIOCSETMAXIUMDISTANCE: { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +SRF02::read(struct file *filp, char *buffer, size_t buflen) +{ + + unsigned count = buflen / sizeof(struct range_finder_report); + struct range_finder_report *rbuf = reinterpret_cast(buffer); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) { + return -ENOSPC; + } + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_reports->get(rbuf)) { + ret += sizeof(*rbuf); + rbuf++; + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + do { + _reports->flush(); + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(_cycling_rate * 2); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + if (_reports->get(rbuf)) { + ret = sizeof(*rbuf); + } + + } while (0); + + return ret; +} + +int +SRF02::measure() +{ + + int ret; + + /* + * Send the command to begin a measurement. + */ + + uint8_t cmd[2]; + cmd[0] = 0x00; + cmd[1] = SRF02_TAKE_RANGE_REG; + ret = transfer(cmd, 2, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + debug("i2c::transfer returned %d", ret); + return ret; + } + + ret = OK; + + return ret; +} + +int +SRF02::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + uint8_t cmd = 0x02; + perf_begin(_sample_perf); + ret = transfer(&cmd,1,nullptr,0); + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) { + debug("error reading from sensor: %d", ret); + perf_count(_comms_errors); + perf_end(_sample_perf); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + struct range_finder_report report; + + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); + + /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ + if (addr_ind.size() == 1) { + report.distance = si_units; + report.distance_vector[0] = si_units; + for (unsigned i = 1; i < (SRF02_MAX_RANGEFINDERS); i++) { + report.distance_vector[i] = 0; + } + + report.just_updated = 0; + + } else { + /* for multiple sonars connected */ + + /* don't use the original single sonar variable */ + report.distance = 0; + + /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ + _latest_sonar_measurements[_cycle_counter] = si_units; + + for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { + report.distance_vector[i] = _latest_sonar_measurements[i]; + } + + /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ + report.just_updated = _index_counter; + + /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ + for (unsigned i = 0; i < (SRF02_MAX_RANGEFINDERS - addr_ind.size()); i++) { + report.distance_vector[addr_ind.size() + i] = 0; + } + } + + report.minimum_distance = get_minimum_distance(); + report.maximum_distance = get_maximum_distance(); + report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it, if we are the primary */ + if (_range_finder_topic >= 0) { + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + } + + if (_reports->force(&report)) { + perf_count(_buffer_overflows); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + + perf_end(_sample_perf); + return ret; +} + +void +SRF02::start() +{ + + /* reset the report ring and state machine */ + _collect_phase = false; + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&SRF02::cycle_trampoline, this, 5); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER + }; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + + + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + + } +} + +void +SRF02::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +SRF02::cycle_trampoline(void *arg) +{ + + SRF02 *dev = (SRF02 *)arg; + + dev->cycle(); + +} + +void +SRF02::cycle() +{ + if (_collect_phase) { + _index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */ + set_address(_index_counter); + + /* perform collection */ + if (OK != collect()) { + debug("collection error"); + /* if error restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* change i2c adress to next sonar */ + _cycle_counter = _cycle_counter + 1; + + if (_cycle_counter >= addr_ind.size()) { + _cycle_counter = 0; + } + + /* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate + Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */ + + if (_measure_ticks > USEC2TICK(_cycling_rate)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&SRF02::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(_cycling_rate)); + return; + } + } + + /* Measurement (firing) phase */ + + /* ensure sonar i2c adress is still correct */ + _index_counter = addr_ind[_cycle_counter]; + set_address(_index_counter); + + /* Perform measurement */ + if (OK != measure()) { + debug("measure error sonar adress %d", _index_counter); + } + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&SRF02::cycle_trampoline, + this, + USEC2TICK(_cycling_rate)); + +} + +void +SRF02::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + _reports->print_info("report queue"); +} + +/** + * Local functions in support of the shell command. + */ +namespace srf02 +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +SRF02 *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) { + errx(1, "already started"); + } + + /* create the driver */ + g_dev = new SRF02(SRF02_BUS); + + if (g_dev == nullptr) { + goto fail; + } + + if (OK != g_dev->init()) { + goto fail; + } + + /* set the poll rate to default, starts automatic data collection */ + fd = open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + goto fail; + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + goto fail; + } + + exit(0); + +fail: + + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) { + delete g_dev; + g_dev = nullptr; + + } else { + errx(1, "driver not running"); + } + + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "%s open failed (try 'srf02 start' if the driver is not running", SRF02_DEVICE_PATH); + } + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "immediate read failed"); + } + + warnx("single read"); + warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { + errx(1, "failed to set 2Hz poll rate"); + } + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) { + errx(1, "timed out waiting for sensor data"); + } + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) { + err(1, "periodic read failed"); + } + + warnx("periodic read %u", i); + + /* Print the sonar rangefinder report sonar distance vector */ + for (uint8_t count = 0; count < SRF02_MAX_RANGEFINDERS; count++) { + warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); + } + + warnx("time: %lld", report.timestamp); + } + + /* reset the sensor polling to default rate */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { + errx(1, "failed to set default poll rate"); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(SRF02_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + err(1, "failed "); + } + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { + err(1, "driver reset failed"); + } + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { + err(1, "driver poll restart failed"); + } + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) { + errx(1, "driver not running"); + } + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} /* namespace */ + +int +srf02_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) { + srf02::start(); + } + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) { + srf02::stop(); + } + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) { + srf02::test(); + } + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) { + srf02::reset(); + } + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { + srf02::info(); + } + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} From 28316d8a445d01d3ad2f653bf49a15435d529643 Mon Sep 17 00:00:00 2001 From: gcarmix Date: Thu, 22 Oct 2015 22:55:23 +0200 Subject: [PATCH 093/155] Corrected code format with astyle --- src/drivers/srf02/srf02.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index 014d430d2b..8bd874d2b6 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -141,7 +141,8 @@ private: int _cycling_rate; /* */ uint8_t _index_counter; /* temporary sonar i2c address */ std::vector addr_ind; /* temp sonar i2c address vector */ - std::vector _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ + std::vector + _latest_sonar_measurements; /* vector to store latest sonar measurements in before writing to report */ /** @@ -561,7 +562,7 @@ SRF02::collect() uint8_t val[2] = {0, 0}; uint8_t cmd = 0x02; perf_begin(_sample_perf); - ret = transfer(&cmd,1,nullptr,0); + ret = transfer(&cmd, 1, nullptr, 0); ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { @@ -583,6 +584,7 @@ SRF02::collect() if (addr_ind.size() == 1) { report.distance = si_units; report.distance_vector[0] = si_units; + for (unsigned i = 1; i < (SRF02_MAX_RANGEFINDERS); i++) { report.distance_vector[i] = 0; } From 4b1e4e63f02d473cebf652a4628a846216a6a5d3 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 22 Oct 2015 20:30:46 -0700 Subject: [PATCH 094/155] eagle: Added posix_eagle_release to Makefile Enable build of posix_eagle_release. Added path to ARM cross compiler in qrlsdk install. Fixed warnings in Toolchain-arm-linux-gnueabihf.cmake Signed-off-by: Mark Charlebois --- Makefile | 3 +++ cmake/configs/posix_eagle_release.cmake | 9 +++++++++ cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake | 6 +++--- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index b896e51391..cd15007625 100644 --- a/Makefile +++ b/Makefile @@ -125,6 +125,9 @@ ros_sitl_simple: qurt_eagle_travis: $(call cmake-build,$@) +posix_eagle_release: + $(call cmake-build,$@) + posix: posix_sitl_simple posix_sitl_default: posix_sitl_simple diff --git a/cmake/configs/posix_eagle_release.cmake b/cmake/configs/posix_eagle_release.cmake index 8e3bb26078..b0b2a9171d 100644 --- a/cmake/configs/posix_eagle_release.cmake +++ b/cmake/configs/posix_eagle_release.cmake @@ -5,6 +5,15 @@ if("${DSPAL_STUBS_ENABLE}" STREQUAL "") set(DSPAL_STUBS_ENABLE "1") endif() +if ("${QRL_SDK_DIR}" STREQUAL "") + set(QRL_SDK_DIR /opt/qrlsdk) +endif() + +set(CMAKE_PROGRAM_PATH + "${QRL_SDK_DIR}/gcc-linaro-4.8-2015.06-x86_64_arm-linux-gnueabihf/bin" + ${CMAKE_PROGRAM_PATH} + ) + set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake) set(config_module_list diff --git a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake index 61d63f8677..eb48554b16 100644 --- a/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake +++ b/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake @@ -26,13 +26,13 @@ set(CMAKE_SYSTEM_VERSION 1) # specify the cross compiler find_program(C_COMPILER arm-linux-gnueabihf-gcc) if(NOT C_COMPILER) - message(FATAL_ERROR "could not find arm-none-eabi-gcc compiler") + message(FATAL_ERROR "could not find arm-linux-gnueabihf-gcc compiler") endif() cmake_force_c_compiler(${C_COMPILER} GNU) find_program(CXX_COMPILER arm-linux-gnueabihf-g++) if(NOT CXX_COMPILER) - message(FATAL_ERROR "could not find arm-none-eabi-g++ compiler") + message(FATAL_ERROR "could not find arm-linux-gnueabihf-g++ compiler") endif() cmake_force_cxx_compiler(${CXX_COMPILER} GNU) @@ -41,7 +41,7 @@ foreach(tool objcopy nm ld) string(TOUPPER ${tool} TOOL) find_program(${TOOL} arm-linux-gnueabihf-${tool}) if(NOT ${TOOL}) - message(FATAL_ERROR "could not find ${tool}") + message(FATAL_ERROR "could not find arm-linux-gnueabihf-${tool}") endif() endforeach() From 06f5e242d19cc5334e4795514d74c6c54f2fc571 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 22 Oct 2015 16:30:23 +0200 Subject: [PATCH 095/155] support launching gazebo automatically for SITL --- Makefile | 2 +- Tools/sitl_run.sh | 9 +++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index cd15007625..c0413ea7ef 100644 --- a/Makefile +++ b/Makefile @@ -138,7 +138,7 @@ run_sitl_quad: posix Tools/sitl_run.sh posix-configs/SITL/init/rcS none jmavsim run_sitl_iris: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS_iris_gazebo + Tools/sitl_run.sh posix-configs/SITL/init/rcS_iris_gazebo none gazebo run_sitl_plane: posix Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 1d80464fad..7bd1a75bb2 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -7,6 +7,15 @@ then ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & cd ../.. +elif [ "$3" == "gazebo" ] +then + if [ -x "$(command -v gazebo)" ] + then + gazebo ${SITL_GAZEBO_PATH}/worlds/iris.world & + else + echo "You need to have gazebo simulator installed!" + exit 1 + fi fi cd build_posix_sitl_simple/src/firmware/posix mkdir -p rootfs/fs/microsd From 766cb716354e4346d842e299ae050147fec06fef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 11:01:56 +0200 Subject: [PATCH 096/155] EKF: Push up stack size as the max frame size is already 3.4K --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 61bd32a1b3..7f9f56ae81 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1122,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 3900, + 4600, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); From 7fd6d2dc4a4fe7a1b439b5cac78ec90fb34aca48 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 11:03:30 +0200 Subject: [PATCH 097/155] Move EKF stack back to same size as on stable --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7f9f56ae81..afdb850579 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1122,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4600, + 4800, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); From 1e973033af7bb0a05f9916f39258ad91222c509e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 11:16:47 +0200 Subject: [PATCH 098/155] Set climbout altitude to 10 meters by default --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index cfd1988f44..5870039c24 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -179,7 +179,7 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); * @max 150.0 * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); +PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f); /** * Maximum climb rate From 7d517978f0dd53b20d9ba8fbfd7917a754c054e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 11:20:53 +0200 Subject: [PATCH 099/155] MAVLink: Disable named value float --- src/modules/mavlink/mavlink_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 297a76239b..a12133c021 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1676,7 +1676,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GPS_RAW_INT", 1.0f); configure_stream("GLOBAL_POSITION_INT", 3.0f); configure_stream("LOCAL_POSITION_NED", 3.0f); - configure_stream("NAMED_VALUE_FLOAT", 2.0f); configure_stream("RC_CHANNELS", 1.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); From f78fea2e962eb8bb2466b5d2fd5fc5d607430de1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 11:21:07 +0200 Subject: [PATCH 100/155] Revert "Move EKF stack back to same size as on stable" This reverts commit 7fd6d2dc4a4fe7a1b439b5cac78ec90fb34aca48. --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index afdb850579..7f9f56ae81 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1122,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start() _estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 20, - 4800, + 4600, (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline, nullptr); From ec1d148b08ef9a639763eae9728ff2d0adbbfd6b Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 23 Oct 2015 13:41:28 +0200 Subject: [PATCH 101/155] fixed missing dt in ekf --- .../ekf_att_pos_estimator_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 7f9f56ae81..04be54eb33 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1245,9 +1245,9 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; } else { - _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]); - _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]); - _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]); + _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; + _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; + _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; } _ekf->angRate.x = _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]; @@ -1265,9 +1265,9 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 1]; _ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[_accel_main * 3 + 2]; } else { - _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]); - _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]); - _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]); + _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]) * _ekf->dtIMU; + _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]) * _ekf->dtIMU; + _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) * _ekf->dtIMU; } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; From b081c5c33b53e7bc80780ae507d77b9de10f60ef Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 23 Oct 2015 14:00:24 +0200 Subject: [PATCH 102/155] added signal handler for floating point exceptions --- src/platforms/posix/main.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 26465c266e..1b22627721 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -66,6 +66,13 @@ extern "C" { cout.flush(); _exit(0); } + void _SigFpeHandler(int sig_num); + void _SigFpeHandler(int sig_num) + { + cout.flush(); + cout << endl << "floating point exception" << endl; + cout.flush(); + } } static void print_prompt() @@ -137,6 +144,7 @@ int main(int argc, char **argv) { bool daemon_mode = false; signal(SIGINT, _SigIntHandler); + signal(SIGFPE, _SigFpeHandler); int index = 1; bool error_detected = false; From e5d6b1c985ada6d1352149b3497fe31e39be3523 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 19:53:09 +0200 Subject: [PATCH 103/155] Tools: Parse new decimals tag --- Tools/px4params/srcparser.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py index 2201fac6dd..b67ad890a9 100644 --- a/Tools/px4params/srcparser.py +++ b/Tools/px4params/srcparser.py @@ -45,6 +45,7 @@ class Parameter(object): "min": 5, "max": 4, "unit": 3, + "decimal": 2, # all others == 0 (sorted alphabetically) } @@ -106,7 +107,7 @@ class SourceParser(object): re_remove_dots = re.compile(r'\.+$') re_remove_carriage_return = re.compile('\n+') - valid_tags = set(["group", "board", "min", "max", "unit"]) + valid_tags = set(["group", "board", "min", "max", "unit", "decimal"]) # Order of parameter groups priority = { From e5ccf82fa27400f7f30e7d953e5bca901a84b58c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 19:53:22 +0200 Subject: [PATCH 104/155] Attitude Q estimator: Set decimal places --- .../attitude_estimator_q/attitude_estimator_q_params.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index d72f62dc61..d3959db4cf 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -47,6 +47,7 @@ * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); @@ -56,6 +57,7 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); @@ -65,6 +67,7 @@ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); * @group Attitude Q estimator * @min 0 * @max 1 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); @@ -77,6 +80,7 @@ PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f); * * @group Attitude Q estimator * @unit degrees + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -106,6 +110,7 @@ PARAM_DEFINE_INT32(ATT_ACC_COMP, 2); * @min 0 * @max 2 * @unit rad/s + * @decimal 3 */ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); @@ -113,7 +118,8 @@ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f); * Threshold (of RMS) to warn about high vibration levels * * @group Attitude Q estimator - * @min 0.001 - * @max 100 + * @min 0.01 + * @max 10 + * @decimal 2 */ PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f); From 3e4f0aabaad24a52fe436214b23cdafb24acab63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 20:29:52 +0200 Subject: [PATCH 105/155] Uncruft IRIS config --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 3 --- 1 file changed, 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index ffaa935055..30af86730e 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -32,6 +32,3 @@ fi set MIXER quad_w set PWM_OUT 1234 - -set PWM_MIN 1200 -set PWM_MAX 1950 From 06b96468b6eed2adc41dec1c8aa49aed24637fb7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 20:30:08 +0200 Subject: [PATCH 106/155] Uncruft Disco config --- ROMFS/px4fmu_common/init.d/10015_tbs_discovery | 8 -------- 1 file changed, 8 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery index 0acb4088f4..97beb6831a 100644 --- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery +++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery @@ -28,11 +28,3 @@ fi set MIXER quad_w set PWM_OUT 1234 -set PWM_MIN 1200 - -set MIXER_AUX pass -set PWM_AUX_RATE 50 -set PWM_AUX_OUT 1234 -set PWM_AUX_DISARMED 1000 -set PWM_AUX_MIN 1000 -set PWM_AUX_MAX 2000 From df3acc2f5113672399da9294dd35403911d68270 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 20:30:21 +0200 Subject: [PATCH 107/155] Uncruft steadidrone config --- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 3 --- 1 file changed, 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index 7e27043156..7accc4f6e9 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -30,7 +30,4 @@ fi set MIXER quad_w -set PWM_MIN 1210 -set PWM_MAX 2100 - set PWM_OUT 1234 From d3125311c87f63663d7e62ac160cc523bb2307cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 20:30:40 +0200 Subject: [PATCH 108/155] Uncruft SK450 config --- ROMFS/px4fmu_common/init.d/10019_sk450_deadcat | 1 - 1 file changed, 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index 4f22ddc436..0b1e3b2dfa 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -30,7 +30,6 @@ fi set MIXER sk450_deadcat set PWM_OUT 1234 -set PWM_MIN 1050 set PWM_AUX_OUT 1234 # set PWM_AUX_MIN 900 From 3276da9ce6d4c8293af09ed092c36e9461d3345a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 23:27:10 +0200 Subject: [PATCH 109/155] Add Snapdragon requirements to Vagrant --- Vagrantfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Vagrantfile b/Vagrantfile index 775abd461f..c24bfe6859 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -78,7 +78,7 @@ Vagrant.configure(2) do |config| echo "cd /Firmware" >> ~/.bashrc # Install software sudo apt-get update - sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip gdb + sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip gdb gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf pushd . cd ~ wget -q https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 From bf09c46c2f2ada7fef00f1b1155b3da84b17170b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 23:28:26 +0200 Subject: [PATCH 110/155] Fix Hexagon toolchain detection for default installer --- cmake/toolchains/Toolchain-hexagon-7.2.10.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake index a3930a066e..4e27436ec7 100644 --- a/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake +++ b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake @@ -35,7 +35,7 @@ list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) include(common/px4_base) if(NOT HEXAGON_TOOLS_ROOT) - set(HEXAGON_TOOLS_ROOT /opt/HEXAGON_Tools/7.2.10/Tools) + set(HEXAGON_TOOLS_ROOT $ENV{HOME}/Qualcomm/HEXAGON_Tools/7.2.10/Tools) endif() macro (list2string out in) From 292b87fe2cfa2a5a00616c5ed13516bd60199f5a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 23:58:28 +0200 Subject: [PATCH 111/155] Exit simulator --- Tools/sitl_run.sh | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 7bd1a75bb2..d80fcc2c70 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -1,17 +1,22 @@ #!/bin/bash cp Tools/posix_lldbinit build_posix_sitl_simple/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit build_posix_sitl_simple/src/firmware/posix/.gdbinit + +SIM_PID=0 + if [ "$3" == "jmavsim" ] then cd Tools/jMAVSim ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & + SIM_PID=echo $! cd ../.. elif [ "$3" == "gazebo" ] then if [ -x "$(command -v gazebo)" ] then gazebo ${SITL_GAZEBO_PATH}/worlds/iris.world & + SIM_PID=echo $! else echo "You need to have gazebo simulator installed!" exit 1 @@ -31,3 +36,11 @@ then else ./mainapp ../../../../$1 fi + +if [ "$3" == "jmavsim" ] +then + kill -9 $SIM_PID +elif [ "$3" == "gazebo" ] +then + kill -9 $SIM_PID +fi From 8349ba180bb0a1acb6b5606163dac1998e173c43 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 23 Oct 2015 17:37:46 -0700 Subject: [PATCH 112/155] DSPAL: removed simlinks that broke vagrant build Vagrant did not like the simlinks in the dspal repo. Signed-off-by: Mark Charlebois --- src/lib/dspal | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/dspal b/src/lib/dspal index 95e91546f4..caf445fe1e 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit 95e91546f42e6d88d34a2bb29d0f428a8706c9e4 +Subproject commit caf445fe1e54a9631c6d4c6451f23452adb9897c From 01ccbd8886ff549fff3239138f5cc6b55e353b91 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 23 Oct 2015 20:42:15 -0700 Subject: [PATCH 113/155] qurt: added HEXAGON_TOOLS_ROOT variable The environment variable HEXAGON_TOOLS_ROOT must be set to the location of the installed HexagonTools. This could be a global install at /opt/HEXAGON_Tools/7.2.10/Tools or it could be a personal install ${HOME}/Qualcomm/HEXAGON_Tools/7.2.10/Tools. The default install path used by the installer is exported in the Vagrantfile. There is still an issue that the hexagon linker fails on mmap of the shared folder. If the shared folder was mounted via NFS for Linux and MacOS the problem would be resolved. Signed-off-by: Mark Charlebois --- Vagrantfile | 2 ++ cmake/toolchains/Toolchain-hexagon-7.2.10.cmake | 17 +++++++++++++++-- src/lib/dspal | 2 +- 3 files changed, 18 insertions(+), 3 deletions(-) diff --git a/Vagrantfile b/Vagrantfile index c24bfe6859..2fbf532b74 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -85,6 +85,8 @@ Vagrant.configure(2) do |config| tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi + exportline2="export HEXAGON_TOOLS_ROOT=$HOME/Qualcomm/HEXAGON_Tools/7.2.10/Tools" + if grep -Fxq "$exportline2" ~/.profile; then echo nothing to do ; else echo $exportline2 >> ~/.profile; fi . ~/.profile popd # setup ccache diff --git a/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake index 4e27436ec7..1ab6fd4060 100644 --- a/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake +++ b/cmake/toolchains/Toolchain-hexagon-7.2.10.cmake @@ -34,8 +34,12 @@ include(CMakeForceCompiler) list(APPEND CMAKE_MODULE_PATH ${CMAKE_SOURCE_DIR}/cmake) include(common/px4_base) -if(NOT HEXAGON_TOOLS_ROOT) - set(HEXAGON_TOOLS_ROOT $ENV{HOME}/Qualcomm/HEXAGON_Tools/7.2.10/Tools) +if ("$ENV{HEXAGON_TOOLS_ROOT}" STREQUAL "") + message(FATAL_ERROR + "The HexagonTools version 7.2.10 must be installed and the environment variable HEXAGON_TOOLS_ROOT must be set" + "(e.g. export HEXAGON_TOOLS_ROOT=/opt/HEXAGON_Tools/7.2.10/Tools)") +else() + set(HEXAGON_TOOLS_ROOT $ENV{HEXAGON_TOOLS_ROOT}) endif() macro (list2string out in) @@ -239,3 +243,12 @@ set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) # for libraries and headers in the target directories set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) + +# The Hexagon compiler doesn't support the -rdynamic flag and this is set +# in the base cmake scripts. We have to redefine the __linux_compiler_gnu +# macro for cmake 2.8 to work +set(__LINUX_COMPILER_GNU 1) +macro(__linux_compiler_gnu lang) + set(CMAKE_SHARED_LIBRARY_LINK_${lang}_FLAGS "") +endmacro() + diff --git a/src/lib/dspal b/src/lib/dspal index caf445fe1e..c8e885aac5 160000 --- a/src/lib/dspal +++ b/src/lib/dspal @@ -1 +1 @@ -Subproject commit caf445fe1e54a9631c6d4c6451f23452adb9897c +Subproject commit c8e885aac51aa34855bb3880d3dc916b9e278083 From d030608def7c74147e895bf038747d315317dbc4 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 23 Oct 2015 21:09:29 -0700 Subject: [PATCH 114/155] Modified Vagrantfile to use NFS folder sharing Virtualbox has issues with certain operations on shared folders using the default sharing mechanism. These are overcome using NFS sharing. On Ubuntu, sudo apt-get install nfs-kernel-server Signed-off-by: Mark Charlebois --- Vagrantfile | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Vagrantfile b/Vagrantfile index 2fbf532b74..cea1289500 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -33,11 +33,14 @@ Vagrant.configure(2) do |config| # your network. # config.vm.network "public_network" + # Virtualbox requires a private network to use NFS + config.vm.network "private_network", type: "dhcp" + # Share an additional folder to the guest VM. The first argument is # the path on the host to the actual folder. The second argument is # the path on the guest to mount the folder. And the optional third # argument is a set of non-required options. - config.vm.synced_folder ".", "/Firmware" + config.vm.synced_folder ".", "/Firmware", type: "nfs" # Provider-specific configuration so you can fine-tune various # backing providers for Vagrant. These expose provider-specific options. From 4cf0af1b3b213be88dd2ac03f05bae4c71be1466 Mon Sep 17 00:00:00 2001 From: carmix Date: Sat, 24 Oct 2015 09:22:20 +0200 Subject: [PATCH 115/155] Adapted to cmake --- .../srf02/{module.mk => CMakeLists.txt} | 23 ++++++++++--------- 1 file changed, 12 insertions(+), 11 deletions(-) rename src/drivers/srf02/{module.mk => CMakeLists.txt} (87%) diff --git a/src/drivers/srf02/module.mk b/src/drivers/srf02/CMakeLists.txt similarity index 87% rename from src/drivers/srf02/module.mk rename to src/drivers/srf02/CMakeLists.txt index a7bbf0225f..da5b878696 100644 --- a/src/drivers/srf02/module.mk +++ b/src/drivers/srf02/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -30,13 +30,14 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -# -# Makefile to build the Maxbotix Sonar driver. -# - -MODULE_COMMAND = srf02 - -SRCS = srf02.cpp - -MAXOPTIMIZATION = -Os +px4_add_module( + MODULE drivers__srf02 + MAIN srf02 + COMPILE_FLAGS + -Os + SRCS + srf02.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : From 974872733132196795a6b9d7a9c4eb7266d477f2 Mon Sep 17 00:00:00 2001 From: gcarmix Date: Sat, 24 Oct 2015 10:35:37 +0200 Subject: [PATCH 116/155] Updated driver to the be compliant with the new mb12xx driver version, inserted a line into the cmake config file for px4fmu-v2 --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 1 + src/drivers/srf02/srf02.cpp | 134 ++++++++------------ 2 files changed, 53 insertions(+), 82 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index a242f4f736..f3638c5749 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -22,6 +22,7 @@ set(config_module_list drivers/hmc5883 drivers/ms5611 drivers/mb12xx + drivers/srf02 drivers/sf0x drivers/ll40ls drivers/trone diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index 8bd874d2b6..061192537f 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -34,11 +34,10 @@ /** * @file srf02.cpp * - * Driver for the SRF02 sonar range finders connected via I2C, - * adapted from the Maxbotix (mb12xx) driver. + * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (mb12xx). */ -#include +#include #include @@ -69,18 +68,19 @@ #include #include +#include #include /* Configuration Constants */ -#define SRF02_MAX_RANGEFINDERS 4 #define SRF02_BUS PX4_I2C_BUS_EXPANSION #define SRF02_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ #define SRF02_DEVICE_PATH "/dev/srf02" -/* SRF02 Registers addresses */ +/* MB12xx Registers addresses */ #define SRF02_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define SRF02_SET_ADDRESS_0 0xA0 /* Change address 0 Register */ #define SRF02_SET_ADDRESS_1 0xAA /* Change address 1 Register */ #define SRF02_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ @@ -125,13 +125,14 @@ private: float _min_distance; float _max_distance; work_s _work; - RingBuffer *_reports; + ringbuffer::RingBuffer *_reports; bool _sensor_ok; int _measure_ticks; bool _collect_phase; - int _class_instance; + int _class_instance; + int _orb_class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -201,7 +202,7 @@ private: extern "C" __EXPORT int srf02_main(int argc, char *argv[]); SRF02::SRF02(int bus, int address) : - I2C("SRF02", SRF02_DEVICE_PATH, bus, address, 100000), + I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000), _min_distance(SRF02_MIN_DISTANCE), _max_distance(SRF02_MAX_DISTANCE), _reports(nullptr), @@ -209,7 +210,8 @@ SRF02::SRF02(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _orb_class_instance(-1), + _distance_sensor_topic(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "srf02_read")), _comms_errors(perf_alloc(PC_COUNT, "srf02_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "srf02_buffer_overflows")), @@ -219,7 +221,7 @@ SRF02::SRF02(int bus, int address) : { /* enable debug() calls */ - _debug_enabled = true; + _debug_enabled = false; /* work_cancel in the dtor will explode if we don't do this... */ memset(&_work, 0, sizeof(_work)); @@ -256,7 +258,7 @@ SRF02::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = SRF02_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ @@ -269,12 +271,13 @@ SRF02::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report = {}; + struct distance_sensor_s ds_report = {}; - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_range_finder_topic < 0) { - log("failed to create sensor_range_finder object. Did you start uOrb?"); + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } @@ -284,14 +287,14 @@ SRF02::init() /* check for connected rangefinders on each i2c port: We start from i2c base address (0x70 = 112) and count downwards So second iteration it uses i2c address 111, third iteration 110 and so on*/ - for (unsigned counter = 0; counter <= SRF02_MAX_RANGEFINDERS; counter++) { + for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) { _index_counter = SRF02_BASEADDR - counter; /* set temp sonar i2c address to base adress - counter */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ int ret2 = measure(); if (ret2 == 0) { /* sonar is present -> store address_index in array */ addr_ind.push_back(_index_counter); - debug("sonar added"); + DEVICE_DEBUG("sonar added"); _latest_sonar_measurements.push_back(200); } } @@ -309,10 +312,10 @@ SRF02::init() /* show the connected sonars in terminal */ for (unsigned i = 0; i < addr_ind.size(); i++) { - log("sonar %d with address %d added", (i + 1), addr_ind[i]); + DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]); } - debug("Number of sonars connected: %d", addr_ind.size()); + DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size()); ret = OK; /* sensor is ok, but we don't really know if it is within range */ @@ -470,8 +473,8 @@ ssize_t SRF02::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -544,7 +547,7 @@ SRF02::measure() if (OK != ret) { perf_count(_comms_errors); - debug("i2c::transfer returned %d", ret); + DEVICE_DEBUG("i2c::transfer returned %d", ret); return ret; } @@ -562,64 +565,34 @@ SRF02::collect() uint8_t val[2] = {0, 0}; uint8_t cmd = 0x02; perf_begin(_sample_perf); + ret = transfer(&cmd, 1, nullptr, 0); ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - debug("error reading from sensor: %d", ret); + DEVICE_DEBUG("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; } - uint16_t distance = val[0] << 8 | val[1]; - float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ - struct range_finder_report report; + uint16_t distance_cm = val[0] << 8 | val[1]; + float distance_m = float(distance_cm) * 1e-2f; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + struct distance_sensor_s report; report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - - /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ - if (addr_ind.size() == 1) { - report.distance = si_units; - report.distance_vector[0] = si_units; - - for (unsigned i = 1; i < (SRF02_MAX_RANGEFINDERS); i++) { - report.distance_vector[i] = 0; - } - - report.just_updated = 0; - - } else { - /* for multiple sonars connected */ - - /* don't use the original single sonar variable */ - report.distance = 0; - - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - report.distance_vector[i] = _latest_sonar_measurements[i]; - } - - /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ - report.just_updated = _index_counter; - - /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ - for (unsigned i = 0; i < (SRF02_MAX_RANGEFINDERS - addr_ind.size()); i++) { - report.distance_vector[addr_ind.size() + i] = 0; - } - } - - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + report.orientation = 8; + report.current_distance = distance_m; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0f; + /* TODO: set proper ID */ + report.id = 0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic != nullptr) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -653,9 +626,9 @@ SRF02::start() true, subsystem_info_s::SUBSYSTEM_TYPE_RANGEFINDER }; - static orb_advert_t pub = -1; + static orb_advert_t pub = nullptr; - if (pub > 0) { + if (pub != nullptr) { orb_publish(ORB_ID(subsystem_info), pub, &info); @@ -690,7 +663,7 @@ SRF02::cycle() /* perform collection */ if (OK != collect()) { - debug("collection error"); + DEVICE_DEBUG("collection error"); /* if error restart the measurement state machine */ start(); return; @@ -729,7 +702,7 @@ SRF02::cycle() /* Perform measurement */ if (OK != measure()) { - debug("measure error sonar adress %d", _index_counter); + DEVICE_DEBUG("measure error sonar adress %d", _index_counter); } /* next phase is collection */ @@ -844,7 +817,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -862,8 +835,8 @@ test() } warnx("single read"); - warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %llu", report.timestamp); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -891,13 +864,10 @@ test() } warnx("periodic read %u", i); - - /* Print the sonar rangefinder report sonar distance vector */ - for (uint8_t count = 0; count < SRF02_MAX_RANGEFINDERS; count++) { - warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); - } - - warnx("time: %lld", report.timestamp); + warnx("valid %u", (float)report.current_distance > report.min_distance + && (float)report.current_distance < report.max_distance ? 1 : 0); + warnx("measurement: %0.3f", (double)report.current_distance); + warnx("time: %llu", report.timestamp); } /* reset the sensor polling to default rate */ From aa82a367474a2efa2c0986770878bc07f8e28a91 Mon Sep 17 00:00:00 2001 From: gcarmix Date: Sat, 24 Oct 2015 10:50:57 +0200 Subject: [PATCH 117/155] Corrected code indent --- src/drivers/srf02/srf02.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index 061192537f..92fda5d41f 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -80,7 +80,7 @@ /* MB12xx Registers addresses */ #define SRF02_TAKE_RANGE_REG 0x51 /* Measure range Register */ -#define SRF02_SET_ADDRESS_0 0xA0 /* Change address 0 Register */ +#define SRF02_SET_ADDRESS_0 0xA0 /* Change address 0 Register */ #define SRF02_SET_ADDRESS_1 0xAA /* Change address 1 Register */ #define SRF02_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ From faa80a51e89ba726c2937464271517e6c2a35630 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 19:00:58 +0200 Subject: [PATCH 118/155] Strip unused header --- src/drivers/lsm303d/lsm303d.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 1252f57c09..8821c60c82 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -66,7 +66,6 @@ #include #include #include -#include #include #include From 0106be3e89d36454c100b763a00dd0d76e86d935 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 10:57:46 -0400 Subject: [PATCH 119/155] Added local position estimator. --- CMakeLists.txt | 13 + Makefile | 52 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 8 +- Tools/check_code_style.sh | 1 + Tools/posix.gdbinit | 2 +- Tools/sitl_run.sh | 36 +- cmake/configs/nuttx_px4fmu-v2_lpe.cmake | 5 + cmake/configs/posix_sitl_lpe.cmake | 9 + cmake/configs/posix_sitl_simple.cmake | 21 + posix-configs/SITL/init/rcS_lpe | 65 + src/firmware/nuttx/CMakeLists.txt | 17 +- src/firmware/posix/CMakeLists.txt | 7 + src/modules/controllib/blocks.cpp | 1 + .../BlockLocalPositionEstimator.cpp | 1379 +++++++++++++++++ .../BlockLocalPositionEstimator.hpp | 310 ++++ .../local_position_estimator/CMakeLists.txt | 57 + .../local_position_estimator_main.cpp | 169 ++ .../matrix/.gitignore | 1 + .../matrix/CMakeLists.txt | 13 + .../matrix/src/Matrix.hpp | 459 ++++++ .../matrix/test/CMakeLists.txt | 15 + .../matrix/test/inverse.cpp | 30 + .../matrix/test/matrixAssignment.cpp | 29 + .../matrix/test/matrixMult.cpp | 26 + .../matrix/test/matrixScalarMult.cpp | 18 + .../matrix/test/setIdentity.cpp | 25 + .../matrix/test/transpose.cpp | 18 + .../matrix/test/vectorAssignment.cpp | 28 + src/modules/local_position_estimator/params.c | 222 +++ src/modules/mavlink/mavlink_messages.cpp | 74 + .../mc_pos_control/mc_pos_control_main.cpp | 18 +- .../mc_pos_control/mc_pos_control_params.c | 8 + src/platforms/posix/px4_layer/drv_hrt.c | 8 +- 33 files changed, 3080 insertions(+), 64 deletions(-) create mode 100644 cmake/configs/nuttx_px4fmu-v2_lpe.cmake create mode 100644 cmake/configs/posix_sitl_lpe.cmake create mode 100644 posix-configs/SITL/init/rcS_lpe create mode 100644 src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp create mode 100644 src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp create mode 100644 src/modules/local_position_estimator/CMakeLists.txt create mode 100644 src/modules/local_position_estimator/local_position_estimator_main.cpp create mode 100644 src/modules/local_position_estimator/matrix/.gitignore create mode 100644 src/modules/local_position_estimator/matrix/CMakeLists.txt create mode 100644 src/modules/local_position_estimator/matrix/src/Matrix.hpp create mode 100644 src/modules/local_position_estimator/matrix/test/CMakeLists.txt create mode 100644 src/modules/local_position_estimator/matrix/test/inverse.cpp create mode 100644 src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp create mode 100644 src/modules/local_position_estimator/matrix/test/matrixMult.cpp create mode 100644 src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp create mode 100644 src/modules/local_position_estimator/matrix/test/setIdentity.cpp create mode 100644 src/modules/local_position_estimator/matrix/test/transpose.cpp create mode 100644 src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp create mode 100644 src/modules/local_position_estimator/params.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 732496778f..9486bf13fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -236,6 +236,19 @@ add_custom_target(submodule_clean COMMAND rm -rf .git/modules/* ) +#============================================================================= +# misc targets +# +add_custom_target(check_format + COMMAND Tools/check_code_style.sh + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) + +add_custom_target(config + COMMAND cmake-gui . + WORKING_DIRECTORY ${CMAKE_BINARY_DIR} + ) + #============================================================================= # external libraries # diff --git a/Makefile b/Makefile index c0413ea7ef..7cba819317 100644 --- a/Makefile +++ b/Makefile @@ -113,12 +113,18 @@ px4fmu-v2_default: px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) +px4fmu-v2_lpe: + $(call cmake-build,nuttx_px4fmu-v2_lpe) + nuttx_sim_simple: $(call cmake-build,$@) posix_sitl_simple: $(call cmake-build,$@) +posix_sitl_lpe: + $(call cmake-build,$@) + ros_sitl_simple: $(call cmake-build,$@) @@ -134,44 +140,13 @@ posix_sitl_default: posix_sitl_simple ros: ros_sitl_simple -run_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS none jmavsim +sitl_deprecation: + @echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead." + @echo "Change init script with 'make posix_sitl_default config'" -run_sitl_iris: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS_iris_gazebo none gazebo - -run_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing - -run_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros - -lldb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS lldb jmavsim - -lldb_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb - -lldb_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb - -gdb_sitl_quad: posix - Tools/sitl_run.sh posix-configs/SITL/init/rcS gdb jmavsim - -gdb_sitl_plane: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc.fixed_wing lldb - -gdb_sitl_ros: posix - Tools/sitl_run.sh posix-configs/SITL/init/rc_iris_ros lldb - -sitl_quad: - @echo "Deprecated. Use 'run_sitl_quad' instead." - -sitl_plane: - @echo "Deprecated. Use 'run_sitl_plane' instead." - -sitl_ros: - @echo "Deprecated. Use 'run_sitl_ros' instead." +sitl_quad: sitl_deprecation +sitl_plane: sitl_deprecation +sitl_ros: sitl_deprecation # Other targets # -------------------------------------------------------------------- @@ -184,7 +159,8 @@ clean: @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) # targets handled by cmake -cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan +cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ + run_sitl config $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 1dfe6c09bc..4c6cdbd7cd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -12,7 +12,13 @@ then attitude_estimator_q start position_estimator_inav start else - ekf_att_pos_estimator start + if param compare LPE_ENABLED 1 + then + attitude_estimator_q start + local_position_estimator start + else + ekf_att_pos_estimator start + fi fi if mc_att_control start diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index 06c6bea1cc..c3dd2a3f32 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -26,6 +26,7 @@ for fn in $(find src/examples \ src/modules/dataman \ src/modules/fixedwing_backside \ src/modules/segway \ + src/modules/local_position_estimator \ src/modules/unit_test \ src/modules/systemlib \ src/modules/controllib \ diff --git a/Tools/posix.gdbinit b/Tools/posix.gdbinit index 0de1123362..23fa44b4e0 100644 --- a/Tools/posix.gdbinit +++ b/Tools/posix.gdbinit @@ -1,2 +1,2 @@ -handle SIGCONT nostop +handle SIGCONT nostop noprint nopass run diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index d80fcc2c70..3f7dc1a883 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -1,10 +1,28 @@ #!/bin/bash -cp Tools/posix_lldbinit build_posix_sitl_simple/src/firmware/posix/.lldbinit -cp Tools/posix.gdbinit build_posix_sitl_simple/src/firmware/posix/.gdbinit + +rc_script=$1 +debugger=$2 +program=$3 +build_path=$4 + +echo SITL ARGS +echo rc_script: $rc_script +echo debugger: $debugger +echo program: $program +echo build_path: $buid_path + +if [ "$#" != 4 ] +then + echo usage: sitl_run.sh rc_script debugger program build_path + exit 1 +fi + +cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit +cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit SIM_PID=0 -if [ "$3" == "jmavsim" ] +if [ "$program" == "jmavsim" ] then cd Tools/jMAVSim ant @@ -22,19 +40,19 @@ then exit 1 fi fi -cd build_posix_sitl_simple/src/firmware/posix +cd $build_path/src/firmware/posix mkdir -p rootfs/fs/microsd mkdir -p rootfs/eeprom touch rootfs/eeprom/parameters # Start Java simulator -if [ "$2" == "lldb" ] +if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../$1 -elif [ "$2" == "gdb" ] + lldb -- mainapp ../../../../$rc_script +elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../$1 + gdb --args mainapp ../../../../$rc_script else - ./mainapp ../../../../$1 + ./mainapp ../../../../$rc_script fi if [ "$3" == "jmavsim" ] diff --git a/cmake/configs/nuttx_px4fmu-v2_lpe.cmake b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake new file mode 100644 index 0000000000..6e5cd10ba4 --- /dev/null +++ b/cmake/configs/nuttx_px4fmu-v2_lpe.cmake @@ -0,0 +1,5 @@ +include(cmake/configs/nuttx_px4fmu-v2_default.cmake) + +list(APPEND config_module_list + modules/local_position_estimator + ) diff --git a/cmake/configs/posix_sitl_lpe.cmake b/cmake/configs/posix_sitl_lpe.cmake new file mode 100644 index 0000000000..0598044844 --- /dev/null +++ b/cmake/configs/posix_sitl_lpe.cmake @@ -0,0 +1,9 @@ +include(cmake/configs/posix_sitl_simple.cmake) + +list(APPEND config_module_list + modules/local_position_estimator + ) + +set(config_sitl_rcS + posix-configs/SITL/init/rcS_lpe + ) diff --git a/cmake/configs/posix_sitl_simple.cmake b/cmake/configs/posix_sitl_simple.cmake index 227bffeab8..b0a8158070 100644 --- a/cmake/configs/posix_sitl_simple.cmake +++ b/cmake/configs/posix_sitl_simple.cmake @@ -62,6 +62,27 @@ set(config_extra_builtin_cmds sercon ) +set(config_sitl_rcS + posix-configs/SITL/init/rcS + CACHE FILEPATH "init script for sitl" + ) + +set(config_sitl_viewer + jmavsim + CACHE STRING "viewer for sitl" + ) +set_property(CACHE config_sitl_viewer + PROPERTY STRINGS "jmavsim;none") + +set(config_sitl_debugger + disable + CACHE STRING "debugger for sitl" + ) +set_property(CACHE config_sitl_debugger + PROPERTY STRINGS "disable;gdb;lldb") + + + add_custom_target(sercon) set_target_properties(sercon PROPERTIES MAIN "sercon" STACK "2048") diff --git a/posix-configs/SITL/init/rcS_lpe b/posix-configs/SITL/init/rcS_lpe new file mode 100644 index 0000000000..9b416a0b26 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe @@ -0,0 +1,65 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set SENS_BOARD_ROT 0 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a + +# start LPE at end, when we know it is ok to init sensors +sleep 5 +local_position_estimator start diff --git a/src/firmware/nuttx/CMakeLists.txt b/src/firmware/nuttx/CMakeLists.txt index 4632383bff..be6f9b1062 100644 --- a/src/firmware/nuttx/CMakeLists.txt +++ b/src/firmware/nuttx/CMakeLists.txt @@ -5,13 +5,6 @@ px4_nuttx_generate_builtin_commands( ${config_extra_builtin_cmds} ) -px4_nuttx_add_romfs(OUT romfs - ROOT ROMFS/px4fmu_common - EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin - ) - -add_dependencies(romfs fw_io) - # add executable add_executable(firmware_nuttx builtin_commands.c) @@ -33,8 +26,6 @@ if(NOT ${BOARD} STREQUAL "sim") set_target_properties(firmware_nuttx PROPERTIES LINK_FLAGS ${main_link_flags}) endif() -set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${target_name}.px4) - target_link_libraries(firmware_nuttx -Wl,--start-group ${module_libraries} @@ -49,6 +40,14 @@ add_custom_target(check_weak ) if(NOT ${BOARD} STREQUAL "sim") + + + px4_nuttx_add_romfs(OUT romfs + ROOT ROMFS/px4fmu_common + EXTRAS ${CMAKE_BINARY_DIR}/src/modules/px4iofirmware/${config_io_board}.bin + ) + add_dependencies(romfs fw_io) + set(fw_file ${CMAKE_CURRENT_BINARY_DIR}/${OS}-${BOARD}-${LABEL}.px4) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 7cef387962..e4c2855c66 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -24,4 +24,11 @@ else() ) endif() +add_custom_target(run_sitl + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" + "${config_sitl_viewer}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + ) +add_dependencies(run_sitl mainapp) + # vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 1aa60e63c8..04b96753ef 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -539,6 +539,7 @@ int blockRandGaussTest() } float stdDev = sqrt(sum / (n - 1)); + (void)(stdDev); ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); printf("PASS\n"); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp new file mode 100644 index 0000000000..08cf51fb5c --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -0,0 +1,1379 @@ +#include "BlockLocalPositionEstimator.hpp" +#include +#include +#include + +static const int MIN_FLOW_QUALITY = 100; +static const int REQ_INIT_COUNT = 100; + +static const uint32_t VISION_POSITION_TIMEOUT = 500000; +static const uint32_t MOCAP_TIMEOUT = 200000; + +static const uint32_t XY_SRC_TIMEOUT = 2000000; + +using namespace std; + +BlockLocalPositionEstimator::BlockLocalPositionEstimator() : + // this block has no parent, and has name LPE + SuperBlock(NULL, "LPE"), + + // subscriptions, set rate, add to list + // TODO topic speed limiting? + _sub_status(ORB_ID(vehicle_status), 0, 0, &getSubscriptions()), + _sub_armed(ORB_ID(actuator_armed), 0, 0, &getSubscriptions()), + _sub_control_mode(ORB_ID(vehicle_control_mode), + 0, 0, &getSubscriptions()), + _sub_att(ORB_ID(vehicle_attitude), 0, 0, &getSubscriptions()), + _sub_att_sp(ORB_ID(vehicle_attitude_setpoint), + 0, 0, &getSubscriptions()), + _sub_flow(ORB_ID(optical_flow), 0, 0, &getSubscriptions()), + _sub_sensor(ORB_ID(sensor_combined), 0, 0, &getSubscriptions()), + _sub_distance(ORB_ID(distance_sensor), + 0, 0, &getSubscriptions()), + _sub_param_update(ORB_ID(parameter_update), 0, 0, &getSubscriptions()), + _sub_manual(ORB_ID(manual_control_setpoint), 0, 0, &getSubscriptions()), + _sub_home(ORB_ID(home_position), 0, 0, &getSubscriptions()), + _sub_gps(ORB_ID(vehicle_gps_position), 0, 0, &getSubscriptions()), + _sub_vision_pos(ORB_ID(vision_position_estimate), 0, 0, &getSubscriptions()), + _sub_mocap(ORB_ID(att_pos_mocap), 0, 0, &getSubscriptions()), + + // publications + _pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()), + _pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()), + _pub_filtered_flow(ORB_ID(filtered_bottom_flow), -1, &getPublications()), + _pub_est_status(ORB_ID(estimator_status), -1, &getPublications()), + + // map projection + _map_ref(), + + // block parameters + _integrate(this, "INTEGRATE"), + _flow_xy_stddev(this, "FLW_XY"), + _sonar_z_stddev(this, "SNR_Z"), + _lidar_z_stddev(this, "LDR_Z"), + _accel_xy_stddev(this, "ACC_XY"), + _accel_z_stddev(this, "ACC_Z"), + _baro_stddev(this, "BAR_Z"), + _gps_xy_stddev(this, "GPS_XY"), + _gps_z_stddev(this, "GPS_Z"), + _gps_vxy_stddev(this, "GPS_VXY"), + _gps_vz_stddev(this, "GPS_VZ"), + _gps_eph_max(this, "EPH_MAX"), + _vision_xy_stddev(this, "VIS_XY"), + _vision_z_stddev(this, "VIS_Z"), + _no_vision(this, "NO_VISION"), + _beta_max(this, "BETA_MAX"), + _mocap_p_stddev(this, "VIC_P"), + _pn_p_noise_power(this, "PN_P"), + _pn_v_noise_power(this, "PN_V"), + _pn_b_noise_power(this, "PN_B"), + + // misc + _polls(), + _timeStamp(hrt_absolute_time()), + _time_last_xy(0), + _time_last_flow(0), + _time_last_baro(0), + _time_last_gps(0), + _time_last_lidar(0), + _time_last_sonar(0), + _time_last_vision_p(0), + _time_last_mocap(0), + + // mavlink log + _mavlink_fd(open(MAVLINK_LOG_DEVICE, 0)), + + // initialization flags + _baroInitialized(false), + _gpsInitialized(false), + _lidarInitialized(false), + _sonarInitialized(false), + _flowInitialized(false), + _visionInitialized(false), + _mocapInitialized(false), + + // init counts + _baroInitCount(0), + _gpsInitCount(0), + _lidarInitCount(0), + _sonarInitCount(0), + _flowInitCount(0), + _visionInitCount(0), + _mocapInitCount(0), + + // reference altitudes + _altHome(0), + _altHomeInitialized(false), + _baroAltHome(0), + _gpsAltHome(0), + _lidarAltHome(0), + _sonarAltHome(0), + _visionHome(), + _mocapHome(), + + // flow integration + _flowX(0), + _flowY(0), + _flowMeanQual(0), + + // reference lat/lon + _gpsLatHome(0), + _gpsLonHome(0), + + // status + _canEstimateXY(false), + _canEstimateZ(false), + _xyTimeout(false), + + // faults + _baroFault(FAULT_NONE), + _gpsFault(FAULT_NONE), + _lidarFault(FAULT_NONE), + _flowFault(FAULT_NONE), + _sonarFault(FAULT_NONE), + _visionFault(FAULT_NONE), + _mocapFault(FAULT_NONE), + + //timeouts + _visionTimeout(true), + _mocapTimeout(true), + + // loop performance + _loop_perf(), + _interval_perf(), + _err_perf(), + + // kf matrices + _x(), _u(), _P() +{ + // setup event triggering based on new flow messages to integrate + _polls[POLL_FLOW].fd = _sub_flow.getHandle(); + _polls[POLL_FLOW].events = POLLIN; + + _polls[POLL_PARAM].fd = _sub_param_update.getHandle(); + _polls[POLL_PARAM].events = POLLIN; + + _polls[POLL_SENSORS].fd = _sub_sensor.getHandle(); + _polls[POLL_SENSORS].events = POLLIN; + + // initialize P to identity*0.1 + initP(); + + _x.setZero(); + _u.setZero(); + + // perf counters + _loop_perf = perf_alloc(PC_ELAPSED, + "local_position_estimator_runtime"); + //_interval_perf = perf_alloc(PC_INTERVAL, + //"local_position_estimator_interval"); + _err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err"); + + // map + _map_ref.init_done = false; + + // intialize parameter dependent matrices + updateParams(); +} + +BlockLocalPositionEstimator::~BlockLocalPositionEstimator() +{ +} + +void BlockLocalPositionEstimator::update() +{ + + // wait for a sensor update, check for exit condition every 100 ms + int ret = poll(_polls, 3, 100); + + if (ret < 0) { + /* poll error, count it in perf */ + perf_count(_err_perf); + return; + } + + uint64_t newTimeStamp = hrt_absolute_time(); + float dt = (newTimeStamp - _timeStamp) / 1.0e6f; + _timeStamp = newTimeStamp; + + //printf("dt: %0.5g\n", double(dt)); + + // set dt for all child blocks + setDt(dt); + + // see which updates are available + bool flowUpdated = _sub_flow.updated(); + bool paramsUpdated = _sub_param_update.updated(); + bool baroUpdated = _sub_sensor.updated(); + bool lidarUpdated = false; + bool sonarUpdated = false; + + if (_sub_distance.updated()) { + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + lidarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + sonarUpdated = true; + } + + if (_sub_distance.get().type == distance_sensor_s::MAV_DISTANCE_SENSOR_INFRARED) { + mavlink_log_info(_mavlink_fd, "[lpe] no support to short-range infrared sensors "); + warnx("[lpe] short-range infrared detected. Ignored... "); + } + } + + bool gpsUpdated = _sub_gps.updated(); + bool homeUpdated = _sub_home.updated(); + bool visionUpdated = _sub_vision_pos.updated(); + bool mocapUpdated = _sub_mocap.updated(); + + // get new data + updateSubscriptions(); + + // update parameters + if (paramsUpdated) { + updateParams(); + } + + // update home position projection + if (homeUpdated) { + updateHome(); + } + + // check for timeouts on external sources + if ((hrt_absolute_time() - _time_last_vision_p > VISION_POSITION_TIMEOUT) && _visionInitialized) { + if (!_visionTimeout) { + _visionTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] vision position timeout "); + warnx("[lpe] vision position timeout "); + } + + } else { + _visionTimeout = false; + } + + if ((hrt_absolute_time() - _time_last_mocap > MOCAP_TIMEOUT) && _mocapInitialized) { + if (!_mocapTimeout) { + _mocapTimeout = true; + mavlink_log_info(_mavlink_fd, "[lpe] mocap timeout "); + warnx("[lpe] mocap timeout "); + } + + } else { + _mocapTimeout = false; + } + + // determine if we should start estimating + _canEstimateZ = _baroInitialized && !_baroFault; + _canEstimateXY = + (_gpsInitialized && !_gpsFault) || + (_flowInitialized && !_flowFault) || + (_visionInitialized && !_visionTimeout && !_visionFault) || + (_mocapInitialized && !_mocapTimeout && !_mocapFault); + + if (_canEstimateXY) { + _time_last_xy = hrt_absolute_time(); + } + + // if we have no lat, lon initialized projection at 0,0 + if (_canEstimateXY && !_map_ref.init_done) { + map_projection_init(&_map_ref, 0, 0); + } + + // reinitialize x if necessary + bool reinit_x = false; + + for (int i = 0; i < n_x; i++) { + // should we do a reinit + // of sensors here? + // don't want it to take too long + if (!isfinite(_x(i))) { + reinit_x = true; + break; + } + } + + if (reinit_x) { + for (int i = 0; i < n_x; i++) { + _x(i) = 0; + } + + mavlink_log_info(_mavlink_fd, "[lpe] reinit x"); + warnx("[lpe] reinit x"); + } + + // reinitialize P if necessary + bool reinit_P = false; + + for (int i = 0; i < n_x; i++) { + for (int j = 0; j < n_x; j++) { + if (!isfinite(_P(i, j))) { + reinit_P = true; + break; + } + } + + if (reinit_P) { break; } + } + + if (reinit_P) { + mavlink_log_info(_mavlink_fd, "[lpe] reinit P"); + warnx("[lpe] reinit P"); + initP(); + } + + // do prediction + predict(); + + // sensor corrections/ initializations + if (gpsUpdated) { + if (!_gpsInitialized) { + initGps(); + + } else { + correctGps(); + } + } + + if (baroUpdated) { + if (!_baroInitialized) { + initBaro(); + + } else { + correctBaro(); + } + } + + if (lidarUpdated) { + if (!_lidarInitialized) { + initLidar(); + + } else { + correctLidar(); + } + } + + if (sonarUpdated) { + if (!_sonarInitialized) { + initSonar(); + + } else { + correctSonar(); + } + } + + if (flowUpdated) { + if (!_flowInitialized) { + initFlow(); + + } else { + perf_begin(_loop_perf);// TODO + correctFlow(); + //perf_count(_interval_perf); + perf_end(_loop_perf); + } + } + + if (_no_vision.get() != CBRK_NO_VISION_KEY) { // check if no vision circuit breaker is set + if (visionUpdated) { + if (!_visionInitialized) { + initVision(); + + } else { + correctVision(); + } + } + } + + if (mocapUpdated) { + if (!_mocapInitialized) { + initmocap(); + + } else { + correctmocap(); + } + } + + _xyTimeout = (hrt_absolute_time() - _time_last_xy > XY_SRC_TIMEOUT); + + if (!_xyTimeout && _altHomeInitialized) { + // update all publications if possible + publishLocalPos(); + publishEstimatorStatus(); + publishGlobalPos(); + publishFilteredFlow(); + + } else if (_altHomeInitialized) { + // publish only Z estimate + publishLocalPos(); + publishEstimatorStatus(); + } + +} + +void BlockLocalPositionEstimator::updateHome() +{ + double lat = _sub_home.get().lat; + double lon = _sub_home.get().lon; + float alt = _sub_home.get().alt; + + mavlink_log_info(_mavlink_fd, "[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + warnx("[lpe] home: lat %5.0f, lon %5.0f, alt %5.0f", lat, lon, double(alt)); + map_projection_init(&_map_ref, lat, lon); + float delta_alt = alt - _altHome; + _altHomeInitialized = true; + _altHome = alt; + _gpsAltHome += delta_alt; + _baroAltHome += delta_alt; + _lidarAltHome += delta_alt; + _sonarAltHome += delta_alt; +} + +void BlockLocalPositionEstimator::initBaro() +{ + // collect baro data + if (!_baroInitialized && + (_sub_sensor.get().baro_timestamp[0] != _time_last_baro)) { + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; + _baroAltHome += _sub_sensor.get().baro_alt_meter[0]; + + if (_baroInitCount++ > REQ_INIT_COUNT) { + _baroAltHome /= _baroInitCount; + mavlink_log_info(_mavlink_fd, + "[lpe] baro offs: %d m", (int)_baroAltHome); + warnx("[lpe] baro offs: %d m", (int)_baroAltHome); + _baroInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _baroAltHome; + } + } + } +} + + +void BlockLocalPositionEstimator::initGps() +{ + // collect gps data + if (!_gpsInitialized && _sub_gps.get().fix_type > 2) { + double lat = _sub_gps.get().lat * 1e-7; + double lon = _sub_gps.get().lon * 1e-7; + float alt = _sub_gps.get().alt * 1e-3f; + // increament sums for mean + _gpsLatHome += lat; + _gpsLonHome += lon; + _gpsAltHome += alt; + _time_last_gps = _sub_gps.get().timestamp_position; + + if (_gpsInitCount++ > REQ_INIT_COUNT) { + _gpsLatHome /= _gpsInitCount; + _gpsLonHome /= _gpsInitCount; + _gpsAltHome /= _gpsInitCount; + map_projection_init(&_map_ref, lat, lon); + mavlink_log_info(_mavlink_fd, "[lpe] gps init: " + "lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + warnx("[lpe] gps init: lat %d, lon %d, alt %d m", + int(_gpsLatHome), int(_gpsLonHome), int(_gpsAltHome)); + _gpsInitialized = true; + + if (!_altHomeInitialized) { + _altHomeInitialized = true; + _altHome = _gpsAltHome; + } + } + } +} + +void BlockLocalPositionEstimator::initLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { return; } + + // collect lidar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_lidarInitialized && valid) { + // increament sums for mean + _lidarAltHome += _sub_distance.get().current_distance; + + if (_lidarInitCount++ > REQ_INIT_COUNT) { + _lidarAltHome /= _lidarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] lidar init: " + "alt %d cm", + int(100 * _lidarAltHome)); + warnx("[lpe] lidar init: alt %d cm", + int(100 * _lidarAltHome)); + _lidarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { return; } + + // collect sonar data + bool valid = false; + float d = _sub_distance.get().current_distance; + + if (d < _sub_distance.get().max_distance && + d > _sub_distance.get().min_distance) { + valid = true; + } + + if (!_sonarInitialized && valid) { + // increament sums for mean + _sonarAltHome += _sub_distance.get().current_distance; + + if (_sonarInitCount++ > REQ_INIT_COUNT) { + _sonarAltHome /= _sonarInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] sonar init: " + "alt %d cm", + int(100 * _sonarAltHome)); + warnx("[lpe] sonar init: alt %d cm", + int(100 * _sonarAltHome)); + _sonarInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initFlow() +{ + + // collect pixel flow data + if (!_flowInitialized) { + // increament sums for mean + _flowMeanQual += _sub_flow.get().quality; + + if (_flowInitCount++ > REQ_INIT_COUNT) { + _flowMeanQual /= _flowInitCount; + + if (_flowMeanQual < MIN_FLOW_QUALITY) { + // retry initialisation till we have better flow data + warnx("[lpe] flow quality bad, retrying init : %d", + int(_flowMeanQual)); + _flowMeanQual = 0; + _flowInitCount = 0; + return; + } + + mavlink_log_info(_mavlink_fd, "[lpe] flow init: " + "quality %d", + int(_flowMeanQual)); + warnx("[lpe] flow init: quality %d", + int(_flowMeanQual)); + _flowInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initVision() +{ + // collect vision position data + if (!_visionInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_vision_pos.get().x; + pos(1) = _sub_vision_pos.get().y; + pos(2) = _sub_vision_pos.get().z; + _visionHome += pos; + + if (_visionInitCount++ > REQ_INIT_COUNT) { + _visionHome /= _visionInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] vision position init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _visionInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::initmocap() +{ + // collect mocap data + if (!_mocapInitialized) { + // increament sums for mean + Vector3f pos; + pos(0) = _sub_mocap.get().x; + pos(1) = _sub_mocap.get().y; + pos(2) = _sub_mocap.get().z; + _mocapHome += pos; + + if (_mocapInitCount++ > REQ_INIT_COUNT) { + _mocapHome /= _mocapInitCount; + mavlink_log_info(_mavlink_fd, "[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + warnx("[lpe] mocap init: " + "%f, %f, %f m", double(pos(0)), double(pos(1)), double(pos(2))); + _mocapInitialized = true; + } + } +} + +void BlockLocalPositionEstimator::publishLocalPos() +{ + // publish local position + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && isfinite(_x(X_vz))) { + _pub_lpos.get().timestamp = _timeStamp; + _pub_lpos.get().xy_valid = _canEstimateXY; + _pub_lpos.get().z_valid = _canEstimateZ; + _pub_lpos.get().v_xy_valid = _canEstimateXY; + _pub_lpos.get().v_z_valid = _canEstimateZ; + _pub_lpos.get().x = _x(X_x); // north + _pub_lpos.get().y = _x(X_y); // east + _pub_lpos.get().z = _x(X_z); // down + _pub_lpos.get().vx = _x(X_vx); // north + _pub_lpos.get().vy = _x(X_vy); // east + _pub_lpos.get().vz = _x(X_vz); // down + _pub_lpos.get().yaw = _sub_att.get().yaw; + _pub_lpos.get().xy_global = _sub_home.get().timestamp != 0; // need home for reference + _pub_lpos.get().z_global = _baroInitialized; + _pub_lpos.get().ref_timestamp = _sub_home.get().timestamp; + _pub_lpos.get().ref_lat = _map_ref.lat_rad * 180 / M_PI; + _pub_lpos.get().ref_lon = _map_ref.lon_rad * 180 / M_PI; + _pub_lpos.get().ref_alt = _sub_home.get().alt; + // TODO, terrain alt + _pub_lpos.get().dist_bottom = -_x(X_z); + _pub_lpos.get().dist_bottom_rate = -_x(X_vz); + _pub_lpos.get().surface_bottom_timestamp = 0; + _pub_lpos.get().dist_bottom_valid = true; + _pub_lpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + _pub_lpos.get().epv = sqrtf(_P(X_z, X_z)); + _pub_lpos.update(); + } +} + +void BlockLocalPositionEstimator::publishEstimatorStatus() +{ + if (isfinite(_x(X_x)) && isfinite(_x(X_y)) && isfinite(_x(X_z)) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) + && isfinite(_x(X_vz))) { + _pub_est_status.get().timestamp = _timeStamp; + + for (int i = 0; i < n_x; i++) { + _pub_est_status.get().states[i] = _x(i); + _pub_est_status.get().covariances[i] = _P(i, i); + } + + _pub_est_status.get().n_states = n_x; + _pub_est_status.get().nan_flags = 0; + _pub_est_status.get().health_flags = + ((_baroFault > 0) << SENSOR_BARO) + + ((_gpsFault > 0) << SENSOR_GPS) + + ((_lidarFault > 0) << SENSOR_LIDAR) + + ((_flowFault > 0) << SENSOR_FLOW) + + ((_sonarFault > 0) << SENSOR_SONAR) + + ((_visionFault > 0) << SENSOR_VISION) + + ((_mocapFault > 0) << SENSOR_MOCAP); + _pub_est_status.get().timeout_flags = + (_xyTimeout << 0) + + (_visionTimeout << 1) + + (_mocapTimeout << 2); + _pub_est_status.update(); + } +} + +void BlockLocalPositionEstimator::publishGlobalPos() +{ + // publish global position + double lat = 0; + double lon = 0; + map_projection_reproject(&_map_ref, _x(X_x), _x(X_y), &lat, &lon); + float alt = -_x(X_z) + _altHome; + + if (isfinite(lat) && isfinite(lon) && isfinite(alt) && + isfinite(_x(X_vx)) && isfinite(_x(X_vy)) && + isfinite(_x(X_vz))) { + _pub_gpos.get().timestamp = _timeStamp; + _pub_gpos.get().time_utc_usec = _sub_gps.get().time_utc_usec; + _pub_gpos.get().lat = lat; + _pub_gpos.get().lon = lon; + _pub_gpos.get().alt = alt; + _pub_gpos.get().vel_n = _x(X_vx); + _pub_gpos.get().vel_e = _x(X_vy); + _pub_gpos.get().vel_d = _x(X_vz); + _pub_gpos.get().yaw = _sub_att.get().yaw; + _pub_gpos.get().eph = sqrtf(_P(X_x, X_x) + _P(X_y, X_y)); + _pub_gpos.get().epv = sqrtf(_P(X_z, X_z)); + _pub_gpos.get().terrain_alt = 0; + _pub_gpos.get().terrain_alt_valid = false; + _pub_gpos.get().dead_reckoning = !_canEstimateXY && !_xyTimeout; + _pub_gpos.update(); + } +} + +void BlockLocalPositionEstimator::publishFilteredFlow() +{ + // publish filtered flow + if (isfinite(_pub_filtered_flow.get().sumx) && + isfinite(_pub_filtered_flow.get().sumy) && + isfinite(_pub_filtered_flow.get().vx) && + isfinite(_pub_filtered_flow.get().vy)) { + _pub_filtered_flow.update(); + } +} + +void BlockLocalPositionEstimator::initP() +{ + _P.setZero(); + _P(X_x, X_x) = 1; + _P(X_y, X_y) = 1; + _P(X_z, X_z) = 1; + _P(X_vx, X_vx) = 1; + _P(X_vy, X_vy) = 1; + _P(X_vz, X_vz) = 1; + _P(X_bx, X_bx) = 1e-6; + _P(X_by, X_by) = 1e-6; + _P(X_bz, X_bz) = 1e-6; +} + +void BlockLocalPositionEstimator::predict() +{ + // if can't update anything, don't propagate + // state or covariance + if (!_canEstimateXY && !_canEstimateZ) { return; } + + if (_integrate.get() && _sub_att.get().R_valid) { + Matrix3f R_att(_sub_att.get().R); + Vector3f a(_sub_sensor.get().accelerometer_m_s2); + Vector3f b(_x(X_bx), _x(X_by), _x(X_bz)); + _u = R_att * (a - b); + _u(U_az) += 9.81f; // add g + + } else { + _u = Vector3f(0, 0, 0); + } + + // dynamics matrix + Matrix A; // state dynamics matrix + A.setZero(); + // derivative of position is velocity + A(X_x, X_vx) = 1; + A(X_y, X_vy) = 1; + A(X_z, X_vz) = 1; + + // derivative of velocity is accelerometer acceleration + // (in input matrix) - bias (in body frame) + Matrix3f R_att(_sub_att.get().R); + A(X_vx, X_bx) = -R_att(0, 0); + A(X_vx, X_by) = -R_att(0, 1); + A(X_vx, X_bz) = -R_att(0, 2); + + A(X_vy, X_bx) = -R_att(1, 0); + A(X_vy, X_by) = -R_att(1, 1); + A(X_vy, X_bz) = -R_att(1, 2); + + A(X_vz, X_bx) = -R_att(2, 0); + A(X_vz, X_by) = -R_att(2, 1); + A(X_vz, X_bz) = -R_att(2, 2); + + // input matrix + Matrix B; // input matrix + B.setZero(); + B(X_vx, U_ax) = 1; + B(X_vy, U_ay) = 1; + B(X_vz, U_az) = 1; + + // input noise covariance matrix + Matrix R; + R.setZero(); + R(U_ax, U_ax) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + R(U_ay, U_ay) = _accel_xy_stddev.get() * _accel_xy_stddev.get(); + R(U_az, U_az) = _accel_z_stddev.get() * _accel_z_stddev.get(); + + // process noise power matrix + Matrix Q; + Q.setZero(); + Q(X_x, X_x) = _pn_p_noise_power.get(); + Q(X_y, X_y) = _pn_p_noise_power.get(); + Q(X_z, X_z) = _pn_p_noise_power.get(); + Q(X_vx, X_vx) = _pn_v_noise_power.get(); + Q(X_vy, X_vy) = _pn_v_noise_power.get(); + Q(X_vz, X_vz) = _pn_v_noise_power.get(); + + // technically, the noise is in the body frame, + // but the components are all the same, so + // ignoring for now + Q(X_bx, X_bx) = _pn_b_noise_power.get(); + Q(X_by, X_by) = _pn_b_noise_power.get(); + Q(X_bz, X_bz) = _pn_b_noise_power.get(); + + // continuous time kalman filter prediction + Matrix dx = (A * _x + B * _u) * getDt(); + + // only predict for components we have + // valid measurements for + if (!_canEstimateXY) { + dx(X_x) = 0; + dx(X_y) = 0; + dx(X_vx) = 0; + dx(X_vy) = 0; + } + + if (!_canEstimateZ) { + dx(X_z) = 0; + dx(X_vz) = 0; + } + + // propagate + _x += dx; + _P += (A * _P + _P * A.transpose() + + B * R * B.transpose() + Q) * getDt(); +} + +void BlockLocalPositionEstimator::correctFlow() +{ + + // flow measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_flow_x, X_x) = 1; + C(Y_flow_y, X_y) = 1; + + Matrix R; + R.setZero(); + R(Y_flow_x, Y_flow_x) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + R(Y_flow_y, Y_flow_y) = + _flow_xy_stddev.get() * _flow_xy_stddev.get(); + + float flow_speed[3] = {0.0f, 0.0f, 0.0f}; + float global_speed[3] = {0.0f, 0.0f, 0.0f}; + + /* calc dt between flow timestamps */ + /* ignore first flow msg */ + if (_time_last_flow == 0) { + _time_last_flow = _sub_flow.get().timestamp; + return; + } + + float dt = (_sub_flow.get().timestamp - _time_last_flow) * 1.0e-6f ; + _time_last_flow = _sub_flow.get().timestamp; + + // calculate velocity over ground + if (_sub_flow.get().integration_timespan > 0) { + flow_speed[0] = (_sub_flow.get().pixel_flow_x_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().pitchspeed) * // Body rotation correction TODO check this + _x(X_z); + flow_speed[1] = (_sub_flow.get().pixel_flow_y_integral / + (_sub_flow.get().integration_timespan / 1e6f) - + _sub_att.get().rollspeed) * // Body rotation correction + _x(X_z); + + } else { + flow_speed[0] = 0; + flow_speed[1] = 0; + } + + flow_speed[2] = 0.0f; + + /* update filtered flow */ + _pub_filtered_flow.get().sumx += flow_speed[0] * dt; + _pub_filtered_flow.get().sumy += flow_speed[1] * dt; + _pub_filtered_flow.get().vx = flow_speed[0]; + _pub_filtered_flow.get().vy = flow_speed[1]; + + // TODO add yaw rotation correction (with distance to vehicle zero) + + // convert to globalframe velocity + for (uint8_t i = 0; i < 3; i++) { + float sum = 0.0f; + + for (uint8_t j = 0; j < 3; j++) { + sum += flow_speed[j] * PX4_R(_sub_att.get().R, i, j); + } + + global_speed[i] = sum; + } + + // flow integral + _flowX += global_speed[0] * dt; + _flowY += global_speed[1] * dt; + + // measurement + Vector2f y; + y(0) = _flowX; + y(1) = _flowY; + + // residual + Vector2f r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (_sub_flow.get().quality < MIN_FLOW_QUALITY) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] bad flow data "); + warnx("[lpe] bad flow data "); + _flowFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_flowFault) { + mavlink_log_info(_mavlink_fd, "[lpe] flow fault, beta %5.2f", double(beta)); + warnx("[lpe] flow fault, beta %5.2f", double(beta)); + _flowFault = FAULT_MINOR; + } + + } else if (_flowFault) { + _flowFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] flow OK"); + warnx("[lpe] flow OK"); + } + + // kalman filter correction if no fault + if (_flowFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + // reset flow integral to current estimate of position + // if a fault occurred + + } else { + _flowX = _x(X_x); + _flowY = _x(X_y); + } + +} + +void BlockLocalPositionEstimator::correctSonar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND) { + return; + } + + float d = _sub_distance.get().current_distance; + + // sonar measurement matrix and noise matrix + Matrix C; + C.setZero(); + C(Y_sonar_z, X_z) = -1; + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _sonar_z_stddev.get() * _sonar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + // measurement + Matrix y; + y(0) = (d - _sonarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Matrix r = y - C * _x; + + // residual covariance, (inverse) + Matrix S_I = + (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar out of range"); + warnx("[lpe] sonar out of range"); + _sonarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_sonarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] sonar fault, beta %5.2f", double(beta)); + warnx("[lpe] sonar fault, beta %5.2f", double(beta)); + _sonarFault = FAULT_MINOR; + } + + } else if (_sonarFault) { + _sonarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] sonar OK"); + warnx("[lpe] sonar OK"); + } + + // kalman filter correction if no fault + if (_sonarFault == FAULT_NONE) { + Matrix K = + _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_sonar = _sub_distance.get().timestamp; + +} + +void BlockLocalPositionEstimator::correctBaro() +{ + + Matrix y; + y(0) = _sub_sensor.get().baro_alt_meter[0] - _baroAltHome; + + // baro measurement matrix + Matrix C; + C.setZero(); + C(Y_baro_z, X_z) = -1; // measured altitude, negative down dir. + + Matrix R; + R.setZero(); + R(0, 0) = _baro_stddev.get() * _baro_stddev.get(); + + // residual + Matrix S_I = + ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - (C * _x); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_baroFault) { + mavlink_log_info(_mavlink_fd, "[lpe] baro fault, beta %5.2f", double(beta)); + warnx("[lpe] baro fault, beta %5.2f", double(beta)); + _baroFault = FAULT_MINOR; + } + + // lower baro trust + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_baroFault) { + _baroFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] baro OK"); + warnx("[lpe] baro OK"); + } + + // kalman filter correction if no fault + if (_baroFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_baro = _sub_sensor.get().baro_timestamp[0]; +} + +void BlockLocalPositionEstimator::correctLidar() +{ + + if (_sub_distance.get().type != distance_sensor_s::MAV_DISTANCE_SENSOR_LASER) { + return; + } + + float d = _sub_distance.get().current_distance; + + Matrix C; + C.setZero(); + C(Y_lidar_z, X_z) = -1; // measured altitude, + // negative down dir. + + // use parameter covariance unless sensor provides reasonable value + Matrix R; + R.setZero(); + float cov = _sub_distance.get().covariance; + + if (cov < 1.0e-3f) { + R(0, 0) = _lidar_z_stddev.get() * _lidar_z_stddev.get(); + + } else { + R(0, 0) = cov; + } + + Matrix y; + y.setZero(); + y(0) = (d - _lidarAltHome) * + cosf(_sub_att.get().roll) * + cosf(_sub_att.get().pitch); + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + // zero is an error code for the lidar + if (d < _sub_distance.get().min_distance || + d > _sub_distance.get().max_distance) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar out of range"); + warnx("[lpe] lidar out of range"); + _lidarFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_lidarFault) { + mavlink_log_info(_mavlink_fd, "[lpe] lidar fault, beta %5.2f", double(beta)); + warnx("[lpe] lidar fault, beta %5.2f", double(beta)); + _lidarFault = FAULT_MINOR; + } + + } else if (_lidarFault) { // disable fault if ok + _lidarFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] lidar OK"); + warnx("[lpe] lidar OK"); + } + + // kalman filter correction if no fault + if (_lidarFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_lidar = _sub_distance.get().timestamp; +} + +void BlockLocalPositionEstimator::correctGps() // TODO : use another other metric for glitch detection +{ + + // gps measurement in local frame + double lat = _sub_gps.get().lat * 1.0e-7; + double lon = _sub_gps.get().lon * 1.0e-7; + float alt = _sub_gps.get().alt * 1.0e-3f; + + float px = 0; + float py = 0; + float pz = alt - _gpsAltHome; + map_projection_project(&_map_ref, lat, lon, &px, &py); + + //printf("gps: lat %10g, lon, %10g alt %10g\n", lat, lon, double(alt)); + //printf("home: lat %10g, lon, %10g alt %10g\n", _sub_home.lat, _sub_home.lon, double(_sub_home.alt)); + //printf("local: x %10g y %10g z %10g\n", double(px), double(py), double(pz)); + + Matrix y; + y.setZero(); + y(0) = px; + y(1) = py; + y(2) = pz; + y(3) = _sub_gps.get().vel_n_m_s; + y(4) = _sub_gps.get().vel_e_m_s; + y(5) = _sub_gps.get().vel_d_m_s; + + // gps measurement matrix, measures position and velocity + Matrix C; + C.setZero(); + C(Y_gps_x, X_x) = 1; + C(Y_gps_y, X_y) = 1; + C(Y_gps_z, X_z) = 1; + C(Y_gps_vx, X_vx) = 1; + C(Y_gps_vy, X_vy) = 1; + C(Y_gps_vz, X_vz) = 1; + + // gps covariance matrix + Matrix R; + R.setZero(); + + // default to parameter, use gps cov if provided + float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get(); + float var_z = _gps_z_stddev.get() * _gps_z_stddev.get(); + float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get(); + float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get(); + + // if field is not zero, set it to the value provided + if (_sub_gps.get().eph > 1e-3f) { + var_xy = _sub_gps.get().eph * _sub_gps.get().eph; + } + + if (_sub_gps.get().epv > 1e-3f) { + var_z = _sub_gps.get().epv * _sub_gps.get().epv; + } + + // TODO is velocity covariance provided from gps sub + R(0, 0) = var_xy; + R(1, 1) = var_xy; + R(2, 2) = var_z; + R(3, 3) = var_vxy; + R(4, 4) = var_vxy; + R(5, 5) = var_vz; + + // residual + Matrix r = y - C * _x; + Matrix S_I = (C * _P * C.transpose() + R).inverse(); + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + uint8_t nSat = _sub_gps.get().satellites_used; + float eph = _sub_gps.get().eph; + + if (nSat < 6 || eph > _gps_eph_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + warnx("[lpe] gps fault nSat: %d eph: %5.2f", nSat, double(eph)); + _gpsFault = FAULT_SEVERE; + } + + } else if (beta > _beta_max.get()) { + if (!_gpsFault) { + mavlink_log_info(_mavlink_fd, "[lpe] gps fault, beta: %5.2f", double(beta)); + warnx("[lpe] gps fault, beta: %5.2f", double(beta)); + mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(r(0)), double(r(1)), double(r(2)), + double(r(3)), double(r(4)), double(r(5))); + mavlink_log_info(_mavlink_fd, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)), + double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5))); + mavlink_log_info(_mavlink_fd, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f", + double(r(0)), double(r(1)), double(r(2)), + double(r(3)), double(r(4)), double(r(5))); + _gpsFault = FAULT_MINOR; + } + + // trust GPS less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_gpsFault) { + _gpsFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] GPS OK"); + warnx("[lpe] GPS OK"); + } + + // kalman filter correction if no hard fault + if (_gpsFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_gps = _timeStamp; +} + +void BlockLocalPositionEstimator::correctVision() +{ + + Matrix y; + y.setZero(); + y(0) = _sub_vision_pos.get().x - _visionHome(0); + y(1) = _sub_vision_pos.get().y - _visionHome(1); + y(2) = _sub_vision_pos.get().z - _visionHome(2); + + // vision measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_vision_x, X_x) = 1; + C(Y_vision_y, X_y) = 1; + C(Y_vision_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + R(Y_vision_x, Y_vision_x) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_y, Y_vision_y) = _vision_xy_stddev.get() * _vision_xy_stddev.get(); + R(Y_vision_z, Y_vision_z) = _vision_z_stddev.get() * _vision_z_stddev.get(); + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_visionFault) { + mavlink_log_info(_mavlink_fd, "[lpe] vision position fault, beta %5.2f", double(beta)); + warnx("[lpe] vision position fault, beta %5.2f", double(beta)); + _visionFault = FAULT_MINOR; + } + + // trust less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_visionFault) { + _visionFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] vision position OK"); + warnx("[lpe] vision position OK"); + } + + // kalman filter correction if no fault + if (_visionFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_vision_p = _sub_vision_pos.get().timestamp_boot; +} + +void BlockLocalPositionEstimator::correctmocap() +{ + + Matrix y; + y.setZero(); + y(Y_mocap_x) = _sub_mocap.get().x - _mocapHome(0); + y(Y_mocap_y) = _sub_mocap.get().y - _mocapHome(1); + y(Y_mocap_z) = _sub_mocap.get().z - _mocapHome(2); + + // mocap measurement matrix, measures position + Matrix C; + C.setZero(); + C(Y_mocap_x, X_x) = 1; + C(Y_mocap_y, X_y) = 1; + C(Y_mocap_z, X_z) = 1; + + // noise matrix + Matrix R; + R.setZero(); + float mocap_p_var = _mocap_p_stddev.get()* \ + _mocap_p_stddev.get(); + R(Y_mocap_x, Y_mocap_x) = mocap_p_var; + R(Y_mocap_y, Y_mocap_y) = mocap_p_var; + R(Y_mocap_z, Y_mocap_z) = mocap_p_var; + + // residual + Matrix S_I = ((C * _P * C.transpose()) + R).inverse(); + Matrix r = y - C * _x; + + // fault detection + float beta = sqrtf((r.transpose() * (S_I * r))(0, 0)); + + if (beta > _beta_max.get()) { + if (!_mocapFault) { + mavlink_log_info(_mavlink_fd, "[lpe] mocap fault, beta %5.2f", double(beta)); + warnx("[lpe] mocap fault, beta %5.2f", double(beta)); + _mocapFault = FAULT_MINOR; + } + + // trust less + S_I = ((C * _P * C.transpose()) + R * 10).inverse(); + + } else if (_mocapFault) { + _mocapFault = FAULT_NONE; + mavlink_log_info(_mavlink_fd, "[lpe] mocap OK"); + warnx("[lpe] mocap OK"); + } + + // kalman filter correction if no fault + if (_mocapFault == FAULT_NONE) { + Matrix K = _P * C.transpose() * S_I; + _x += K * r; + _P -= K * C * _P; + } + + _time_last_mocap = _sub_mocap.get().timestamp_boot; +} diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp new file mode 100644 index 0000000000..66f301efe3 --- /dev/null +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -0,0 +1,310 @@ +#pragma once + +#include +#include +#include +#include + +#ifdef USE_MATRIX_LIB +#include "matrix/src/Matrix.hpp" +using namespace matrix; +#else +#include +using namespace Eigen; +#endif + +// uORB Subscriptions +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// uORB Publications +#include +#include +#include +#include +#include + +#define CBRK_NO_VISION_KEY 328754 + +using namespace control; + +enum fault_t { + FAULT_NONE = 0, + FAULT_MINOR, + FAULT_SEVERE +}; + +enum sensor_t { + SENSOR_BARO = 0, + SENSOR_GPS, + SENSOR_LIDAR, + SENSOR_FLOW, + SENSOR_SONAR, + SENSOR_VISION, + SENSOR_MOCAP +}; + +class BlockLocalPositionEstimator : public control::SuperBlock +{ +// +// The purpose of this estimator is to provide a robust solution for +// indoor flight. +// +// dynamics: +// +// x(+) = A * x(-) + B * u(+) +// y_i = C_i*x +// +// kalman filter +// +// E[xx'] = P +// E[uu'] = W +// E[y_iy_i'] = R_i +// +// prediction +// x(+|-) = A*x(-|-) + B*u(+) +// P(+|-) = A*P(-|-)*A' + B*W*B' +// +// correction +// x(+|+) = x(+|-) + K_i * (y_i - H_i * x(+|-) ) +// +// +// input: +// ax, ay, az (acceleration NED) +// +// states: +// px, py, pz , ( position NED) +// vx, vy, vz ( vel NED), +// bx, by, bz ( TODO accelerometer bias) +// tz (TODO terrain altitude) +// +// measurements: +// +// sonar: pz (measured d*cos(phi)*cos(theta)) +// +// baro: pz +// +// flow: vx, vy (flow is in body x, y frame) +// +// gps: px, py, pz, vx, vy, vz (flow is in body x, y frame) +// +// lidar: px (actual measured d*cos(phi)*cos(theta)) +// +// vision: px, py, pz, vx, vy, vz +// +// mocap: px, py, pz +// +public: + BlockLocalPositionEstimator(); + void update(); + virtual ~BlockLocalPositionEstimator(); + +private: + // prevent copy and assignment + BlockLocalPositionEstimator(const BlockLocalPositionEstimator &); + BlockLocalPositionEstimator operator=(const BlockLocalPositionEstimator &); + + // constants + static const uint8_t n_x = 9; + static const uint8_t n_u = 3; // 3 accelerations + static const uint8_t n_y_flow = 2; + static const uint8_t n_y_sonar = 1; + static const uint8_t n_y_baro = 1; + static const uint8_t n_y_lidar = 1; + static const uint8_t n_y_gps = 6; + static const uint8_t n_y_vision = 3; + static const uint8_t n_y_mocap = 3; + enum {X_x = 0, X_y, X_z, X_vx, X_vy, X_vz, X_bx, X_by, X_bz}; + enum {U_ax = 0, U_ay, U_az}; + enum {Y_baro_z = 0}; + enum {Y_lidar_z = 0}; + enum {Y_flow_x = 0, Y_flow_y}; + enum {Y_sonar_z = 0}; + enum {Y_gps_x = 0, Y_gps_y, Y_gps_z, Y_gps_vx, Y_gps_vy, Y_gps_vz}; + enum {Y_vision_x = 0, Y_vision_y, Y_vision_z, Y_vision_vx, Y_vision_vy, Y_vision_vz}; + enum {Y_mocap_x = 0, Y_mocap_y, Y_mocap_z}; + enum {POLL_FLOW, POLL_SENSORS, POLL_PARAM}; + + // methods + // ---------------------------- + void initP(); + + // predict the next state + void predict(); + + // correct the state prediction wtih a measurement + void correctBaro(); + void correctGps(); + void correctLidar(); + void correctFlow(); + void correctSonar(); + void correctVision(); + void correctmocap(); + + // sensor initialization + void updateHome(); + void initBaro(); + void initGps(); + void initLidar(); + void initSonar(); + void initFlow(); + void initVision(); + void initmocap(); + + // publications + void publishLocalPos(); + void publishGlobalPos(); + void publishFilteredFlow(); + void publishEstimatorStatus(); + + // attributes + // ---------------------------- + + // subscriptions + uORB::Subscription _sub_status; + uORB::Subscription _sub_armed; + uORB::Subscription _sub_control_mode; + uORB::Subscription _sub_att; + uORB::Subscription _sub_att_sp; + uORB::Subscription _sub_flow; + uORB::Subscription _sub_sensor; + uORB::Subscription _sub_distance; + uORB::Subscription _sub_param_update; + uORB::Subscription _sub_manual; + uORB::Subscription _sub_home; + uORB::Subscription _sub_gps; + uORB::Subscription _sub_vision_pos; + uORB::Subscription _sub_mocap; + + // publications + uORB::Publication _pub_lpos; + uORB::Publication _pub_gpos; + uORB::Publication _pub_filtered_flow; + uORB::Publication _pub_est_status; + + // map projection + struct map_projection_reference_s _map_ref; + + // parameters + BlockParamInt _integrate; + + BlockParamFloat _flow_xy_stddev; + BlockParamFloat _sonar_z_stddev; + + BlockParamFloat _lidar_z_stddev; + + BlockParamFloat _accel_xy_stddev; + BlockParamFloat _accel_z_stddev; + + BlockParamFloat _baro_stddev; + + BlockParamFloat _gps_xy_stddev; + BlockParamFloat _gps_z_stddev; + + BlockParamFloat _gps_vxy_stddev; + BlockParamFloat _gps_vz_stddev; + + BlockParamFloat _gps_eph_max; + + BlockParamFloat _vision_xy_stddev; + BlockParamFloat _vision_z_stddev; + BlockParamInt _no_vision; + BlockParamFloat _beta_max; + + BlockParamFloat _mocap_p_stddev; + + // process noise + BlockParamFloat _pn_p_noise_power; + BlockParamFloat _pn_v_noise_power; + BlockParamFloat _pn_b_noise_power; + + // misc + struct pollfd _polls[3]; + uint64_t _timeStamp; + uint64_t _time_last_xy; + uint64_t _time_last_flow; + uint64_t _time_last_baro; + uint64_t _time_last_gps; + uint64_t _time_last_lidar; + uint64_t _time_last_sonar; + uint64_t _time_last_vision_p; + uint64_t _time_last_mocap; + int _mavlink_fd; + + // initialization flags + bool _baroInitialized; + bool _gpsInitialized; + bool _lidarInitialized; + bool _sonarInitialized; + bool _flowInitialized; + bool _visionInitialized; + bool _mocapInitialized; + + // init counts + int _baroInitCount; + int _gpsInitCount; + int _lidarInitCount; + int _sonarInitCount; + int _flowInitCount; + int _visionInitCount; + int _mocapInitCount; + + // reference altitudes + float _altHome; + bool _altHomeInitialized; + float _baroAltHome; + float _gpsAltHome; + float _lidarAltHome; + float _sonarAltHome; + float _flowAltHome; + Vector3f _visionHome; + Vector3f _mocapHome; + + // flow integration + float _flowX; + float _flowY; + float _flowMeanQual; + + // referene lat/lon + double _gpsLatHome; + double _gpsLonHome; + + // status + bool _canEstimateXY; + bool _canEstimateZ; + bool _xyTimeout; + + // sensor faults + fault_t _baroFault; + fault_t _gpsFault; + fault_t _lidarFault; + fault_t _flowFault; + fault_t _sonarFault; + fault_t _visionFault; + fault_t _mocapFault; + + bool _visionTimeout; + bool _mocapTimeout; + + perf_counter_t _loop_perf; + perf_counter_t _interval_perf; + perf_counter_t _err_perf; + + // state space + Matrix _x; // state vector + Matrix _u; // input vector + Matrix _P; // state covariance matrix +}; diff --git a/src/modules/local_position_estimator/CMakeLists.txt b/src/modules/local_position_estimator/CMakeLists.txt new file mode 100644 index 0000000000..e0d1be2d0a --- /dev/null +++ b/src/modules/local_position_estimator/CMakeLists.txt @@ -0,0 +1,57 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +if(${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=6500) +elseif(${OS} STREQUAL "posix") + list(APPEND MODULE_CFLAGS -Wno-error) + # add matrix tests + add_subdirectory(matrix) +endif() + +# use custom matrix lib instead of Eigen +add_definitions(-DUSE_MATRIX_LIB) + + +px4_add_module( + MODULE modules__local_position_estimator + MAIN local_position_estimator + STACK 9216 + COMPILE_FLAGS ${MODULE_CFLAGS} + SRCS + local_position_estimator_main.cpp + BlockLocalPositionEstimator.cpp + params.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/local_position_estimator/local_position_estimator_main.cpp b/src/modules/local_position_estimator/local_position_estimator_main.cpp new file mode 100644 index 0000000000..b0cd25554e --- /dev/null +++ b/src/modules/local_position_estimator/local_position_estimator_main.cpp @@ -0,0 +1,169 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file local_position_estimator.cpp + * @author James Goppert + * @author Mohammed Kabir + * @author Nuno Marques + * + * Local position estimator + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "BlockLocalPositionEstimator.hpp" + +static volatile bool thread_should_exit = false; /**< Deamon exit flag */ +static volatile bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int local_position_estimator_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int local_position_estimator_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static int usage(const char *reason); + +static int +usage(const char *reason) +{ + if (reason) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: local_position_estimator {start|stop|status} [-p ]\n\n"); + return 1; +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int local_position_estimator_main(int argc, char *argv[]) +{ + + if (argc < 1) { + usage("missing command"); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("already running"); + /* this is not an error */ + return 0; + } + + thread_should_exit = false; + + deamon_task = px4_task_spawn_cmd("lp_estimator", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 10240, + local_position_estimator_thread_main, + (argv && argc > 2) ? (char *const *) &argv[2] : (char *const *) NULL); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (thread_running) { + warnx("stop"); + thread_should_exit = true; + + } else { + warnx("not started"); + } + + return 0; + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("is running"); + + } else { + warnx("not started"); + } + + return 0; + } + + usage("unrecognized command"); + return 1; +} + +int local_position_estimator_thread_main(int argc, char *argv[]) +{ + + warnx("starting"); + + using namespace control; + + + thread_running = true; + + BlockLocalPositionEstimator est; + + while (!thread_should_exit) { + est.update(); + } + + warnx("exiting."); + + thread_running = false; + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/.gitignore b/src/modules/local_position_estimator/matrix/.gitignore new file mode 100644 index 0000000000..a5309e6b90 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/.gitignore @@ -0,0 +1 @@ +build*/ diff --git a/src/modules/local_position_estimator/matrix/CMakeLists.txt b/src/modules/local_position_estimator/matrix/CMakeLists.txt new file mode 100644 index 0000000000..5a16ced707 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 2.8) + +project(matrix CXX) + +set(CMAKE_BUILD_TYPE "RelWithDebInfo") + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Weffc++") + +enable_testing() + +include_directories(src) + +add_subdirectory(test) diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp new file mode 100644 index 0000000000..8a0432b7ef --- /dev/null +++ b/src/modules/local_position_estimator/matrix/src/Matrix.hpp @@ -0,0 +1,459 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace matrix +{ + +template +class Matrix +{ + +private: + T _data[M][N]; + size_t _rows; + size_t _cols; + +public: + + Matrix() : + _data(), + _rows(M), + _cols(N) + { + } + + Matrix(const T *data) : + _data(), + _rows(M), + _cols(N) + { + memcpy(_data, data, sizeof(_data)); + } + + Matrix(const Matrix &other) : + _data(), + _rows(M), + _cols(N) + { + memcpy(_data, other._data, sizeof(_data)); + } + + Matrix(T x, T y, T z) : + _data(), + _rows(M), + _cols(N) + { + // TODO, limit to only 3x1 matrices + _data[0][0] = x; + _data[1][0] = y; + _data[2][0] = z; + } + + /** + * Accessors/ Assignment etc. + */ + + inline size_t rows() const + { + return _rows; + } + + inline size_t cols() const + { + return _rows; + } + + inline T operator()(size_t i) const + { + return _data[i][0]; + } + + inline T operator()(size_t i, size_t j) const + { + return _data[i][j]; + } + + inline T &operator()(size_t i) + { + return _data[i][0]; + } + + inline T &operator()(size_t i, size_t j) + { + return _data[i][j]; + } + + /** + * Matrix Operations + */ + + // this might use a lot of programming memory + // since it instantiates a class for every + // required mult pair, but it provides + // compile time size_t checking + template + Matrix operator*(const Matrix &other) const + { + const Matrix &self = *this; + Matrix res; + res.setZero(); + + for (size_t i = 0; i < M; i++) { + for (size_t k = 0; k < P; k++) { + for (size_t j = 0; j < N; j++) { + res(i, k) += self(i, j) * other(j, k); + } + } + } + + return res; + } + + Matrix operator+(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) + other(i, j); + } + } + + return res; + } + + bool operator==(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + if (self(i , j) != other(i, j)) { + return false; + } + } + } + + return true; + } + + Matrix operator-(const Matrix &other) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) - other(i, j); + } + } + + return res; + } + + void operator+=(const Matrix &other) + { + Matrix &self = *this; + self = self + other; + } + + void operator-=(const Matrix &other) + { + Matrix &self = *this; + self = self - other; + } + + void operator*=(const Matrix &other) + { + Matrix &self = *this; + self = self * other; + } + + /** + * Scalar Operations + */ + + Matrix operator*(T scalar) const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) * scalar; + } + } + + return res; + } + + Matrix operator+(T scalar) const + { + Matrix res; + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(i , j) = self(i, j) + scalar; + } + } + + return res; + } + + void operator*=(T scalar) + { + Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + self(i, j) = self(i, j) * scalar; + } + } + } + + void operator/=(T scalar) + { + Matrix &self = *this; + self = self * (1.0f / scalar); + } + + /** + * Misc. Functions + */ + + void print() + { + Matrix &self = *this; + printf("\n"); + + for (size_t i = 0; i < M; i++) { + printf("["); + + for (size_t j = 0; j < N; j++) { + printf("%10g\t", double(self(i, j))); + } + + printf("]\n"); + } + } + + Matrix transpose() const + { + Matrix res; + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + for (size_t j = 0; j < N; j++) { + res(j, i) = self(i, j); + } + } + + return res; + } + + Matrix expm(float dt, size_t n) const + { + Matrix res; + res.setIdentity(); + Matrix A_pow = *this; + float k_fact = 1; + size_t k = 1; + + while (k < n) { + res += A_pow * (float(pow(dt, k)) / k_fact); + + if (k == n) { break; } + + A_pow *= A_pow; + k_fact *= k; + k++; + } + + return res; + } + + Matrix diagonal() const + { + Matrix res; + // force square for now + const Matrix &self = *this; + + for (size_t i = 0; i < M; i++) { + res(i) = self(i, i); + } + + return res; + } + + void setZero() + { + memset(_data, 0, sizeof(_data)); + } + + void setIdentity() + { + setZero(); + Matrix &self = *this; + + for (size_t i = 0; i < M and i < N; i++) { + self(i, i) = 1; + } + } + + inline void swapRows(size_t a, size_t b) + { + if (a == b) { return; } + + Matrix &self = *this; + + for (size_t j = 0; j < cols(); j++) { + T tmp = self(a, j); + self(a, j) = self(b, j); + self(b, j) = tmp; + } + } + + inline void swapCols(size_t a, size_t b) + { + if (a == b) { return; } + + Matrix &self = *this; + + for (size_t i = 0; i < rows(); i++) { + T tmp = self(i, a); + self(i, a) = self(i, b); + self(i, b) = tmp; + } + } + + /** + * inverse based on LU factorization with partial pivotting + */ + Matrix inverse() const + { + Matrix L; + L.setIdentity(); + const Matrix &A = (*this); + Matrix U = A; + Matrix P; + P.setIdentity(); + + //printf("A:\n"); A.print(); + + // for all diagonal elements + for (size_t n = 0; n < N; n++) { + + // if diagonal is zero, swap with row below + if (fabsf(U(n, n)) < 1e-8f) { + //printf("trying pivot for row %d\n",n); + for (size_t i = 0; i < N; i++) { + if (i == n) { continue; } + + //printf("\ttrying row %d\n",i); + if (fabsf(U(i, n)) > 1e-8f) { + //printf("swapped %d\n",i); + U.swapRows(i, n); + P.swapRows(i, n); + } + } + } + +#ifdef MATRIX_ASSERT + //printf("A:\n"); A.print(); + //printf("U:\n"); U.print(); + //printf("P:\n"); P.print(); + //fflush(stdout); + ASSERT(fabsf(U(n, n)) > 1e-8f); +#endif + + // failsafe, return zero matrix + if (fabsf(U(n, n)) < 1e-8f) { + Matrix zero; + zero.setZero(); + return zero; + } + + // for all rows below diagonal + for (size_t i = (n + 1); i < N; i++) { + L(i, n) = U(i, n) / U(n, n); + + // add i-th row and n-th row + // multiplied by: -a(i,n)/a(n,n) + for (size_t k = n; k < N; k++) { + U(i, k) -= L(i, n) * U(n, k); + } + } + } + + //printf("L:\n"); L.print(); + //printf("U:\n"); U.print(); + + // solve LY=P*I for Y by forward subst + Matrix Y = P; + + // for all columns of Y + for (size_t c = 0; c < N; c++) { + // for all rows of L + for (size_t i = 0; i < N; i++) { + // for all columns of L + for (size_t j = 0; j < i; j++) { + // for all existing y + // subtract the component they + // contribute to the solution + Y(i, c) -= L(i, j) * Y(j, c); + } + + // divide by the factor + // on current + // term to be solved + // Y(i,c) /= L(i,i); + // but L(i,i) = 1.0 + } + } + + //printf("Y:\n"); Y.print(); + + // solve Ux=y for x by back subst + Matrix X = Y; + + // for all columns of X + for (size_t c = 0; c < N; c++) { + // for all rows of U + for (size_t k = 0; k < N; k++) { + // have to go in reverse order + size_t i = N - 1 - k; + + // for all columns of U + for (size_t j = i + 1; j < N; j++) { + // for all existing x + // subtract the component they + // contribute to the solution + X(i, c) -= U(i, j) * X(j, c); + } + + // divide by the factor + // on current + // term to be solved + X(i, c) /= U(i, i); + } + } + + //printf("X:\n"); X.print(); + return X; + } + +}; + +typedef Matrix Vector2f; +typedef Matrix Vector3f; +typedef Matrix Matrix3f; + +}; // namespace matrix diff --git a/src/modules/local_position_estimator/matrix/test/CMakeLists.txt b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt new file mode 100644 index 0000000000..cf280e287b --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/CMakeLists.txt @@ -0,0 +1,15 @@ +set(tests + setIdentity + inverse + matrixMult + vectorAssignment + matrixAssignment + matrixScalarMult + transpose + ) + +foreach(test ${tests}) + add_executable(${test} + ${test}.cpp) + add_test(${test} ${test}) +endforeach() diff --git a/src/modules/local_position_estimator/matrix/test/inverse.cpp b/src/modules/local_position_estimator/matrix/test/inverse.cpp new file mode 100644 index 0000000000..73f7c0b432 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/inverse.cpp @@ -0,0 +1,30 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; + Matrix3f A(data); + Matrix3f A_I = A.inverse(); + float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; + Matrix3f A_I_check(data_check); + (void)A_I; + assert(A_I == A_I_check); + + // stess test + static const size_t n = 100; + Matrix A_large; + A_large.setIdentity(); + Matrix A_large_I; + A_large_I.setZero(); + + for (size_t i = 0; i < 100; i++) { + A_large_I = A_large.inverse(); + assert(A_large == A_large_I); + } + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp new file mode 100644 index 0000000000..5a625d0cc1 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixAssignment.cpp @@ -0,0 +1,29 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Matrix3f m; + m.setZero(); + m(0, 0) = 1; + m(0, 1) = 2; + m(0, 2) = 3; + m(1, 0) = 4; + m(1, 1) = 5; + m(1, 2) = 6; + m(2, 0) = 7; + m(2, 1) = 8; + m(2, 2) = 9; + + m.print(); + + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f m2(data); + m2.print(); + + assert(m == m2); + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp new file mode 100644 index 0000000000..7b42d947d4 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixMult.cpp @@ -0,0 +1,26 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 0, 0, 0, 1, 0, 1, 0, 1}; + Matrix3f A(data); + float data_check[9] = {1, 0, 0, 0, 1, 0, -1, 0, 1}; + Matrix3f A_I(data_check); + Matrix3f I; + I.setIdentity(); + A.print(); + A_I.print(); + Matrix3f R = A * A_I; + R.print(); + assert(R == I); + + Matrix3f R2 = A; + R2 *= A_I; + R2.print(); + assert(R2 == I); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp new file mode 100644 index 0000000000..78bdb5b27f --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/matrixScalarMult.cpp @@ -0,0 +1,18 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, 4, 5, 6, 7, 8, 9}; + Matrix3f A(data); + A = A * 2; + float data_check[9] = {2, 4, 6, 8, 10, 12, 14, 16, 18}; + Matrix3f A_check(data_check); + A.print(); + A_check.print(); + assert(A == A_check); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/setIdentity.cpp b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp new file mode 100644 index 0000000000..f80e437939 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/setIdentity.cpp @@ -0,0 +1,25 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Matrix3f A; + A.setIdentity(); + assert(A.rows() == 3); + assert(A.cols() == 3); + + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + if (i == j) { + assert(A(i, j) == 1); + + } else { + assert(A(i, j) == 0); + } + } + } + + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/transpose.cpp b/src/modules/local_position_estimator/matrix/test/transpose.cpp new file mode 100644 index 0000000000..3623fc1f9a --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/transpose.cpp @@ -0,0 +1,18 @@ +#include "Matrix.hpp" +#include +#include + +using namespace matrix; + +int main() +{ + float data[9] = {1, 2, 3, 4, 5, 6}; + Matrix A(data); + Matrix A_T = A.transpose(); + float data_check[9] = {1, 4, 2, 5, 3, 6}; + Matrix A_T_check(data_check); + A_T.print(); + A_T_check.print(); + assert(A_T == A_T_check); + return 0; +} diff --git a/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp new file mode 100644 index 0000000000..68f5070194 --- /dev/null +++ b/src/modules/local_position_estimator/matrix/test/vectorAssignment.cpp @@ -0,0 +1,28 @@ +#include "Matrix.hpp" +#include + +using namespace matrix; + +int main() +{ + Vector3f v; + v(0) = 1; + v(1) = 2; + v(2) = 3; + + v.print(); + + assert(v(0) == 1); + assert(v(1) == 2); + assert(v(2) == 3); + + Vector3f v2(4, 5, 6); + + v2.print(); + + assert(v2(0) == 4); + assert(v2(1) == 5); + assert(v2(2) == 6); + + return 0; +} diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c new file mode 100644 index 0000000000..8fbde4ab69 --- /dev/null +++ b/src/modules/local_position_estimator/params.c @@ -0,0 +1,222 @@ +#include + +// 16 is max name length + + +/** + * Enable local position estimator. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_ENABLED, 1); + +/** + * Enable accelerometer integration for prediction. + * + * @group Local Position Estimator + */ +PARAM_DEFINE_INT32(LPE_INTEGRATE, 1); + +/** + * Optical flow xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_FLW_XY, 0.01f); + +/** + * Sonar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_SNR_Z, 0.2f); + +/** + * Lidar z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_LDR_Z, 0.03f); + +/** + * Accelerometer xy standard deviation + * + * Data sheet sqrt(Noise power) = 150ug/sqrt(Hz) + * std dev = (150*9.8*1e-6)*sqrt(1000 Hz) m/s^2 + * Since accels sampled at 1000 Hz. + * + * should be 0.0464 + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_XY, 0.0454f); + +/** + * Accelerometer z standard deviation + * + * (see Accel x comments) + * + * @group Local Position Estimator + * @unit m/s^2 + * @min 0.00001 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_ACC_Z, 0.0454f); + +/** + * Barometric presssure altitude z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 3 + */ +PARAM_DEFINE_FLOAT(LPE_BAR_Z, 1.0f); + +/** + * GPS xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_XY, 2.0f); + +/** + * GPS z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 20 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_Z, 10.0f); + +/** + * GPS xy velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VXY, 0.275f); + +/** + * GPS z velocity standard deviation. + * + * @group Local Position Estimator + * @unit m/s + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_GPS_VZ, 0.237f); + +/** + * GPS max eph + * + * @group Local Position Estimator + * @unit m + * @min 1.0 + * @max 5.0 + */ +PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f); + + + +/** + * Vision xy standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_XY, 0.5f); + +/** + * Vision z standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 2 + */ +PARAM_DEFINE_FLOAT(LPE_VIS_Z, 0.5f); + +/** + * Circuit breaker to disable vision input. + * + * Set to the appropriate key (328754) to disable vision input. + * + * @group Local Position Estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(LPE_NO_VISION, 0); + +/** + * Vicon position standard deviation. + * + * @group Local Position Estimator + * @unit m + * @min 0.01 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_VIC_P, 0.05f); + +/** + * Position propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s^2)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_P, 0.0f); + +/** + * Velocity propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 5 + */ +PARAM_DEFINE_FLOAT(LPE_PN_V, 0.0f); + +/** + * Accel bias propagation process noise power (variance*sampling rate). + * + * @group Local Position Estimator + * @unit (m/s)-s + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(LPE_PN_B, 1e-8f); + +/** + * Fault detection threshold, for chi-squared dist. + * + * TODO add separate params for 1 dof, 3 dof, and 6 dof beta + * or false alarm rate in false alarms/hr + * + * @group Local Position Estimator + * @unit + * @min 3 + * @max 1000 + */ +PARAM_DEFINE_FLOAT(LPE_BETA_MAX, 1000.0f); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fed42bef2b..be3ab850f4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -1271,6 +1272,78 @@ protected: }; +class MavlinkStreamLocalPositionNEDCOV : public MavlinkStream +{ +public: + const char *get_name() const + { + return MavlinkStreamLocalPositionNEDCOV::get_name_static(); + } + + static const char *get_name_static() + { + return "LOCAL_POSITION_NED_COV"; + } + + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) + { + return new MavlinkStreamLocalPositionNEDCOV(mavlink); + } + + unsigned get_size() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + MavlinkOrbSubscription *_est_sub; + uint64_t _est_time; + + /* do not allow top copying this class */ + MavlinkStreamLocalPositionNEDCOV(MavlinkStreamLocalPositionNEDCOV &); + MavlinkStreamLocalPositionNEDCOV& operator = (const MavlinkStreamLocalPositionNEDCOV &); + +protected: + explicit MavlinkStreamLocalPositionNEDCOV(Mavlink *mavlink) : MavlinkStream(mavlink), + _est_sub(_mavlink->add_orb_subscription(ORB_ID(estimator_status))), + _est_time(0) + {} + + void send(const hrt_abstime t) + { + struct estimator_status_s est; + + if (_est_sub->update(&_est_time, &est)) { + mavlink_local_position_ned_cov_t msg; + + msg.time_boot_ms = est.timestamp / 1000; + msg.x = est.states[0]; + msg.y = est.states[1]; + msg.z = est.states[2]; + msg.vx = est.states[3]; + msg.vy = est.states[4]; + msg.vz = est.states[5]; + msg.ax = est.states[6]; + msg.ay = est.states[7]; + msg.az = est.states[8]; + for (int i=0;i<9;i++) { + msg.covariance[i] = est.covariances[i]; + } + msg.covariance[9] = est.nan_flags; + msg.covariance[10] = est.health_flags; + msg.covariance[11] = est.timeout_flags; + memcpy(msg.covariance, est.covariances, sizeof(est.covariances)); + + _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV, &msg); + } + } +}; + class MavlinkStreamAttPosMocap : public MavlinkStream { public: @@ -2478,6 +2551,7 @@ const StreamListItem *streams_list[] = { new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), + new StreamListItem(&MavlinkStreamLocalPositionNEDCOV::new_instance, &MavlinkStreamLocalPositionNEDCOV::get_name_static), new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static), new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static), new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static), diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 60747854cd..80847d8e47 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -155,6 +155,10 @@ private: control::BlockParamFloat _manual_thr_min; control::BlockParamFloat _manual_thr_max; + control::BlockDerivative _vel_x_deriv; + control::BlockDerivative _vel_y_deriv; + control::BlockDerivative _vel_z_deriv; + struct { param_t thr_min; param_t thr_max; @@ -331,6 +335,9 @@ MulticopterPositionControl::MulticopterPositionControl() : _global_vel_sp_pub(nullptr), _manual_thr_min(this, "MANTHR_MIN"), _manual_thr_max(this, "MANTHR_MAX"), + _vel_x_deriv(this, "VELD"), + _vel_y_deriv(this, "VELD"), + _vel_z_deriv(this, "VELD"), _ref_alt(0.0f), _ref_timestamp(0), @@ -1039,6 +1046,9 @@ MulticopterPositionControl::task_main() float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; t_prev = t; + // set dt for control blocks + setDt(dt); + if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; @@ -1217,8 +1227,12 @@ MulticopterPositionControl::task_main() /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; - /* derivative of velocity error, not includes setpoint acceleration */ - math::Vector<3> vel_err_d = (_vel - _vel_prev) / dt; + /* derivative of velocity error, / + * does not includes setpoint acceleration */ + math::Vector<3> vel_err_d; + vel_err_d(0) = _vel_x_deriv.update(-_vel(0)); + vel_err_d(1) = _vel_y_deriv.update(-_vel(1)); + vel_err_d(2) = _vel_z_deriv.update(-_vel(2)); /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 09843de1f1..0fa51f6f0c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -303,3 +303,11 @@ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.5f); */ PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.5f); +/** + * Low pass filter cut freq. for numerical velocity derivative + * + * @unit Hz + * @min 0.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); diff --git a/src/platforms/posix/px4_layer/drv_hrt.c b/src/platforms/posix/px4_layer/drv_hrt.c index c925574b42..0544131705 100644 --- a/src/platforms/posix/px4_layer/drv_hrt.c +++ b/src/platforms/posix/px4_layer/drv_hrt.c @@ -366,10 +366,10 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte #if 1 // Use this to debug busy CPU that keeps rescheduling with 0 period time - if (interval < HRT_INTERVAL_MIN) { - PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval); - PX4_BACKTRACE(); - } + /*if (interval < HRT_INTERVAL_MIN) {*/ + /*PX4_ERR("hrt_call_internal interval too short: %" PRIu64, interval);*/ + /*PX4_BACKTRACE();*/ + /*}*/ #endif entry->deadline = deadline; From 6cce823dc6fd0c2f6477c871e1e57dd37098645a Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:06:59 -0400 Subject: [PATCH 120/155] Replaced wigen with custom matrix lib. --- .gitmodules | 3 -- src/lib/mathlib/math/Matrix.hpp | 29 +++++++------------ src/modules/commander/commander_helper.cpp | 2 +- src/modules/controllib/blocks.cpp | 4 +-- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 12 ++++---- .../matrix/src/Matrix.hpp | 4 +++ src/modules/navigator/mission_block.cpp | 2 +- src/platforms/px4_defines.h | 1 + 8 files changed, 26 insertions(+), 31 deletions(-) diff --git a/.gitmodules b/.gitmodules index b05206ecfa..50f26b3a99 100644 --- a/.gitmodules +++ b/.gitmodules @@ -13,9 +13,6 @@ [submodule "Tools/gencpp"] path = Tools/gencpp url = https://github.com/ros/gencpp.git -[submodule "src/lib/eigen"] - path = src/lib/eigen - url = https://github.com/PX4/eigen.git [submodule "src/lib/dspal"] path = src/lib/dspal url = https://github.com/mcharleb/dspal.git diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index b81f8868ec..40c32ca348 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -49,8 +49,7 @@ #ifdef CONFIG_ARCH_ARM #include "../CMSIS/Include/arm_math.h" #else -#include -#include +#include "modules/local_position_estimator/matrix/src/Matrix.hpp" #endif #include @@ -308,11 +307,9 @@ public: arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix Him = Eigen::Map > - (m.arm_mat.pData); - Eigen::Matrix Product = Me * Him; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Him(m.arm_mat.pData); + matrix::Matrix Product = Me * Him; Matrix res(Product.data()); return res; #endif @@ -327,10 +324,8 @@ public: arm_mat_trans_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Me.transposeInPlace(); - Matrix res(Me.data()); + matrix::Matrix Me(this->arm_mat.pData); + Matrix res(Me.transpose().data()); return res; #endif } @@ -344,9 +339,8 @@ public: arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat); return res; #else - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea Matrix res(MyInverse.data()); return res; #endif @@ -413,10 +407,9 @@ public: arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); #else //probably nicer if this could go into a function like "eigen_mat_mult" or so - Eigen::Matrix Me = Eigen::Map > - (this->arm_mat.pData); - Eigen::VectorXf Vec = Eigen::Map(v.arm_col.pData, N); - Eigen::VectorXf Product = Me * Vec; + matrix::Matrix Me(this->arm_mat.pData); + matrix::Matrix Vec(v.arm_col.pData); + matrix::Matrix Product = Me * Vec; Vector res(Product.data()); #endif return res; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0ffaea5bec..f411e1c128 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -387,7 +387,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged, float // XXX this time constant needs to become tunable // but really, the right fix are smart batteries. float val = throttle_lowpassed * 0.97f + throttle_normalized * 0.03f; - if (isfinite(val)) { + if (PX4_ISFINITE(val)) { throttle_lowpassed = val; } diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 04b96753ef..3915ecc5e1 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -123,7 +123,7 @@ int blockLimitSymTest() float BlockLowPass::update(float input) { - if (!isfinite(getState())) { + if (!PX4_ISFINITE(getState())) { setState(input); } @@ -203,7 +203,7 @@ int blockHighPassTest() float BlockLowPass2::update(float input) { - if (!isfinite(getState())) { + if (!PX4_ISFINITE(getState())) { setState(input); } diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 6de6d9d8f0..7a045fb1bb 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -88,8 +88,8 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(altitude) || - !isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) || + !PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -125,8 +125,8 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng float airspeedSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) { return -1; } @@ -163,8 +163,8 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride) { /* check if all input arguments are numbers and abort if not so */ - if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || - !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) || + !PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) { return -1; } /* time measurement */ diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp index 8a0432b7ef..222c5d1872 100644 --- a/src/modules/local_position_estimator/matrix/src/Matrix.hpp +++ b/src/modules/local_position_estimator/matrix/src/Matrix.hpp @@ -58,6 +58,10 @@ public: * Accessors/ Assignment etc. */ + T * data() { + return _data[0]; + } + inline size_t rows() const { return _rows; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 2ce37ef94d..94ded8a4b9 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -153,7 +153,7 @@ MissionBlock::is_mission_item_reached() if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ - if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 06bddf2eb6..4d453e5983 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -216,6 +216,7 @@ __END_DECLS #ifndef __PX4_QURT #if defined(__cplusplus) +#include #define PX4_ISFINITE(x) std::isfinite(x) #else #define PX4_ISFINITE(x) isfinite(x) From 0acf6db64f0d6a79998879517519df628e388023 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:15:33 -0400 Subject: [PATCH 121/155] Removed more eigen references. --- CMakeLists.txt | 3 +-- src/lib/eigen | 1 - src/platforms/common/CMakeLists.txt | 4 ---- 3 files changed, 1 insertion(+), 7 deletions(-) delete mode 160000 src/lib/eigen diff --git a/CMakeLists.txt b/CMakeLists.txt index 9486bf13fb..fb13d8283a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -225,7 +225,6 @@ px4_add_git_submodule(TARGET git_gencpp PATH "Tools/gencpp") px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0") px4_add_git_submodule(TARGET git_gtest PATH "unittets/gtest") px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") -px4_add_git_submodule(TARGET git_eigen PATH "src/lib/eigen") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") @@ -288,7 +287,7 @@ px4_generate_messages(TARGET msg_gen px4_generate_parameters_xml(OUT parameters.xml BOARD ${BOARD}) px4_generate_airframes_xml(OUT airframes.xml BOARD ${BOARD}) add_custom_target(xml_gen - DEPENDS git_eigen parameters.xml airframes.xml) + DEPENDS parameters.xml airframes.xml) #============================================================================= # external projects diff --git a/src/lib/eigen b/src/lib/eigen deleted file mode 160000 index eb7213abbf..0000000000 --- a/src/lib/eigen +++ /dev/null @@ -1 +0,0 @@ -Subproject commit eb7213abbf38a987a18ed5071172d9d26cc38306 diff --git a/src/platforms/common/CMakeLists.txt b/src/platforms/common/CMakeLists.txt index 7b50768ac1..09defae9b4 100644 --- a/src/platforms/common/CMakeLists.txt +++ b/src/platforms/common/CMakeLists.txt @@ -37,10 +37,6 @@ set(depends git_uavcan ) -if(${OS} STREQUAL "posix" OR ${BOARD} STREQUAL "sim") - list(APPEND depends git_eigen) -endif() - px4_add_module( MODULE platforms__common SRCS From 65a0de38bb168eb70c4c98bf4949a3d97b3400e0 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:51:32 -0400 Subject: [PATCH 122/155] Format fix. --- src/modules/local_position_estimator/matrix/src/Matrix.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/local_position_estimator/matrix/src/Matrix.hpp b/src/modules/local_position_estimator/matrix/src/Matrix.hpp index 222c5d1872..ae1111894c 100644 --- a/src/modules/local_position_estimator/matrix/src/Matrix.hpp +++ b/src/modules/local_position_estimator/matrix/src/Matrix.hpp @@ -58,7 +58,8 @@ public: * Accessors/ Assignment etc. */ - T * data() { + T *data() + { return _data[0]; } From d198c6a324967fa27cc59e9e18c4a66bcee4b953 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 15:59:11 -0400 Subject: [PATCH 123/155] Removed comment. --- src/lib/mathlib/math/Matrix.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 40c32ca348..38322e56a7 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -406,7 +406,6 @@ public: Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); #else - //probably nicer if this could go into a function like "eigen_mat_mult" or so matrix::Matrix Me(this->arm_mat.pData); matrix::Matrix Vec(v.arm_col.pData); matrix::Matrix Product = Me * Vec; From aff78e0f5e4f480467241cfe4fabfc09dd020f7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 11:20:28 +0100 Subject: [PATCH 124/155] CMake / Ninja: Use console flag so upload targets can print their normal output and receive input. --- Makefile | 5 +++-- cmake/common/px4_base.cmake | 1 + src/firmware/posix/CMakeLists.txt | 1 + 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 7cba819317..2a0e843f94 100644 --- a/Makefile +++ b/Makefile @@ -68,11 +68,11 @@ ARGS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) j ?= 4 # disable ninja by default for now because it hides upload progress -#NINJA_BUILD := $(shell ninja --version 2>/dev/null) +NINJA_BUILD := $(shell ninja --version 2>/dev/null) ifdef NINJA_BUILD PX4_CMAKE_GENERATOR ?= "Ninja" PX4_MAKE = ninja - PX4_MAKE_ARGS = + PX4_MAKE_ARGS = else ifdef SYSTEMROOT @@ -89,6 +89,7 @@ endif # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build ++@if [ $(PX4_CMAKE_GENERATOR) == "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi +@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 5c8ea5787a..bd9cf1f406 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -433,6 +433,7 @@ function(px4_add_upload) WORKING_DIRECTORY ${CMAKE_BINARY_DIR} COMMENT "uploading ${BUNDLE}" VERBATIM + USES_TERMINAL ) endfunction() diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index e4c2855c66..88028d176e 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -28,6 +28,7 @@ add_custom_target(run_sitl COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" "${config_sitl_viewer}" "${CMAKE_BINARY_DIR}" WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL ) add_dependencies(run_sitl mainapp) From 376e77c17205b7c03439726a4923f1c3bc8c8b87 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 11:35:11 +0100 Subject: [PATCH 125/155] Add CMake 3.2 --- .travis.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 9aeedc9fbe..9e0ccbb71a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,11 +21,12 @@ addons: sources: - kubuntu-backports - ubuntu-toolchain-r-test + - george-edison55-precise-backports packages: - build-essential - ccache - clang-3.5 - - cmake + - cmake-3.2 - g++-4.8 - gcc-4.8 - genromfs From 0b8419018e86a8d3b87b7ab7f3d17a46baf70997 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 11:54:42 +0100 Subject: [PATCH 126/155] Upgrade Travis to CMake .32 --- .travis.yml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 9e0ccbb71a..baac5c3f1b 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,12 +21,10 @@ addons: sources: - kubuntu-backports - ubuntu-toolchain-r-test - - george-edison55-precise-backports packages: - build-essential - ccache - clang-3.5 - - cmake-3.2 - g++-4.8 - gcc-4.8 - genromfs @@ -44,6 +42,11 @@ before_install: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then pushd . && cd ~ + && wget http://www.cmake.org/files/v3.2/cmake-3.2.2-Linux-x86_64.sh -O /tmp/cmake_install.sh + && mkdir -p cmake + && sh /tmp/cmake_install.sh --exclude-subdir --prefix=${HOME}/cmake + && export PATH="${HOME}/cmake/bin:${PATH}" + && cmake --version && wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 && tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 && exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" From a81e23744112beda53892eb1f085cbfaf992a2d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 11:59:42 +0100 Subject: [PATCH 127/155] Travis CI: Simpler approach to CMake --- .travis.yml | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/.travis.yml b/.travis.yml index baac5c3f1b..3786602b09 100644 --- a/.travis.yml +++ b/.travis.yml @@ -21,10 +21,12 @@ addons: sources: - kubuntu-backports - ubuntu-toolchain-r-test + - george-edison55-precise-backports packages: - build-essential - ccache - clang-3.5 + - cmake - g++-4.8 - gcc-4.8 - genromfs @@ -42,11 +44,6 @@ before_install: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then pushd . && cd ~ - && wget http://www.cmake.org/files/v3.2/cmake-3.2.2-Linux-x86_64.sh -O /tmp/cmake_install.sh - && mkdir -p cmake - && sh /tmp/cmake_install.sh --exclude-subdir --prefix=${HOME}/cmake - && export PATH="${HOME}/cmake/bin:${PATH}" - && cmake --version && wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 && tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 && exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" From 5aaa45c62888f07c28b73909bc05acfdfbef65a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 12:07:18 +0100 Subject: [PATCH 128/155] Update README --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 7cb07907ff..eb33fff02e 100644 --- a/README.md +++ b/README.md @@ -4,7 +4,7 @@ [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) -This repository contains the PX4 Flight Core, with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones. +This repository contains the [PX4 Flight Core](http://px4.io), with the main applications located in the src/modules directory. It also contains the PX4 Drone Platform, which contains drivers and middleware to run drones. * Official Website: http://px4.io * License: BSD 3-clause (see [LICENSE.md](https://github.com/PX4/Firmware/blob/master/LICENSE.md)) @@ -32,7 +32,7 @@ Software in the Loop guide: Use software in the loop [to get started with the codebase](https://pixhawk.org/dev/simulation/native_sitl). Developer guide: -http://px4.io/dev/ +http://dev.px4.io Testing guide: http://px4.io/dev/unit_tests From ca052ac68a3a7a3fa6486512e2748448f03bc4de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 12:20:05 +0100 Subject: [PATCH 129/155] Vagrant: Upgrade CMake version --- Vagrantfile | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Vagrantfile b/Vagrantfile index cea1289500..d7e40a6da6 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -80,6 +80,8 @@ Vagrant.configure(2) do |config| # Ensure we start in the Firmware folder echo "cd /Firmware" >> ~/.bashrc # Install software + sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded -y + sudo add-apt-repository ppa:george-edison55/cmake-3.x -y sudo apt-get update sudo apt-get install -y build-essential ccache cmake clang-3.5 lldb-3.5 g++-4.8 gcc-4.8 genromfs libc6-i386 libncurses5-dev python-argparse python-empy python-serial s3cmd texinfo zlib1g-dev git-core zip gdb gcc-arm-linux-gnueabihf g++-arm-linux-gnueabihf pushd . From 06472861ce874da957eca0a83e28171af7052ef0 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sat, 24 Oct 2015 10:11:03 -0400 Subject: [PATCH 130/155] Added buzzer circuit breaker. --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 20 ++++++++++++++++++- .../systemlib/circuit_breaker_params.c | 15 +++++++++++++- 2 files changed, 33 insertions(+), 2 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index 55b36f8bb4..1a54e11809 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -116,6 +116,7 @@ #include #include +#include /* Check that tone alarm and HRT timers are different */ #if defined(TONE_ALARM_TIMER) && defined(HRT_TIMER) @@ -273,6 +274,8 @@ # define rDMAR REG(STM32_GTIM_DMAR_OFFSET) #endif +#define CBRK_BUZZER_KEY 782097 + class ToneAlarm : public device::CDev { public: @@ -288,6 +291,12 @@ public: return _tune_names[tune]; } + enum { + CBRK_OFF = 0, + CBRK_ON, + CBRK_UNINIT + }; + private: static const unsigned _tune_max = 1024 * 8; // be reasonable about user tunes const char *_default_tunes[TONE_NUMBER_OF_TUNES]; @@ -307,6 +316,7 @@ private: unsigned _octave; unsigned _silence_length; // if nonzero, silence before next note bool _repeat; // if true, tune restarts at end + int _cbrk; //if true, no audio output hrt_call _note_call; // HRT callout for note completion @@ -375,7 +385,8 @@ ToneAlarm::ToneAlarm() : _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), - _next(nullptr) + _next(nullptr), + _cbrk(CBRK_UNINIT) { // enable debug() calls //_debug_enabled = true; @@ -537,6 +548,13 @@ ToneAlarm::rest_duration(unsigned rest_length, unsigned dots) void ToneAlarm::start_note(unsigned note) { + // check if circuit breaker is enabled + if (_cbrk == CBRK_UNINIT) { + _cbrk = circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY); + } + + if (_cbrk != CBRK_OFF) { return; } + // compute the divisor unsigned divisor = note_to_divisor(note); diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index 36b35246db..003b73bb62 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -133,4 +133,17 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); * @max 240024 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); \ No newline at end of file +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + +/** + * Circuit breaker for disabling buzzer + * + * Setting this parameter to 782097 will disable the buzzer audio notification. + * + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 782097 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_BUZZER, 0); From e0d346e49d984068f0afca59d94c74b4d1ad7e27 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 13:43:07 +0100 Subject: [PATCH 131/155] Require CMake 3.2 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fb13d8283a..aae6c1f865 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -117,7 +117,7 @@ # #============================================================================= -cmake_minimum_required(VERSION 2.8 FATAL_ERROR) +cmake_minimum_required(VERSION 3.2 FATAL_ERROR) #============================================================================= # parameters From 6c3c18b7c94d39594ad9afae82a17bca4fd3045a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:35:20 +0100 Subject: [PATCH 132/155] Robustify toolchain string matching --- cmake/common/px4_base.cmake | 14 +-- .../Toolchain-posix-clang-native.cmake | 95 ------------------- 2 files changed, 4 insertions(+), 105 deletions(-) delete mode 100644 cmake/toolchains/Toolchain-posix-clang-native.cmake diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index bd9cf1f406..66ec86b848 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -512,7 +512,7 @@ function(px4_add_common_flags) list(APPEND warnings -Wframe-larger-than=1024) endif() - if (${CMAKE_C_COMPILER_ID} STREQUAL "Clang") + if (${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") # QuRT 6.4.X compiler identifies as Clang but does not support this option if (NOT ${OS} STREQUAL "qurt") list(APPEND warnings @@ -540,7 +540,7 @@ function(px4_add_common_flags) -fdata-sections ) - if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") + if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") list(APPEND optimization_flags -fno-strength-reduce -fno-builtin-printf @@ -554,7 +554,7 @@ function(px4_add_common_flags) -Wnested-externs ) - if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") + if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") list(APPEND c_warnings -Wold-style-declaration -Wmissing-parameter-type @@ -633,16 +633,10 @@ function(px4_add_common_flags) -DCONFIG_ARCH_BOARD_${board_config} ) - if (NOT ${CMAKE_C_COMPILER_ID} STREQUAL "Clang") - set(added_exe_link_flags - -Wl,--warn-common - -Wl,--gc-sections - ) - else() + set(added_exe_link_flags -Wl,--warn-common ) - endif() # output foreach(var ${inout_vars}) diff --git a/cmake/toolchains/Toolchain-posix-clang-native.cmake b/cmake/toolchains/Toolchain-posix-clang-native.cmake deleted file mode 100644 index 948928150f..0000000000 --- a/cmake/toolchains/Toolchain-posix-clang-native.cmake +++ /dev/null @@ -1,95 +0,0 @@ - -set(WARNINGS - -Wall - -Werror - -Wextra - -Wdouble-promotion - -Wshadow - -Wfloat-equal - -Wframe-larger-than=1024 - -Wpointer-arith - -Wlogical-op - -Wmissing-declarations - -Wno-unused-parameter - -Werror=format-security - -Werror=array-bounds - -Wfatal-errors - -Wformat=1 - -Werror=unused-but-set-variable - -Werror=unused-variable - -Werror=double-promotion - -Werror=reorder - -Werror=uninitialized - -Werror=init-self - -Werror=return-type - -Werror=deprecated - -Werror=unused-private-field - -Wno-packed - -Wno-frame-larger-than= - -Wno-varargs - #-Wcast-qual - generates spurious noreturn attribute warnings, - # try again later - #-Wconversion - would be nice, but too many "risky-but-safe" - # conversions in the code - #-Wcast-align - would help catch bad casts in some cases, - # but generates too many false positives - ) - -set(OPT_FLAGS - -Os -g3 - ) - -#============================================================================= -# c flags -# -set(C_WARNINGS - -Wbad-function-cast - -Wstrict-prototypes - -Wold-style-declaration - -Wmissing-parameter-type - -Wmissing-prototypes - -Wnested-externs - ) -set(C_FLAGS - -std=gnu99 - -fno-common - -fsanitize=address - -fno-omit-frame-pointer - ) - -#============================================================================= -# cxx flags -# -set(CXX_WARNINGS - -Wno-missing-field-initializers - -Wno-varargs - ) -set(CXX_FLAGS - -fno-exceptions - -fno-rtti - -std=gnu++0x - -fno-threadsafe-statics - -fsanitize=address - -fno-omit-frame-pointer - -DCONFIG_WCHAR_BUILTIN - -D__CUSTOM_FILE_IO__ - ) - -#============================================================================= -# ld flags -# -set(LD_FLAGS - -fsanitize=address - -Wl,--warn-common - -Wl,--gc-sections - ) - -#============================================================================= -# misc flags -# -set(VISIBILITY_FLAGS - -fvisibility=hidden - "-include ${CMAKE_SOURCE_DIR}/src/include/visibility.h" - ) -set(EXE_LINK_FLAGS) - From ba61f3f36c53919494375adfe73420844e58e0de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:36:58 +0100 Subject: [PATCH 133/155] Re-add missing flags --- cmake/common/px4_base.cmake | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 66ec86b848..26598fef70 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -633,10 +633,16 @@ function(px4_add_common_flags) -DCONFIG_ARCH_BOARD_${board_config} ) - + if (NOT ${CMAKE_C_COMPILER_ID} MATCHES ".*Clang.*") + set(added_exe_link_flags + -Wl,--warn-common + -Wl,--gc-sections + ) + else() set(added_exe_link_flags -Wl,--warn-common ) + endif() # output foreach(var ${inout_vars}) From f2b76a065c4a537e1351b91b525b09ede1bac3e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 14:47:21 +0100 Subject: [PATCH 134/155] Fix Mac OS by going back down with CMake version --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index aae6c1f865..686256f480 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -117,7 +117,9 @@ # #============================================================================= -cmake_minimum_required(VERSION 3.2 FATAL_ERROR) +# Warning: Changing this modifies CMake's internal workings +# and leads to wrong toolchain detection +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) #============================================================================= # parameters From 318144ee8bbeca36d67a1741ffe4179b2c80eeac Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 11:18:14 -0400 Subject: [PATCH 135/155] Changed sitl targets to be easier to call from make. --- Makefile | 3 ++- src/firmware/posix/CMakeLists.txt | 24 ++++++++++++++++++++++-- 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/Makefile b/Makefile index 2a0e843f94..5e68ee3c42 100644 --- a/Makefile +++ b/Makefile @@ -161,7 +161,8 @@ clean: # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ - run_sitl config + run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ + jmavsim_gdb jmavsim_lldb $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 88028d176e..59398c2668 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -24,12 +24,32 @@ else() ) endif() -add_custom_target(run_sitl +add_custom_target(run_config COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" "${config_sitl_viewer}" "${CMAKE_BINARY_DIR}" WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} USES_TERMINAL ) -add_dependencies(run_sitl mainapp) +add_dependencies(run_config mainapp) + +foreach(viewer jmavsim gazebo) + foreach(debugger none gdb lldb) + message(STATUS "viewer: ${viewer}") + message(STATUS "debugger: ${debugger}") + if (debugger STREQUAL "none") + set(_targ_name "${viewer}") + else() + set(_targ_name "${viewer}_${debugger}") + endif() + add_custom_target(${_targ_name} + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" + "${debugger}" + "${viewer}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL + ) + add_dependencies(${_targ_name} mainapp) + endforeach() +endforeach() # vim: set noet ft=cmake fenc=utf-8 ff=unix : From aeb7bff9c994e185e8e732ba63af37f39673f03e Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 11:22:23 -0400 Subject: [PATCH 136/155] Removed cmake debug messages. --- src/firmware/posix/CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 59398c2668..3417f49d18 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -34,8 +34,6 @@ add_dependencies(run_config mainapp) foreach(viewer jmavsim gazebo) foreach(debugger none gdb lldb) - message(STATUS "viewer: ${viewer}") - message(STATUS "debugger: ${debugger}") if (debugger STREQUAL "none") set(_targ_name "${viewer}") else() From 21cf3e0cce6e1076c63d09a097355a89550a439d Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 10:57:24 -0400 Subject: [PATCH 137/155] This kills some annoying processes that might still be around. --- Tools/sitl_run.sh | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 3f7dc1a883..614ecd3fba 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -17,6 +17,12 @@ then exit 1 fi +# kill process names that might stil +# be running from last time +pkill java +pkill mainapp + + cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit @@ -28,7 +34,6 @@ then ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & SIM_PID=echo $! - cd ../.. elif [ "$3" == "gazebo" ] then if [ -x "$(command -v gazebo)" ] From c520a678a6f2b9e384dc79644ec8a5150f83d783 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 11:47:46 -0400 Subject: [PATCH 138/155] More intelligent jmavsim killing. --- Tools/sitl_run.sh | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 614ecd3fba..9be388e610 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -19,9 +19,8 @@ fi # kill process names that might stil # be running from last time -pkill java pkill mainapp - +pkill `jps | grep Simulator | cut -d" " -f1` cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit From a688c894607718f8ae54f97af33e955bf9db33d2 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 11:52:11 -0400 Subject: [PATCH 139/155] Fixed kill command for java. --- Tools/sitl_run.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 9be388e610..98a50bfb04 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -20,7 +20,7 @@ fi # kill process names that might stil # be running from last time pkill mainapp -pkill `jps | grep Simulator | cut -d" " -f1` +kill -9 `jps | grep Simulator | cut -d" " -f1` cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit From a23ba97f3f40b6ec6e5ddd603e9daf371e2e1c8a Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 12:25:15 -0400 Subject: [PATCH 140/155] sitl bash fix for pid recording --- Tools/sitl_run.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 98a50bfb04..943cf05cda 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -38,7 +38,7 @@ then if [ -x "$(command -v gazebo)" ] then gazebo ${SITL_GAZEBO_PATH}/worlds/iris.world & - SIM_PID=echo $! + SIM_PID=`echo $!` else echo "You need to have gazebo simulator installed!" exit 1 From f831cc6cfa5aed69dbe913a784ff87eb19a29708 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 12:33:48 -0400 Subject: [PATCH 141/155] Don't kill if jmavsim not found. --- Tools/sitl_run.sh | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 943cf05cda..9d950dc946 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -20,7 +20,12 @@ fi # kill process names that might stil # be running from last time pkill mainapp -kill -9 `jps | grep Simulator | cut -d" " -f1` +jmavsim_pid=`jps | grep Simulator | cut -d" " -f1` +echo jmavsim_pid=\"${jmavsim_pid}\" +if [ -n "$jmavsim_pid" ] +then + kill $jmavsim_pid +fi cp Tools/posix_lldbinit $build_path/src/firmware/posix/.lldbinit cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit From 7d9c6592fd4d4798be8527bfd603629191b5f3ef Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 12:34:19 -0400 Subject: [PATCH 142/155] Remove bash debugging. --- Tools/sitl_run.sh | 1 - 1 file changed, 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 9d950dc946..b1a1351d58 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -21,7 +21,6 @@ fi # be running from last time pkill mainapp jmavsim_pid=`jps | grep Simulator | cut -d" " -f1` -echo jmavsim_pid=\"${jmavsim_pid}\" if [ -n "$jmavsim_pid" ] then kill $jmavsim_pid From 7d5a9c5baa4a8e7ea5ee268dbca152a0db2dac5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 18:20:53 +0100 Subject: [PATCH 143/155] Added sitl_gazebo as submodule --- .gitmodules | 3 +++ Tools/sitl_gazebo | 1 + 2 files changed, 4 insertions(+) create mode 160000 Tools/sitl_gazebo diff --git a/.gitmodules b/.gitmodules index 50f26b3a99..178cf86771 100644 --- a/.gitmodules +++ b/.gitmodules @@ -19,6 +19,9 @@ [submodule "Tools/jMAVSim"] path = Tools/jMAVSim url = https://github.com/PX4/jMAVSim.git +[submodule "Tools/sitl_gazebo"] + path = Tools/sitl_gazebo + url = https://github.com/PX4/sitl_gazebo.git [submodule "unittests/googletest"] path = unittests/googletest url = https://github.com/google/googletest.git diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo new file mode 160000 index 0000000000..d362e661b4 --- /dev/null +++ b/Tools/sitl_gazebo @@ -0,0 +1 @@ +Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe From 5df1f84a533a8eae92844acbc4dcb4fd869bd760 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 18:25:21 +0100 Subject: [PATCH 144/155] Add sitl_gazebo as submodule --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 686256f480..23616c2db2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -230,6 +230,7 @@ px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan") px4_add_git_submodule(TARGET git_nuttx PATH "NuttX") px4_add_git_submodule(TARGET git_dspal PATH "src/lib/dspal") px4_add_git_submodule(TARGET git_jmavsim PATH "Tools/jMAVSim") +px4_add_git_submodule(TARGET git_gazebo PATH "Tools/sitl_gazebo") add_custom_target(submodule_clean WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} From 9cbcca1f4943022df795e6e0c6b2943eec479996 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 18:25:37 +0100 Subject: [PATCH 145/155] Fix Gazebo handling --- Tools/sitl_run.sh | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index b1a1351d58..68637cc1bd 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -4,6 +4,7 @@ rc_script=$1 debugger=$2 program=$3 build_path=$4 +curr_dir=`pwd` echo SITL ARGS echo rc_script: $rc_script @@ -41,7 +42,17 @@ elif [ "$3" == "gazebo" ] then if [ -x "$(command -v gazebo)" ] then - gazebo ${SITL_GAZEBO_PATH}/worlds/iris.world & + # Set the plugin path so Gazebo finds our model and sim + GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build + # Set the model path so Gazebo finds the airframes + GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models + # Disable online model lookup since this is quite experimental and unstable + GAZEBO_MODEL_DATABASE_URI="" + mkdir -p Tools/sitl_gazebo/Build + cd Tools/sitl_gazebo/Build + cmake .. + make -j4 + gazebo ../worlds/iris.world & SIM_PID=`echo $!` else echo "You need to have gazebo simulator installed!" From 8421742906fdad8a0606573ffcf2e2a968dc4247 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Oct 2015 18:25:53 +0100 Subject: [PATCH 146/155] Add Gazebo submodule as simulator dependency --- src/modules/simulator/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index de452c5de4..0a7e221086 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -47,6 +47,8 @@ px4_add_module( ${SIMULATOR_SRCS} DEPENDS platforms__common + git_jmavsim + git_gazebo ) # vim: set noet ft=cmake fenc=utf-8 ff=unix : From fb3465c11713716ce0c181213319f444b9d4e0b0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 25 Oct 2015 12:28:00 -0400 Subject: [PATCH 147/155] Makefile fix /bin/shs ninja check --- Makefile | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Makefile b/Makefile index 5e68ee3c42..3a0155bd64 100644 --- a/Makefile +++ b/Makefile @@ -67,7 +67,6 @@ all: px4fmu-v2_default ARGS := $(wordlist 2,$(words $(MAKECMDGOALS)),$(MAKECMDGOALS)) j ?= 4 -# disable ninja by default for now because it hides upload progress NINJA_BUILD := $(shell ninja --version 2>/dev/null) ifdef NINJA_BUILD PX4_CMAKE_GENERATOR ?= "Ninja" @@ -89,7 +88,7 @@ endif # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build -+@if [ $(PX4_CMAKE_GENERATOR) == "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi ++@if [ $(PX4_CMAKE_GENERATOR) = "Ninja" ] && [ -e $(PWD)/build_$@/Makefile ]; then rm -rf $(PWD)/build_$@; fi +@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef From e2ec4af127bf5e59376689f76828bb6c7e174934 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Sat, 24 Oct 2015 10:01:53 -0700 Subject: [PATCH 148/155] Switched Vagrant to use static IP MacOS was having isues with DHCP for NFS shared folder Signed-off-by: Mark Charlebois --- Vagrantfile | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/Vagrantfile b/Vagrantfile index d7e40a6da6..dfa4da4d8b 100644 --- a/Vagrantfile +++ b/Vagrantfile @@ -26,16 +26,13 @@ Vagrant.configure(2) do |config| # Create a private network, which allows host-only access to the machine # using a specific IP. - # config.vm.network "private_network", ip: "192.168.33.10" + config.vm.network "private_network", ip: "192.168.33.10" # Create a public network, which generally matched to bridged network. # Bridged networks make the machine appear as another physical device on # your network. # config.vm.network "public_network" - # Virtualbox requires a private network to use NFS - config.vm.network "private_network", type: "dhcp" - # Share an additional folder to the guest VM. The first argument is # the path on the host to the actual folder. The second argument is # the path on the guest to mount the folder. And the optional third From dab48553c7ce628d528803246c62984780d92a56 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 25 Oct 2015 19:25:18 -0400 Subject: [PATCH 149/155] Added export to gazebo variables. --- Tools/sitl_run.sh | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 68637cc1bd..f13ed94509 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -10,7 +10,7 @@ echo SITL ARGS echo rc_script: $rc_script echo debugger: $debugger echo program: $program -echo build_path: $buid_path +echo build_path: $build_path if [ "$#" != 4 ] then @@ -20,6 +20,7 @@ fi # kill process names that might stil # be running from last time +pkill gazebo pkill mainapp jmavsim_pid=`jps | grep Simulator | cut -d" " -f1` if [ -n "$jmavsim_pid" ] @@ -43,11 +44,12 @@ then if [ -x "$(command -v gazebo)" ] then # Set the plugin path so Gazebo finds our model and sim - GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build + export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$curr_dir/Tools/sitl_gazebo/Build # Set the model path so Gazebo finds the airframes - GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models + export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$curr_dir/Tools/sitl_gazebo/models # Disable online model lookup since this is quite experimental and unstable - GAZEBO_MODEL_DATABASE_URI="" + export GAZEBO_MODEL_DATABASE_URI="" + export SITL_GAZEBO_PATH=$curr_dir/Tools/sitl_gazebo mkdir -p Tools/sitl_gazebo/Build cd Tools/sitl_gazebo/Build cmake .. From 529d9b4d3a08dfafdd5a998bdfed4edb33424e6e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 10:19:30 +0100 Subject: [PATCH 150/155] Check and enforce CMake version --- CMakeLists.txt | 1 + Makefile | 14 +++++++++++++- Tools/check_cmake.sh | 14 ++++++++++++++ 3 files changed, 28 insertions(+), 1 deletion(-) create mode 100755 Tools/check_cmake.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index 23616c2db2..05a3cd59b3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -186,6 +186,7 @@ project(px4 CXX C ASM) if (NOT ${CMAKE_VERSION} VERSION_LESS 3.0.0) cmake_policy(SET CMP0045 NEW) # error on non-existent target in get prop cmake_policy(SET CMP0046 NEW) # no non-existent targets as dependencies + cmake_policy(SET CMP0025 OLD) # still report AppleClang as Clang endif() if (NOT ${CMAKE_VERSION} VERSION_LESS 3.1.0) cmake_policy(SET CMP0054 NEW) # don't dereference quoted variables diff --git a/Makefile b/Makefile index 3a0155bd64..e67c21e1af 100644 --- a/Makefile +++ b/Makefile @@ -36,7 +36,19 @@ # We depend on our submodules, so we have to prevent attempts to # compile without it being present. ifeq ($(wildcard .git),) - $(error YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING.) + $(error YOU HAVE TO USE GIT TO DOWNLOAD THIS REPOSITORY. ABORTING.) +endif + +CMAKE_VER := $(shell Tools/check_cmake.sh; echo $$?) +ifneq ($(CMAKE_VER),0) + $(warning Not a valid CMake version or CMake not installed.) + $(warning On Ubuntu, install or upgrade via:) + $(warning ) + $(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y) + $(warning sudo apt-get update) + $(warning sudo apt-get install cmake) + $(warning ) + $(error Fatal) endif # Help diff --git a/Tools/check_cmake.sh b/Tools/check_cmake.sh new file mode 100755 index 0000000000..423b790dbd --- /dev/null +++ b/Tools/check_cmake.sh @@ -0,0 +1,14 @@ +#!/bin/bash +cmake_ver=`cmake --version` + +if [[ $cmake_ver == "" ]] +then + exit 1; +fi + +if [[ $cmake_ver == *"2.8"* ]] || [[ $cmake_ver == *"2.9"* ]] || [[ $cmake_ver == *"3.0"* ]] || [[ $cmake_ver == *"3.1"* ]] +then + exit 1; +fi + +exit 0; From 327481d749f2042fab121aa3949ca2eb995c17fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 11:58:11 +0100 Subject: [PATCH 151/155] Fix EKF frame size flag use --- src/modules/ekf_att_pos_estimator/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/CMakeLists.txt b/src/modules/ekf_att_pos_estimator/CMakeLists.txt index fc7fefd2bf..13eaeb6d2b 100644 --- a/src/modules/ekf_att_pos_estimator/CMakeLists.txt +++ b/src/modules/ekf_att_pos_estimator/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS ) -if(NOT ${OS} STREQUAL "qurt") +if(${OS} STREQUAL "nuttx") list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400) endif() From 0c3f51944500b713eebba524e2a072b77fb47a65 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 26 Oct 2015 12:18:24 +0100 Subject: [PATCH 152/155] renamed SITL startup scripts --- posix-configs/SITL/init/{rcS_iris_gazebo => rcS_gazebo} | 0 posix-configs/SITL/init/{rcS => rcS_jmavsim} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename posix-configs/SITL/init/{rcS_iris_gazebo => rcS_gazebo} (100%) rename posix-configs/SITL/init/{rcS => rcS_jmavsim} (100%) diff --git a/posix-configs/SITL/init/rcS_iris_gazebo b/posix-configs/SITL/init/rcS_gazebo similarity index 100% rename from posix-configs/SITL/init/rcS_iris_gazebo rename to posix-configs/SITL/init/rcS_gazebo diff --git a/posix-configs/SITL/init/rcS b/posix-configs/SITL/init/rcS_jmavsim similarity index 100% rename from posix-configs/SITL/init/rcS rename to posix-configs/SITL/init/rcS_jmavsim From 1767acd1e9966d198916a84a1421f630b229d241 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 26 Oct 2015 12:23:22 +0100 Subject: [PATCH 153/155] use correct startup scipt depending on simulator --- Tools/sitl_run.sh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index f13ed94509..fff0080ca9 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -68,12 +68,12 @@ touch rootfs/eeprom/parameters # Start Java simulator if [ "$debugger" == "lldb" ] then - lldb -- mainapp ../../../../$rc_script + lldb -- mainapp ../../../../${rc_script}_${program} elif [ "$debugger" == "gdb" ] then - gdb --args mainapp ../../../../$rc_script + gdb --args mainapp ../../../../${rc_script}_${program} else - ./mainapp ../../../../$rc_script + ./mainapp ../../../../${rc_script}_${program} fi if [ "$3" == "jmavsim" ] From 1e7da5064f726529be58c561d722b7e9733857f5 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 26 Oct 2015 15:59:36 -0400 Subject: [PATCH 154/155] Fixed LPE bug for accel bias estimation. --- .../local_position_estimator/BlockLocalPositionEstimator.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 08cf51fb5c..f3295e1fc9 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -749,8 +749,7 @@ void BlockLocalPositionEstimator::predict() if (_integrate.get() && _sub_att.get().R_valid) { Matrix3f R_att(_sub_att.get().R); Vector3f a(_sub_sensor.get().accelerometer_m_s2); - Vector3f b(_x(X_bx), _x(X_by), _x(X_bz)); - _u = R_att * (a - b); + _u = R_att * a; _u(U_az) += 9.81f; // add g } else { From 4241e526aac9fad483949ce6b7cf03597acbd9b6 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 26 Oct 2015 16:03:22 -0400 Subject: [PATCH 155/155] Updated sitl scripts for LPE. --- Tools/sitl_run.sh | 2 +- posix-configs/SITL/init/rcS_lpe_gazebo | 66 +++++++++++++++++++ .../SITL/init/{rcS_lpe => rcS_lpe_jmavsim} | 0 3 files changed, 67 insertions(+), 1 deletion(-) create mode 100644 posix-configs/SITL/init/rcS_lpe_gazebo rename posix-configs/SITL/init/{rcS_lpe => rcS_lpe_jmavsim} (100%) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index fff0080ca9..6eb69eb46b 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -38,7 +38,7 @@ then cd Tools/jMAVSim ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & - SIM_PID=echo $! + SIM_PID=`echo $!` elif [ "$3" == "gazebo" ] then if [ -x "$(command -v gazebo)" ] diff --git a/posix-configs/SITL/init/rcS_lpe_gazebo b/posix-configs/SITL/init/rcS_lpe_gazebo new file mode 100644 index 0000000000..6093991c13 --- /dev/null +++ b/posix-configs/SITL/init/rcS_lpe_gazebo @@ -0,0 +1,66 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.3 +param set MC_ROLLRATE_P 0.3 +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293760 +param set CAL_ACC0_ID 1376256 +param set CAL_ACC1_ID 1310720 +param set CAL_MAG0_ID 196608 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 1 +param set CAL_ACC0_YOFF 2 +param set CAL_ACC0_ZOFF 3 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.15 +param set MPC_XY_VEL_P 0.05 +param set MPC_XY_VEL_D 0.005 +param set MPC_XY_FF 0.1 +param set SENS_BOARD_ROT 8 + +# changes for LPE +param set COM_RC_IN_MODE 1 +param set LPE_BETA_MAX 10000 + +rgbled start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_w.main.mix +mavlink start -u 14556 -r 2000000 +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a + +# start LPE at end, when we know it is ok to init sensors +sleep 5 +local_position_estimator start diff --git a/posix-configs/SITL/init/rcS_lpe b/posix-configs/SITL/init/rcS_lpe_jmavsim similarity index 100% rename from posix-configs/SITL/init/rcS_lpe rename to posix-configs/SITL/init/rcS_lpe_jmavsim