forked from Archive/PX4-Autopilot
navigator: update mission after changing home position (#22834)
This commit is contained in:
parent
63850873eb
commit
37caddedbb
|
@ -253,6 +253,7 @@ MissionBase::on_active()
|
||||||
|
|
||||||
updateMavlinkMission();
|
updateMavlinkMission();
|
||||||
updateDatamanCache();
|
updateDatamanCache();
|
||||||
|
updateMissionAltAfterHomeChanged();
|
||||||
|
|
||||||
/* Check the mission */
|
/* Check the mission */
|
||||||
if (!_mission_checked && canRunMissionFeasibility()) {
|
if (!_mission_checked && canRunMissionFeasibility()) {
|
||||||
|
@ -1375,3 +1376,29 @@ bool MissionBase::canRunMissionFeasibility()
|
||||||
(_geofence_status_sub.get().geofence_id == _mission.geofence_id) &&
|
(_geofence_status_sub.get().geofence_id == _mission.geofence_id) &&
|
||||||
(_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY);
|
(_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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -448,8 +448,16 @@ private:
|
||||||
*/
|
*/
|
||||||
bool checkMissionDataChanged(mission_s new_mission);
|
bool checkMissionDataChanged(mission_s new_mission);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief update current mission altitude after the home position has changed.
|
||||||
|
*/
|
||||||
|
|
||||||
|
void updateMissionAltAfterHomeChanged();
|
||||||
|
|
||||||
bool canRunMissionFeasibility();
|
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.
|
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 {};
|
mission_item_s _last_gimbal_configure_item {};
|
||||||
|
|
Loading…
Reference in New Issue