From 0263ab8cd24eb321fe9e2130f0f435e08a6c41e1 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 13 Feb 2017 09:42:31 +0100 Subject: [PATCH] enable takeoff in gps denied areas and minor requested changes --- .../commander/state_machine_helper.cpp | 13 ++++++----- src/modules/navigator/mission_block.cpp | 8 +------ src/modules/navigator/navigator.h | 22 +++++++++---------- 3 files changed, 19 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ea352b4f2c..243aa76409 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -77,6 +77,7 @@ static const char reason_no_rc_and_no_offboard[] = "no RC and no offboard"; static const char reason_no_gps[] = "no gps"; static const char reason_no_gps_cmd[] = "no gps cmd"; static const char reason_no_home[] = "no home"; +static const char reason_no_local_position[] = "no local position"; static const char reason_no_datalink[] = "no datalink"; // This array defines the arming state transitions. The rows are the new state, and the columns @@ -940,13 +941,13 @@ bool set_nav_state(struct vehicle_status_s *status, case commander_state_s::MAIN_STATE_AUTO_TAKEOFF: - /* require global position and home */ + /* require local position */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); + } else if (!status_flags->condition_local_position_valid) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position); if (status_flags->condition_local_position_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LAND; @@ -966,13 +967,13 @@ bool set_nav_state(struct vehicle_status_s *status, case commander_state_s::MAIN_STATE_AUTO_LAND: - /* require global position and home */ + /* require local position */ if (status->engine_failure) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if (status_flags->gps_failure || !status_flags->condition_local_position_valid) { - enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_gps); + } else if (!status_flags->condition_local_position_valid) { + enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_local_position); if (status_flags->condition_local_altitude_valid) { status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8f7789c17a..3800d5054a 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -711,13 +711,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio if (at_current_location) { item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; - - if (_navigator->home_position_valid()) { - item->yaw = _navigator->get_global_position()->yaw; - - } else { - item->yaw = _navigator->get_local_position()->yaw; - } + item->yaw = _navigator->get_local_position()->yaw; /* use home position */ diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 078a187e98..e192e54108 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -134,20 +134,20 @@ public: /** * Getters */ - struct vehicle_status_s *get_vstatus() { return &_vstatus; } - 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; } - bool home_position_valid() { return (_home_pos.timestamp > 0); } + struct vehicle_status_s *get_vstatus() { return &_vstatus; } + 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; } + bool home_position_valid() { return (_home_pos.timestamp > 0); } struct position_setpoint_triplet_s *get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct position_setpoint_triplet_s *get_reposition_triplet() { return &_reposition_triplet; } struct position_setpoint_triplet_s *get_takeoff_triplet() { return &_takeoff_triplet; } - struct mission_result_s *get_mission_result() { return &_mission_result; } - struct geofence_result_s *get_geofence_result() { return &_geofence_result; } + struct mission_result_s *get_mission_result() { return &_mission_result; } + struct geofence_result_s *get_geofence_result() { return &_geofence_result; } struct vehicle_attitude_setpoint_s *get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; }