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

multirotor_pos_control: fixes for AUTO mode, minor cleanup

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/multirotor_pos_control/multirotor_pos_control.c
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;
}
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);
/* 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);
}
}
if (reset_sp_alt) {
if (status.flag_control_manual_enabled && reset_sp_alt) {
reset_sp_alt = false;
local_pos_sp.z = local_pos.z;
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);
}
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;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;