multirotor_pos_control: global_position_setpoint support in AUTO mode

1 files changed, 38 insertions, 1 deletions

diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index d56c3d58f..3e5857c26 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -60,6 +60,7 @@
 #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 <systemlib/systemlib.h>
 #include <systemlib/pid/pid.h>
 #include <mavlink/mavlink_log.h>
@@ -179,6 +180,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	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(&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));
@@ -188,6 +191,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	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));
 
 	/* publish setpoint */
 	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
@@ -201,6 +205,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	const float pos_ctl_dz = 0.05f;
 	float home_alt = 0.0f;
 	hrt_abstime home_alt_t = 0;
+	uint64_t local_home_timestamp = 0;
 
 	static PID_t xy_pos_pids[2];
 	static PID_t xy_vel_pids[2];
@@ -222,6 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 
 	int paramcheck_counter = 0;
+	bool global_pos_sp_updated = false;
 
 	while (!thread_should_exit) {
 		orb_copy(ORB_ID(vehicle_status), state_sub, &status);
@@ -243,6 +249,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			paramcheck_counter = 0;
 		}
 
+		/* only check global position setpoint updates but not read */
+		if (!global_pos_sp_updated) {
+			orb_check(global_pos_sp_sub, &global_pos_sp_updated);
+		}
+
 		/* Check if controller should act */
 		bool act = status.flag_system_armed && (
 		   		       /* SAS modes */
@@ -271,7 +282,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) {
 			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
 			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
 			if (status.state_machine == SYSTEM_STATE_AUTO) {
-				orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
+				//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
+				if (local_pos.home_timestamp != local_home_timestamp) {
+					local_home_timestamp = local_pos.home_timestamp;
+					/* init local projection using local position home */
+					double lat_home = local_pos.home_lat * 1e-7;
+					double lon_home = local_pos.home_lon * 1e-7;
+					map_projection_init(lat_home, lon_home);
+					warnx("local pos home: lat = %.10f,  lon = %.10f", lat_home, lon_home);
+					mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
+				}
+				if (global_pos_sp_updated) {
+					global_pos_sp_updated = false;
+					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
+					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));
+					if (global_pos_sp.altitude_is_relative) {
+						local_pos_sp.z = -global_pos_sp.altitude;
+					} else {
+						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);
+					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) {