dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

various fixed for bugs encountered during testing 

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-10-26 10:00:38 +03:00 committed by Lorenz Meier
parent cfb3cdc82f
commit b63f202b3c
4 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/navigator/mission.cpp
View File

@ -403,8 +403,8 @@ Mission::find_mission_land_start()
_land_start_index = i;
}
if (_land_start_available && (missionitem.nav_cmd == NAV_CMD_LOITER_TO_ALT)) {
// use the loiter to altitude item of the mission landing as target landing location
if (_land_start_available && i > _land_start_index && item_contains_position(missionitem)) {
// use the position of any waypoint after the land start marker which specifies a position.
_landing_lat = missionitem.lat;
_landing_lon = missionitem.lon;
_landing_alt = missionitem.altitude_is_relative ? missionitem.altitude +

5
src/modules/navigator/navigator_main.cpp
View File

@ -564,10 +564,11 @@ Navigator::run()
}
if (!rtl_activated && _rtl.initialClimbDone() && get_mission_start_land_available()) {
if (!rtl_activated && _rtl.getClimbAndReturnDone() && get_mission_start_land_available()) {
_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
if (!getMissionLandingInProgress()) {
if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
&& !get_land_detected()->landed) {
start_mission_landing();
}

11
src/modules/navigator/rtl.cpp
View File

@ -231,8 +231,9 @@ void RTL::on_activation()
_rtl_state = RTL_STATE_LANDED;
} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) {
// RTL straight to RETURN state, but mission will takeover for landing.
_rtl_state = RTL_STATE_RETURN;
// we were just on a mission landing, set _rtl_state past RTL_STATE_RETURN such that navigator will engage mission mode,
// which will continue executing the landing
_rtl_state = RTL_STATE_DESCEND;
} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
@ -246,7 +247,7 @@ void RTL::on_activation()
_rtl_state = RTL_STATE_RETURN;
}
setInitialClimbDone(_rtl_state != RTL_STATE_CLIMB);
setClimbAndReturnDone(_rtl_state > RTL_STATE_RETURN);
set_rtl_item();
@ -273,7 +274,7 @@ void RTL::set_rtl_item()
// Landing using planned mission landing, fly to DO_LAND_START instead of returning _destination.
// After reaching DO_LAND_START, do nothing, let navigator takeover with mission landing.
if (_destination.type == RTL_DESTINATION_MISSION_LANDING) {
if (_rtl_state > RTL_STATE_CLIMB) {
if (_rtl_state > RTL_STATE_RETURN) {
if (_navigator->start_mission_landing()) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing");
return;
@ -486,11 +487,11 @@ void RTL::advance_rtl()
switch (_rtl_state) {
case RTL_STATE_CLIMB:
setInitialClimbDone(true);
_rtl_state = RTL_STATE_RETURN;
break;
case RTL_STATE_RETURN:
setClimbAndReturnDone(true);
if (vtol_in_fw_mode || descend_and_loiter) {
_rtl_state = RTL_STATE_DESCEND;

6
src/modules/navigator/rtl.h
View File

@ -83,9 +83,9 @@ public:
int rtl_destination();
void setInitialClimbDone(bool done) { _initial_climb_done = done; }
void setClimbAndReturnDone(bool done) { _climb_and_return_done = done; }
bool initialClimbDone() { return _initial_climb_done; }
bool getClimbAndReturnDone() { return _climb_and_return_done; }
private:
/**
@ -138,7 +138,7 @@ private:
float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position
bool _rtl_alt_min{false};
bool _initial_climb_done{false}; // this flag is set to true if RTL is active and we are past the climb state
bool _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_rtl_return_alt,