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

Merge pull request #409 from PX4/mpc_yaw_fix 

multirotor_pos_control: yaw setpoint bug fixed
This commit is contained in:
Lorenz Meier 2013-09-22 00:57:50 -07:00
parent 7033aa173b 96d8ee3483
commit 669d4c6dd2
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -193,7 +193,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(local_pos_sp));
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
@ -408,6 +408,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
@ -430,7 +431,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
@ -471,8 +471,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
local_pos_sp.yaw = global_pos_sp.yaw;
att_sp.yaw_body = global_pos_sp.yaw;
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
@ -485,7 +483,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = global_pos_sp.yaw;
}
if (reset_auto_sp_z) {
@ -509,6 +506,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
reset_mission_sp = true;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = true;
reset_man_sp_z = true;