Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

mc_pos_control: minor fixes

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-01 23:20:36 +04:00
parent 5dda4dc993
commit 352a1ef095
1 changed files with 6 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+6 -12
View File
@@ -130,8 +130,6 @@ private:
struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */ struct vehicle_global_velocity_setpoint_s _global_vel_sp; /**< vehicle global velocity setpoint */
struct { struct {
param_t takeoff_alt;
param_t takeoff_gap;
param_t thr_min; param_t thr_min;
param_t thr_max; param_t thr_max;
param_t z_p; param_t z_p;
@@ -155,8 +153,6 @@ private:
} _params_handles; /**< handles for interesting parameters */ } _params_handles; /**< handles for interesting parameters */
struct { struct {
float takeoff_alt;
float takeoff_gap;
float thr_min; float thr_min;
float thr_max; float thr_max;
float tilt_max; float tilt_max;
@@ -265,8 +261,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
_vel_sp.zero(); _vel_sp.zero();
_vel_prev.zero(); _vel_prev.zero();
_params_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
_params_handles.takeoff_gap = param_find("NAV_TAKEOFF_GAP");
_params_handles.thr_min = param_find("MPC_THR_MIN"); _params_handles.thr_min = param_find("MPC_THR_MIN");
_params_handles.thr_max = param_find("MPC_THR_MAX"); _params_handles.thr_max = param_find("MPC_THR_MAX");
_params_handles.z_p = param_find("MPC_Z_P"); _params_handles.z_p = param_find("MPC_Z_P");
@@ -327,8 +321,6 @@ MulticopterPositionControl::parameters_update(bool force)
orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd); orb_copy(ORB_ID(parameter_update), _params_sub, &param_upd);
if (updated || force) { if (updated || force) {
param_get(_params_handles.takeoff_alt, &_params.takeoff_alt);
param_get(_params_handles.takeoff_gap, &_params.takeoff_gap);
param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_min, &_params.thr_min);
param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.thr_max, &_params.thr_max);
param_get(_params_handles.tilt_max, &_params.tilt_max); param_get(_params_handles.tilt_max, &_params.tilt_max);
@@ -622,6 +614,9 @@ MulticopterPositionControl::task_main()
if (_mission_items.current_valid) { if (_mission_items.current_valid) {
struct mission_item_s item = _mission_items.current; struct mission_item_s item = _mission_items.current;
// TODO home altitude can be != ref_alt, check home_position topic
_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt);
if (item.nav_cmd == NAV_CMD_TAKEOFF) { if (item.nav_cmd == NAV_CMD_TAKEOFF) {
/* use current position setpoint or current position */ /* use current position setpoint or current position */
if (reset_sp_xy) { if (reset_sp_xy) {
@@ -637,10 +632,9 @@ MulticopterPositionControl::task_main()
} else { } else {
map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1)); map_projection_project(item.lat, item.lon, &_pos_sp(0), &_pos_sp(1));
// TODO home altitude can be != ref_alt, check home_position topic if (isfinite(_mission_items.current.yaw)) {
_pos_sp(2) = -(item.altitude_is_relative ? item.altitude : item.altitude - ref_alt); _att_sp.yaw_body = _mission_items.current.yaw;
}
_att_sp.yaw_body = _mission_items.current.yaw;
/* in case of interrupted mission don't go to waypoint but stay at current position */ /* in case of interrupted mission don't go to waypoint but stay at current position */
reset_sp_xy = true; reset_sp_xy = true;