From 3f4a8bf76dea9b50bcf734aa119f28a2c74fd8b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:58:54 +0100 Subject: [PATCH] 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;