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 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 9e0e170f2a..54463ee94d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -150,7 +151,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 @@ -223,7 +224,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. @@ -258,7 +259,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. @@ -296,7 +298,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]); @@ -495,7 +497,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) @@ -529,7 +531,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) { @@ -838,7 +840,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) { @@ -860,6 +863,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); @@ -886,6 +891,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; + bool was_landed = true; bool was_armed = false; bool startup_in_hil = false; @@ -1037,17 +1043,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)); @@ -1146,13 +1150,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 @@ -1609,6 +1615,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) { @@ -2239,7 +2253,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; } } @@ -2303,14 +2317,18 @@ 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 */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { - commander_set_home_position(home_pub, _home, local_position, global_position); + /* 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, attitude); } + was_landed = status.condition_landed; + was_armed = armed.armed; + /* print new state */ if (arming_state_changed) { status_changed = true; @@ -2318,8 +2336,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, 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;