diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-26 12:21:20 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-06-26 12:21:20 +0200 |
commit | 2eb3077f4642e6a7d92d653b292418d2afa592a1 (patch) | |
tree | 64ea0c2fe07c70d2a7ea6bb4168f06db90dd3ba6 /src | |
parent | 3e0c318311a05c323f68177d01f461066b06b76d (diff) | |
parent | 4f560f729ec1f40427401119a0f17d9d0e9843f4 (diff) | |
download | px4-firmware-2eb3077f4642e6a7d92d653b292418d2afa592a1.tar.gz px4-firmware-2eb3077f4642e6a7d92d653b292418d2afa592a1.tar.bz2 px4-firmware-2eb3077f4642e6a7d92d653b292418d2afa592a1.zip |
Merge pull request #1073 from PX4/launchdetectorreset
FW: reset attitude control integrals in launchdetection mode (when waiting for takeoff)
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 11 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_attitude_setpoint.h | 2 |
3 files changed, 23 insertions, 4 deletions
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 178b590ae..3cd4ce928 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -707,14 +707,21 @@ FixedwingAttitudeControl::task_main() float throttle_sp = 0.0f; if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; throttle_sp = _att_sp.thrust; /* reset integrals where needed */ - if (_att_sp.roll_reset_integral) + if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); - + } + if (_att_sp.pitch_reset_integral) { + _pitch_ctrl.reset_integrator(); + } + if (_att_sp.yaw_reset_integral) { + _yaw_ctrl.reset_integrator(); + } } else { /* * Scale down roll and pitch as the setpoints are radians diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 116d3cc63..0e065211e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -841,6 +841,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* current waypoint (the one currently heading for) */ math::Vector<2> curr_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); + /* Initialize attitude controller integrator reset flags to 0 */ + _att_sp.roll_reset_integral = false; + _att_sp.pitch_reset_integral = false; + _att_sp.yaw_reset_integral = false; /* previous waypoint */ math::Vector<2> prev_wp; @@ -1020,15 +1024,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { /* Perform launch detection */ -// warnx("Launch detection running"); if(!launch_detected) { //do not do further checks once a launch was detected if (launchDetector.launchDetectionEnabled()) { static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { -// warnx("Launch detection running"); mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); last_sent = hrt_absolute_time(); } + + /* Tell the attitude controller to stop integrating while we are waiting + * for the launch */ + _att_sp.roll_reset_integral = true; + _att_sp.pitch_reset_integral = true; + _att_sp.yaw_reset_integral = true; + + /* Detect launch */ launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index d526a8ff2..8446e9c6e 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s { float thrust; /**< Thrust in Newton the power system should generate */ bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */ + bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */ }; |