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

navigator: update mission after changing home position (#22834)

This commit is contained in:
Drone-Lab 2024-03-20 15:37:19 +08:00 committed by GitHub
parent 63850873eb
commit 37caddedbb
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 35 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/navigator/mission_base.cpp
View File

@ -253,6 +253,7 @@ MissionBase::on_active()
updateMavlinkMission();
updateDatamanCache();
updateMissionAltAfterHomeChanged();
/* Check the mission */
if (!_mission_checked && canRunMissionFeasibility()) {
@ -1375,3 +1376,29 @@ bool MissionBase::canRunMissionFeasibility()
(_geofence_status_sub.get().geofence_id == _mission.geofence_id) &&
(_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY);
}
void MissionBase::updateMissionAltAfterHomeChanged()
{
if (_navigator->get_home_position()->update_count > _home_update_counter) {
float new_alt = get_absolute_altitude_for_item(_mission_item);
float altitude_diff = new_alt - _navigator->get_position_setpoint_triplet()->current.alt;
if (_navigator->get_position_setpoint_triplet()->previous.valid
&& PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->previous.alt)) {
_navigator->get_position_setpoint_triplet()->previous.alt = _navigator->get_position_setpoint_triplet()->previous.alt +
altitude_diff;
}
_navigator->get_position_setpoint_triplet()->current.alt = _navigator->get_position_setpoint_triplet()->current.alt +
altitude_diff;
if (_navigator->get_position_setpoint_triplet()->next.valid
&& PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->next.alt)) {
_navigator->get_position_setpoint_triplet()->next.alt = _navigator->get_position_setpoint_triplet()->next.alt +
altitude_diff;
}
_navigator->set_position_setpoint_triplet_updated();
_home_update_counter = _navigator->get_home_position()->update_count;
}
}

8
src/modules/navigator/mission_base.h
View File

@ -448,8 +448,16 @@ private:
*/
bool checkMissionDataChanged(mission_s new_mission);
/**
* @brief update current mission altitude after the home position has changed.
*/
void updateMissionAltAfterHomeChanged();
bool canRunMissionFeasibility();
uint32_t _home_update_counter = 0; /**< Variable to store the previous value for home change detection.*/
bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment.
mission_item_s _last_gimbal_configure_item {};