diff options
Diffstat (limited to 'src/modules/multirotor_pos_control/multirotor_pos_control.c')
| -rw-r--r-- | src/modules/multirotor_pos_control/multirotor_pos_control.c | 134 |
1 files changed, 1 insertions, 133 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 5af7e2a82..2ca650420 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,140 +415,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ reset_mission_sp = true; - - } else if (control_mode.flag_control_auto_enabled) { - /* AUTO mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_takeoff_sp) { - reset_takeoff_sp = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - 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); - } - - reset_auto_sp_xy = false; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { - // TODO - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - reset_mission_sp = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); - } - - if (reset_mission_sp) { - reset_mission_sp = false; - /* update global setpoint projection */ - - if (global_pos_sp_valid) { - - /* project global setpoint to local setpoint */ - map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (triplet.current.altitude_is_relative) { - local_pos_sp.z = -triplet.current.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - triplet.current.lat; - } - /* update yaw setpoint only if value is valid */ - if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) { - att_sp.yaw_body = triplet.current.yaw; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y); - - } else { - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - } - - if (reset_auto_sp_z) { - reset_auto_sp_z = false; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - } - - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { - reset_takeoff_sp = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - 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; - - } else { - /* no control (failsafe), loiter or stay on ground */ - if (local_pos.landed) { - /* landed: move setpoint down */ - /* in air: hold altitude */ - if (local_pos_sp.z < 5.0f) { - /* set altitude setpoint to 5m under ground, - * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ - local_pos_sp.z = 5.0f; - mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); - } - - reset_man_sp_z = true; - - } else { - /* in air: hold altitude */ - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); - } - - reset_auto_sp_z = false; - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - reset_auto_sp_xy = false; - } } + /* AUTO not implemented */ /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); |