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 | 39 |
1 files changed, 17 insertions, 22 deletions
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3d23d0c09..5af7e2a82 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,8 +61,8 @@ #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> +#include <uORB/topics/mission_item_triplet.h> #include <systemlib/systemlib.h> #include <systemlib/pid/pid.h> #include <mavlink/mavlink_log.h> @@ -190,12 +190,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) memset(&manual, 0, sizeof(manual)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); - 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(global_pos_sp)); + struct mission_item_triplet_s triplet; + memset(&triplet, 0, sizeof(triplet)); struct vehicle_global_velocity_setpoint_s global_vel_sp; memset(&global_vel_sp, 0, sizeof(global_vel_sp)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -203,9 +203,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + int mission_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); @@ -292,11 +290,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } - orb_check(global_pos_sp_sub, &updated); + orb_check(mission_triplet_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - global_pos_sp_valid = true; + orb_copy(ORB_ID(mission_item_triplet), mission_triplet_sub, &triplet); + global_pos_sp_valid = triplet.current_valid; reset_mission_sp = true; } @@ -329,7 +327,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; @@ -459,24 +456,22 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* update global setpoint projection */ if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y)); - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; + if (triplet.current.altitude_is_relative) { + local_pos_sp.z = -triplet.current.altitude; } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + local_pos_sp.z = local_pos.ref_alt - triplet.current.lat; } /* update yaw setpoint only if value is valid */ - if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) { - att_sp.yaw_body = global_pos_sp.yaw; + 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)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); + 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) { |