dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Always check reset flags in auto. Only set reset flags to true when required. Do not limit hor/vert setpoint acceleration when setpoints are reset.

This commit is contained in:
devbharat 2016-11-15 13:56:09 +01:00 committed by Lorenz Meier
parent d720cbe189
commit 59c1dd7183
2 changed files with 120 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

154
Firmware.sublime-project
View File

@ -1,8 +1,50 @@
{
"build_systems":
[
{
"cmd":
[
"make"
],
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"name": "PX4: make all",
"shell": true,
"working_dir": "${project_path}"
},
{
"cmd":
[
"make upload px4fmu-v2_default -j8"
],
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"name": "PX4: make and upload",
"shell": true,
"working_dir": "${project_path}"
},
{
"cmd":
[
"make posix"
],
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"name": "PX4: make posix",
"shell": true,
"working_dir": "${project_path}"
},
{
"cmd":
[
"make upload mindpx-v2_default -j8"
],
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"name": "MindPX_V2: make and upload",
"shell": true,
"working_dir": "${project_path}"
}
],
"folders":
[
{
"path": ".",
"file_exclude_patterns":
[
"*.o",
@ -26,57 +68,87 @@
".settings",
"nuttx/arch/arm/src/board",
"nuttx/arch/arm/src/chip"
]
],
"path": "."
},
{
"path": "build_posix_sitl_default"
},
{
"path": "build_px4fmu-v2_default"
},
{
"path": "build_px4fmu-v4_default"
},
{
"path": "cmake"
},
{
"path": "Debug"
},
{
"path": "Documentation"
},
{
"path": "Images"
},
{
"path": "integrationtests"
},
{
"path": "launch"
},
{
"path": "mavlink"
},
{
"path": "misc"
},
{
"path": "msg"
},
{
"path": "NuttX"
},
{
"path": "nuttx-configs"
},
{
"path": "posix-configs"
},
{
"path": "ROMFS"
},
{
"path": "src"
},
{
"path": "test_data"
},
{
"path": "Tools"
},
{
"path": "unittests"
}
],
"settings":
{
"tab_size": 8,
"translate_tabs_to_spaces": false,
"highlight_line": true,
"AStyleFormatter":
{
"options_c":
{
"use_only_additional_options": true,
"additional_options_file": "${project_path}/Tools/astylerc"
"additional_options_file": "${project_path}/Tools/astylerc",
"use_only_additional_options": true
},
"options_c++":
{
"use_only_additional_options": true,
"additional_options_file": "${project_path}/Tools/astylerc"
"additional_options_file": "${project_path}/Tools/astylerc",
"use_only_additional_options": true
}
}
},
"build_systems":
[
{
"name": "PX4: make all",
"working_dir": "${project_path}",
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"cmd": ["make"],
"shell": true
},
{
"name": "PX4: make and upload",
"working_dir": "${project_path}",
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"cmd": ["make upload px4fmu-v2_default -j8"],
"shell": true
},
{
"name": "PX4: make posix",
"working_dir": "${project_path}",
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"cmd": ["make posix"],
"shell": true
},
{
"name": "MindPX_V2: make and upload",
"working_dir": "${project_path}",
"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
"cmd": ["make upload mindpx-v2_default -j8"],
"shell": true
}
]
"highlight_line": true,
"tab_size": 8,
"translate_tabs_to_spaces": false
}
}

31
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -1076,11 +1076,12 @@ void MulticopterPositionControl::control_auto(float dt)
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_pos_sp();
reset_alt_sp();
}
// Always check reset state of altitude and position control flags in auto
reset_pos_sp();
reset_alt_sp();
//Poll position setpoint
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
@ -1239,25 +1240,6 @@ void MulticopterPositionControl::control_auto(float dt)
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
/*
* if we're already near the current takeoff setpoint don't reset in case we switch back to posctl.
* this makes the takeoff finish smoothly.
*/
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF
|| _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER)
&& _pos_sp_triplet.current.acceptance_radius > 0.0f
/* need to detect we're close a bit before the navigator switches from takeoff to next waypoint */
&& (_pos - _pos_sp).length() < _pos_sp_triplet.current.acceptance_radius * 1.2f) {
_reset_pos_sp = false;
_reset_alt_sp = false;
/* otherwise: in case of interrupted mission don't go to waypoint but stay at current position */
} else {
_reset_pos_sp = true;
_reset_alt_sp = true;
}
// During a mission or in loiter it's safe to retract the landing gear.
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
@ -1660,7 +1642,7 @@ MulticopterPositionControl::task_main()
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if (acc_hor.length() > _params.acc_hor_max) {
if ((acc_hor.length() > _params.acc_hor_max) & !_reset_pos_sp) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
@ -1670,9 +1652,10 @@ MulticopterPositionControl::task_main()
}
// limit vertical acceleration
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
if (fabsf(acc_v) > 2 * _params.acc_hor_max) {
if ((fabsf(acc_v) > 2 * _params.acc_hor_max) & !_reset_alt_sp) {
acc_v /= fabsf(acc_v);
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
}