From 8d0e10e830b4f3b676391acdcc63b95374118ff8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 19:14:23 +0200 Subject: [PATCH 1/5] Commander: Set home on takeoff --- src/modules/commander/commander.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3395b8c069..0b0b8dd5d4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -881,7 +881,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; - bool was_armed = false; + bool was_landed = true; bool startup_in_hil = false; @@ -1029,17 +1029,15 @@ int commander_thread_main(int argc, char *argv[]) px4_task_exit(ERROR); } - /* armed topic */ - orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); + /* armed topic */ + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* vehicle control mode topic */ memset(&control_mode, 0, sizeof(control_mode)); orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - /* home position */ orb_advert_t home_pub = nullptr; memset(&_home, 0, sizeof(_home)); @@ -2250,9 +2248,10 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + else if (was_landed && !status.condition_landed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { commander_set_home_position(home_pub, _home, local_position, global_position); } + was_landed = status.condition_landed; /* print new state */ if (arming_state_changed) { @@ -2261,8 +2260,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = false; } - was_armed = armed.armed; - /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, mission_result.finished, From b7f3c96d4a28a8432eb838364f059d6e79cda90d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:36:09 +0100 Subject: [PATCH 2/5] Set home on arming and on takeoff --- src/modules/commander/commander.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0b0b8dd5d4..86f8ad546b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -149,7 +149,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 -#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 +#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000 #define HIL_ID_MIN 1000 #define HIL_ID_MAX 1999 @@ -882,6 +882,7 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; bool was_landed = true; + bool was_armed = false; bool startup_in_hil = false; @@ -2248,10 +2249,12 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (was_landed && !status.condition_landed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && + (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { commander_set_home_position(home_pub, _home, local_position, global_position); } was_landed = status.condition_landed; + was_armed = armed.armed; /* print new state */ if (arming_state_changed) { From 12bd1ecb7c0d68b3fbe5eb2cebada52fc63b9d8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:57:42 +0100 Subject: [PATCH 3/5] Home pos: Add yaw field --- msg/home_position.msg | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/msg/home_position.msg b/msg/home_position.msg index d8aff3f634..7135c07b18 100644 --- a/msg/home_position.msg +++ b/msg/home_position.msg @@ -8,3 +8,8 @@ float32 alt # Altitude in meters (AMSL) float32 x # X coordinate in meters float32 y # Y coordinate in meters float32 z # Z coordinate in meters + +float32 yaw # Yaw angle in radians +float32 direction_x # Takeoff direction in NED X +float32 direction_y # Takeoff direction in NED Y +float32 direction_z # Takeoff direction in NED Z From 3f4a8bf76dea9b50bcf734aa119f28a2c74fd8b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:58:54 +0100 Subject: [PATCH 4/5] Commander: Set yaw on takeoff and use it as yaw during descend phase of RTL --- src/modules/commander/CMakeLists.txt | 2 +- src/modules/commander/commander.cpp | 44 +++++++++++++++++++--------- 2 files changed, 31 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 5ca0ff2ca6..6cf2531ade 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS -Os) if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=2400) endif() px4_add_module( MODULE modules__commander diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 86f8ad546b..53fe14dc08 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -221,7 +222,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -253,7 +254,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -291,7 +293,7 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3400, + 3500, commander_thread_main, (char * const *)&argv[0]); @@ -490,7 +492,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub) + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -524,7 +526,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { - commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude); } if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { @@ -833,7 +835,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude) { //Need global position fix to be able to set home if (!status.condition_global_position_valid) { @@ -855,6 +858,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & home.y = localPosition.y; home.z = localPosition.z; + home.yaw = attitude.yaw; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -1137,13 +1142,15 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); + struct vehicle_local_position_s local_position = {}; + + /* Subscribe to attitude data */ + int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct vehicle_attitude_s attitude = {}; /* Subscribe to land detector */ int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - struct vehicle_land_detected_s land_detector; - memset(&land_detector, 0, sizeof(land_detector)); + struct vehicle_land_detected_s land_detector = {}; /* * The home position is set based on GPS only, to prevent a dependency between @@ -1573,6 +1580,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } + /* update attitude estimate */ + orb_check(attitude_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude); + } + //update condition_global_position_valid //Global positions are only published by the estimators if they are valid if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { @@ -2183,7 +2198,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) { status_changed = true; } } @@ -2245,14 +2260,15 @@ int commander_thread_main(int argc, char *argv[]) /* First time home position update - but only if disarmed */ if (!status.condition_home_position_valid && !armed.armed) { - commander_set_home_position(home_pub, _home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + /* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */ else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { - commander_set_home_position(home_pub, _home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } + was_landed = status.condition_landed; was_armed = armed.armed; From 5759358da9965045478cd9b24d40be64cc9516b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:59:09 +0100 Subject: [PATCH 5/5] Navigator: Use yaw of home position --- src/modules/navigator/rtl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 246007ad81..8a4d89ef35 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -190,7 +190,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;