fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 11:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor_pos_control: fixes for AUTO mode, minor cleanup

Browse Source
This commit is contained in:
Anton Babushkin
2013-07-17 22:22:51 +04:00
parent c4248c055f
commit 17366c4b0d
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/multirotor_pos_control/multirotor_pos_control.c
+3 -3
View File
@@ -333,21 +333,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude;
} }
warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
/* publish local position setpoint as projection of global position setpoint */ /* publish local position setpoint as projection of global position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
} }
} }
if (reset_sp_alt) { if (status.flag_control_manual_enabled && reset_sp_alt) {
reset_sp_alt = false; reset_sp_alt = false;
local_pos_sp.z = local_pos.z; local_pos_sp.z = local_pos.z;
z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside
mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle);
} }
if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { if (status.flag_control_manual_enabled && status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) {
reset_sp_pos = false; reset_sp_pos = false;
local_pos_sp.x = local_pos.x; local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y; local_pos_sp.y = local_pos.y;