From a27a6acb54756600b0504f25efdadc521b12101d Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Fri, 20 Nov 2015 22:29:40 +1100 Subject: [PATCH 1/7] Don't call param_opcode unless there are dirty nodes [resolves PX4/Firmware#3160] --- src/modules/uavcan/uavcan_servers.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index ca5bcede66..961002c2e2 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -523,8 +523,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) switch (command_id) { case 1: { // Param save request - _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; - param_opcode(get_next_dirty_node_id(1)); + int node_id; + node_id = get_next_dirty_node_id(1); + if (node_id < 128) { + _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; + param_opcode(node_id); + } break; } case 2: { From ff5db6b1be3c0fdd27eced5f29e51b5c58b8fbb4 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 13:10:46 +0100 Subject: [PATCH 2/7] fixed alt hold bug --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 +++++++--- 1 file changed, 7 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 c36ba5bfc6..1b261bef81 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -744,20 +744,24 @@ MulticopterPositionControl::control_manual(float dt) /* check for pos. hold */ 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)) + if (!_alt_hold_engaged) { - _alt_hold_engaged = true; + if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { + _alt_hold_engaged = true; + } else { + _alt_hold_engaged = false; + } } } else { _alt_hold_engaged = false; - _pos_sp(2) = _pos(2); } /* 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); + _pos_sp(2) = _pos(2); } } } From 5303a63da0ab3bf54d7a76f229bf757c47ebbead Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 13:16:42 +0100 Subject: [PATCH 3/7] mc_pos_control: code formatting --- .../mc_pos_control/mc_pos_control_main.cpp | 138 ++++++++++-------- 1 file changed, 76 insertions(+), 62 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 1b261bef81..a26a65d468 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -279,8 +279,8 @@ private: */ void control_offboard(float dt); - bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + bool cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res); /** * Set position setpoint for AUTO @@ -321,7 +321,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _control_task(-1), _mavlink_fd(-1), -/* subscriptions */ + /* subscriptions */ _ctrl_state_sub(-1), _att_sp_sub(-1), _control_mode_sub(-1), @@ -332,7 +332,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _pos_sp_triplet_sub(-1), _global_vel_sp_sub(-1), -/* publications */ + /* publications */ _att_sp_pub(nullptr), _local_pos_sp_pub(nullptr), _global_vel_sp_pub(nullptr), @@ -517,7 +517,7 @@ MulticopterPositionControl::parameters_update(bool force) _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); - param_get(_params_handles.mc_att_yaw_p,&v); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -626,9 +626,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) * _vel_sp(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) * _vel_sp(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)); } } @@ -703,7 +703,8 @@ MulticopterPositionControl::control_manual(float dt) /* _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 + 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 @@ -711,20 +712,20 @@ MulticopterPositionControl::control_manual(float dt) */ /* horizontal axes */ - if (_control_mode.flag_control_position_enabled) - { + if (_control_mode.flag_control_position_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) - { + if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { if (!_pos_hold_engaged) { - if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) { + if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy + && fabsf(_vel(1)) < _params.hold_max_xy)) { _pos_hold_engaged = true; + } else { _pos_hold_engaged = false; } } - } - else { + + } else { _pos_hold_engaged = false; } @@ -739,21 +740,19 @@ MulticopterPositionControl::control_manual(float dt) } /* vertical axis */ - if (_control_mode.flag_control_altitude_enabled) - { + if (_control_mode.flag_control_altitude_enabled) { /* check for pos. hold */ - if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) - { - if (!_alt_hold_engaged) - { + if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) { + if (!_alt_hold_engaged) { if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) { _alt_hold_engaged = true; + } else { _alt_hold_engaged = false; } } - } - else { + + } else { _alt_hold_engaged = false; } @@ -781,6 +780,7 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; + } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -795,6 +795,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_pos_sp_triplet.current.yaw_valid) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } @@ -802,6 +803,7 @@ MulticopterPositionControl::control_offboard(float dt) if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { /* Control altitude */ _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -811,6 +813,7 @@ MulticopterPositionControl::control_offboard(float dt) _run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */ } + } else { reset_pos_sp(); reset_alt_sp(); @@ -818,8 +821,8 @@ MulticopterPositionControl::control_offboard(float dt) } bool -MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, - const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res) { /* project center of sphere on line */ /* normalized AB */ @@ -854,13 +857,14 @@ void MulticopterPositionControl::control_auto(float dt) //Poll position setpoint bool updated; orb_check(_pos_sp_triplet_sub, &updated); + if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); //Make sure that the position setpoint is valid if (!PX4_ISFINITE(_pos_sp_triplet.current.lat) || - !PX4_ISFINITE(_pos_sp_triplet.current.lon) || - !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { + !PX4_ISFINITE(_pos_sp_triplet.current.lon) || + !PX4_ISFINITE(_pos_sp_triplet.current.alt)) { _pos_sp_triplet.current.valid = false; } } @@ -880,21 +884,21 @@ void MulticopterPositionControl::control_auto(float dt) curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); if (PX4_ISFINITE(curr_sp(0)) && - PX4_ISFINITE(curr_sp(1)) && - PX4_ISFINITE(curr_sp(2))) { + PX4_ISFINITE(curr_sp(1)) && + PX4_ISFINITE(curr_sp(2))) { current_setpoint_valid = true; } } if (_pos_sp_triplet.previous.valid) { map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp.data[0], &prev_sp.data[1]); + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if (PX4_ISFINITE(prev_sp(0)) && - PX4_ISFINITE(prev_sp(1)) && - PX4_ISFINITE(prev_sp(2))) { + PX4_ISFINITE(prev_sp(1)) && + PX4_ISFINITE(prev_sp(2))) { previous_setpoint_valid = true; } } @@ -924,14 +928,15 @@ void MulticopterPositionControl::control_auto(float dt) math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); + if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than unit radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ if (_pos_sp_triplet.next.valid) { math::Vector<3> next_sp; map_projection_project(&_ref_pos, - _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, - &next_sp.data[0], &next_sp.data[1]); + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); if ((next_sp - curr_sp).length() > MIN_DIST) { @@ -949,6 +954,7 @@ void MulticopterPositionControl::control_auto(float dt) if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than unit radius, limit it */ if (curr_next_s_len > 1.0f) { cos_a_curr_next /= curr_next_s_len; @@ -956,8 +962,8 @@ void MulticopterPositionControl::control_auto(float dt) /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * - (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); pos_sp_s += pos_ff; } } @@ -965,6 +971,7 @@ void MulticopterPositionControl::control_auto(float dt) } else { bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); + if (near) { /* unit sphere crosses trajectory */ @@ -992,6 +999,7 @@ void MulticopterPositionControl::control_auto(float dt) /* difference between current and desired position setpoints, 1 = max speed */ math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } @@ -1149,7 +1157,8 @@ MulticopterPositionControl::task_main() control_auto(dt); } - if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid + && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { /* idle state, don't run controller and set zero thrust */ R.identity(); memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); @@ -1182,18 +1191,20 @@ MulticopterPositionControl::task_main() } /* make sure velocity setpoint is saturated in xy*/ - float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + - _vel_sp(1)*_vel_sp(1)); - if (vel_norm_xy > _params.vel_max(0)) { + float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + + _vel_sp(1) * _vel_sp(1)); + + if (vel_norm_xy > _params.vel_max(0)) { /* note assumes vel_max(0) == vel_max(1) */ - _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; - _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + _vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy; + _vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy; } /* make sure velocity setpoint is saturated in z*/ - float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { - _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; + _vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z; } if (!_control_mode.flag_control_position_enabled) { @@ -1214,7 +1225,8 @@ MulticopterPositionControl::task_main() } /* 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) { + 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; } @@ -1304,7 +1316,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; @@ -1514,6 +1526,7 @@ MulticopterPositionControl::task_main() /* publish local position setpoint */ if (_local_pos_sp_pub != nullptr) { orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp); + } else { _local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp); } @@ -1528,7 +1541,7 @@ MulticopterPositionControl::task_main() } /* generate attitude setpoint from manual controls */ - if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { + if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { /* reset yaw setpoint to current position if needed */ if (reset_yaw_sp) { @@ -1537,8 +1550,7 @@ MulticopterPositionControl::task_main() } /* do not move yaw while arming */ - else if (_manual.z > 0.1f) - { + else if (_manual.z > 0.1f) { const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; @@ -1569,8 +1581,8 @@ MulticopterPositionControl::task_main() } /* construct attitude setpoint rotation matrix */ - math::Matrix<3,3> R_sp; - R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); + math::Matrix<3, 3> R_sp; + R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); /* copy quaternion setpoint to attitude setpoint topic */ @@ -1578,8 +1590,8 @@ MulticopterPositionControl::task_main() q_sp.from_dcm(R_sp); memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d)); _att_sp.timestamp = hrt_absolute_time(); - } - else { + + } else { reset_yaw_sp = true; } @@ -1591,17 +1603,19 @@ MulticopterPositionControl::task_main() * in this case the attitude setpoint is published by the mavlink app */ if (!(_control_mode.flag_control_offboard_enabled && - !(_control_mode.flag_control_position_enabled || - _control_mode.flag_control_velocity_enabled))) { + !(_control_mode.flag_control_position_enabled || + _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + } else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ - reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled; + reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled + && !_control_mode.flag_control_climb_rate_enabled; } mavlink_log_info(_mavlink_fd, "[mpc] stopped"); @@ -1616,11 +1630,11 @@ MulticopterPositionControl::start() /* start the task */ _control_task = px4_task_spawn_cmd("mc_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 1500, - (px4_main_t)&MulticopterPositionControl::task_main_trampoline, - nullptr); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 1500, + (px4_main_t)&MulticopterPositionControl::task_main_trampoline, + nullptr); if (_control_task < 0) { warn("task start failed"); From 442c0d47ef191deedf3c269fc9b872341ab661df Mon Sep 17 00:00:00 2001 From: Ben Dyer Date: Fri, 20 Nov 2015 23:32:54 +1100 Subject: [PATCH 4/7] Fix off-by-one error in parameter count [resolves PX4/Firmware#3162] --- src/modules/uavcan/uavcan_servers.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index ca5bcede66..19316a8b8e 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -575,7 +575,8 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult Date: Fri, 20 Nov 2015 14:58:29 +0100 Subject: [PATCH 5/7] ekf: use airspeed directly, the other thing was just wrong --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 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 49f08f3cab..44c7b3e434 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 @@ -894,8 +894,10 @@ void AttitudePositionEstimatorEKF::publishControlState() _ctrl_state.q[3] = _ekf->states[3]; /* Airspeed (Groundspeed - Windspeed) */ - _ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); - + //_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2)); + // the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL + // and in outdoor tests + _ctrl_state.airspeed = _airspeed.true_airspeed_m_s; /* Attitude Rates */ _ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt; _ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt; From 3337eb8f1a1935452b4d7579ad3987a19d6bcd6a Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 20 Nov 2015 16:09:49 +0100 Subject: [PATCH 6/7] unsubscribe from topic to prevent overflow in device handler list --- Tools/generate_listener.py | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index e8cd1ae114..f097cb741b 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -200,6 +200,7 @@ for index,m in enumerate(messages[1:]): print("\t} else {") print("\t\t printf(\" Topic did not match any known topics\\n\");") print("\t}") +print("\t\torb_unsubscribe(sub);") print("\t return 0;") print("}") From 275761f3d516d1cbcd7e9f33a6141681364a775b Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 20 Nov 2015 23:32:53 +0100 Subject: [PATCH 7/7] properly assign struct member --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 373e7505b6..d27f81291e 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -474,8 +474,9 @@ PWMSim::task_main() } /* do mixing */ - actuator_outputs_s outputs; + actuator_outputs_s outputs = {}; num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); + outputs.noutputs = num_outputs; outputs.timestamp = hrt_absolute_time(); /* disable unused ports by setting their output to NaN */