diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 012825bef4..03b8dc2d3c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -433,7 +433,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "takeoff")) { /* see if we got a home position */ - if (status_flags.condition_home_position_valid) { + if (status_flags.condition_local_position_valid) { if (TRANSITION_DENIED != arm_disarm(true, &mavlink_log_pub, "command line")) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d2e76a39c0..ea352b4f2c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -428,8 +428,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta case commander_state_s::MAIN_STATE_AUTO_MISSION: case commander_state_s::MAIN_STATE_AUTO_RTL: - case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: - case commander_state_s::MAIN_STATE_AUTO_LAND: /* need global position and home position */ if (status_flags->condition_global_position_valid && status_flags->condition_home_position_valid) { @@ -438,6 +436,16 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; + case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: + case commander_state_s::MAIN_STATE_AUTO_LAND: + + /* need local position */ + if (status_flags->condition_local_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + case commander_state_s::MAIN_STATE_OFFBOARD: /* need offboard signal @@ -937,8 +945,7 @@ bool set_nav_state(struct vehicle_status_s *status, if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid || - !status_flags->condition_home_position_valid)) { + } else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); if (status_flags->condition_local_position_valid) { @@ -964,8 +971,7 @@ bool set_nav_state(struct vehicle_status_s *status, if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure || (!status_flags->condition_global_position_valid || - !status_flags->condition_home_position_valid)) { + } else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); if (status_flags->condition_local_altitude_valid) { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ffb1c6250a..078a187e98 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,7 @@ public: struct vehicle_land_detected_s *get_land_detected() { return &_land_detected; } struct vehicle_control_mode_s *get_control_mode() { return &_control_mode; } struct vehicle_global_position_s *get_global_position() { return &_global_pos; } + struct vehicle_local_position_s *get_local_position() { return &_local_pos; } struct vehicle_gps_position_s *get_gps_position() { return &_gps_pos; } struct sensor_combined_s *get_sensor_combined() { return &_sensor_combined; } struct home_position_s *get_home_position() { return &_home_pos; } @@ -238,6 +240,7 @@ private: orb_advert_t _mavlink_log_pub; /**< the uORB advert to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ + int _local_pos_sub; /**< local position subscription */ int _gps_pos_sub; /**< gps position subscription */ int _sensor_combined_sub; /**< sensor combined subscription */ int _home_pos_sub; /**< home position subscription */ @@ -261,6 +264,7 @@ private: vehicle_land_detected_s _land_detected; /**< vehicle land_detected */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ + vehicle_local_position_s _local_pos; /**< local vehicle position */ vehicle_gps_position_s _gps_pos; /**< gps position */ sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ @@ -324,6 +328,11 @@ private: */ void global_position_update(); + /** + * Retrieve local position + */ + void local_position_update(); + /** * Retrieve gps position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 577f395b9c..a20fac1f92 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -105,6 +105,7 @@ Navigator::Navigator() : _navigator_task(-1), _mavlink_log_pub(nullptr), _global_pos_sub(-1), + _local_pos_sub(-1), _gps_pos_sub(-1), _sensor_combined_sub(-1), _home_pos_sub(-1), @@ -212,6 +213,12 @@ Navigator::global_position_update() orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } +void +Navigator::local_position_update() +{ + orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos); +} + void Navigator::gps_position_update() { @@ -301,6 +308,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status)); @@ -318,6 +326,7 @@ Navigator::task_main() vehicle_land_detected_update(); vehicle_control_mode_update(); global_position_update(); + local_position_update(); gps_position_update(); sensor_combined_update(); home_position_update(true); @@ -382,6 +391,13 @@ Navigator::task_main() } } + /* local position updated */ + orb_check(_local_pos_sub, &updated); + + if (updated) { + local_position_update(); + } + /* sensors combined updated */ orb_check(_sensor_combined_sub, &updated); @@ -492,7 +508,13 @@ Navigator::task_main() rep->current.loiter_radius = get_loiter_radius(); rep->current.loiter_direction = 1; rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF; - rep->current.yaw = cmd.param4; + if (home_position_valid()) { + rep->current.yaw = cmd.param4; + rep->previous.valid = true; + } else { + rep->current.yaw = get_local_position()->yaw; + rep->previous.valid = false; + } if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7; @@ -506,7 +528,6 @@ Navigator::task_main() rep->current.alt = cmd.param7; - rep->previous.valid = true; rep->current.valid = true; rep->next.valid = false;