Precision Landing - Weber-State-UAV-Program/Documentation-2024-2025 GitHub Wiki
The precision landing custom mode is implemented using the ROS2-PX4 Interface Library. It is essentially a state machine with 5 main states as illustrated in the overall system FSM.
All code for the precision landing mode can be found in the precision_land.cpp file in the projects code repository here
The going home state is responsible to directing the drone to its recorded home position using the 'GoTo' setpoint types as supported by the ROS2-PX4 interface library.
case State::GoingHome: {
// go to home position with RTL height, with heading as direction of travel
const Eigen::Vector3f home_position{_vehicle_home_position->localPosition().x(), _vehicle_home_position->localPosition().y(), _rtl_z};
const Eigen::Vector2f vehicle_to_target_xy = home_position.head(2) -_vehicle_local_position->positionNed().head(2);
const float heading_target_rad = atan2f(vehicle_to_target_xy(1), vehicle_to_target_xy(0));
if (vehicle_to_target_xy.norm() < 0.1f)
{
// stop caring about heading (the arctangent becomes undefined)
_goto_setpoint->update(home_position);
}
else
{
_goto_setpoint->update(home_position, heading_target_rad);
}
//If we have reached our reported home position, tags should be in frame.
if (positionReached(home_position, _goto_position_reached_error_threshold, _goto_velocity_reached_error_threshold)) {
_state = State::ApproachingTarget;
}
}
break;The home position is accessed using a shared pointer. It is essentially recorded as soon as the external mode is registered with the flight copmuter. The drone is considered to have reached the home position when the magnitude of the difference between the drones current position, and its home position is less than the threshold configured in precision_land.yaml
The approaching target state is responsible for the initial correction of the GPS/Home position error by using the pose of the first detected tag in the frame. It uses 'GoTo' position setpoints similar to the Go Home state.
case State::ApproachingTarget: {
//check if we already have an approach target
if (!_approach_target_set) {
Eigen::Vector2f velocity_setpoint = Eigen::Vector2f::Zero();
logDeltaErrors(velocity_setpoint);
//if a tag has been detected, set the approach target to the landing target position
if (!checkTagTimeout()){
_approach_target_position = Eigen::Vector3f(_landing_target_position.x(), _landing_target_position.y(), _vehicle_local_position->positionNed().z());
_approach_target_set = true;
_goto_setpoint->update(_approach_target_position);
}
else{
//Just stay put
_trajectory_setpoint->update(Eigen::Vector3f::Zero());
}
}
//if we have an approach target, go to it.
else
{
_goto_setpoint->update(_approach_target_position);
if(positionReached(_approach_target_position, _goto_position_reached_error_threshold, _goto_velocity_reached_error_threshold)){
_state = State::CenteringTarget;
}
}
break;
}If there is no tag in frame at this point, the drone will hold position and await manual takeover or failsafe activation. In testing, the error returning to the home position was never enough to result in no tag being in frame. Once the position is reached within the threshold specified in precision_land.yaml we switch to the active centering state.
The center target is the state where we switch from position set-points to velocity set-points generated by our PI control loop. Since the state machine runs at a frequency of 50hz, and a velocity set-point is calculated at each tick of the state machine, our control loop thus runs at 50 hz.
INFO We are running this control loop at 50hz. It is REQUIRED by PX4 that outpoints are updated with a rate of at least 2hz. Anything below will be considered unresponsive and the flight computer will failsafe.
case State::CenteringTarget: {
// Check if we have a recent tag detection
if (!checkTagTimeout()) {
Eigen::Vector2f velocity_setpoint = GetVelocitySetpoint(dt_s);
//log delta errors
logDeltaErrors(velocity_setpoint);
_trajectory_setpoint->update(Eigen::Vector3f(velocity_setpoint.x(), velocity_setpoint.y(), 0.0));
if (min_time_elapsed && positionReached(Eigen::Vector3f(_landing_target_position.x(), _landing_target_position.y(), _vehicle_local_position->positionNed().z()), _position_reached_error_threshold, _velocity_reached_error_threshold)) {
_state = State::OrientingTarget;
}
}
else {
//maintain current position
_trajectory_setpoint->update(Eigen::Vector3f::Zero());
}
break;
}The GetVelocitySetpoint() function is called to calculated the x and y velocity setpoint. It is passed dt_s which is the time that has passed since it was last called. dt_s is required for the integral portion of this controller.
Eigen::Vector2f PrecisionLandDemo::GetVelocitySetpoint(float dt_s)
{
//error between current position and target position
float delta_x = _vehicle_local_position->positionNed().x() - _landing_target_position.x();
float delta_y = _vehicle_local_position->positionNed().y() - _landing_target_position.y();
// Update integral terms with clamping
_integral_error_x = std::clamp(_integral_error_x + delta_x * dt_s, -_i_vel_clamp, _i_vel_clamp);
_integral_error_y = std::clamp(_integral_error_y + delta_y * dt_s, -_i_vel_clamp, _i_vel_clamp);
//scale by gains
float Xp = delta_x * _p_vel_gain_x;
float Yp = delta_y * _p_vel_gain_y;
float Xi = _integral_error_x * _i_vel_gain_x;
float Yi = _integral_error_y * _i_vel_gain_y;
//we want to counteract that error, negate
float vx = -1.0f * (Xp + Xi);
float vy = -1.0f * (Yp + Yi);
// clamp to max values
vx = std::clamp(vx, -1.0f * _max_xy_velocity, _max_xy_velocity);
vy = std::clamp(vy, -1.0f * _max_xy_velocity, _max_xy_velocity);
return Eigen::Vector2f(vx, vy);
}This is the implemented PI controller. The integral term is clamped to prevent integral windup, and the output velocity is clamped to restrict the response of the drone to relatively slow corrections to keep the AprilTags in frame.
The orient target state performs an initial orientation of the drone before beginning descent. This is to ensure that the drone yaw is at least roughly alight so that aggressive corrections are not required during descent.
case State::OrientingTarget: {
//if we dont have our yaw target based on a tag, just stay put
if (!checkTagTimeout()){
Eigen::Vector2f velocity_setpoint = GetVelocitySetpoint(dt_s);
if (!_approach_target_yaw_set){
_approach_target_yaw = _yaw_setpoint;
_approach_target_yaw_set = true;
}
logDeltaErrors(velocity_setpoint);
_trajectory_setpoint->update(Eigen::Vector3f(velocity_setpoint.x(), velocity_setpoint.y(), 0.0), std::nullopt, _approach_target_yaw);
}
else
{
// stay put.
_trajectory_setpoint->update(Eigen::Vector3f::Zero());
}
Eigen::Vector3f target_position(_landing_target_position.x(),
_landing_target_position.y(),
_vehicle_local_position->positionNed().z());
//If we are centered, oriented, and have spent enough time in this state, descend.
if (min_time_elapsed &&
orientationReached(_approach_target_yaw) &&
positionReached(target_position, _position_reached_error_threshold, _velocity_reached_error_threshold)) {
_state = State::DescendingTarget;
}
break;
}The descend target state couples all previous states into one. It continuously centers using the PI control loop, and adjusts the yaw based on the orientation of the latest tag detection.
case State::DescendingTarget: {
// Check if we have a recent tag detection
if (!checkTagTimeout()) {
Eigen::Vector2f velocity_setpoint = GetVelocitySetpoint(dt_s);
//log delta errors
logDeltaErrors(velocity_setpoint);
//if we are already at the target orientation, no need to yaw.
if(!orientationReached(_yaw_setpoint)){
_trajectory_setpoint->update(Eigen::Vector3f(velocity_setpoint.x(), velocity_setpoint.y(), _descend_velocity), std::nullopt, _yaw_setpoint);
}
else{
_trajectory_setpoint->update(Eigen::Vector3f(velocity_setpoint.x(), velocity_setpoint.y(), _descend_velocity));
}
if (_land_detected->landed()) {
_state = State::Finished;
}
}
else {
//maintain current position
_trajectory_setpoint->update(Eigen::Vector3f::Zero());
}
break;
}
case State::Finished: {
if (_enable_delta_error_logging && _log_file.is_open()) {
_log_file.close();
}
ModeBase::completed(px4_ros2::Result::Success);
break;
}Once the flight computer indicates that a landing has been detected, we move into the finished state, which is essentially a placeholder for no action.
The apriltag detection callback is executed each time an apriltag is detected by the apriltag_ros node. Tag detections are published to the /detections topic, to which the precision landing node is subscribed. Tags are prioritized based on their size. ID: 3 is the smallest tag, followed by ID: 4. IDs 1 and 2 are the same size, so the priority order of these detections does not matter.
void PrecisionLandDemo::detectionCallback(const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr msg)
{
if (!msg->detections.empty()) {
_last_detection_time = _node.get_clock()->now();
//priority order of tags
std::vector<int> priority_order = {3, 4, 1, 2};
std::vector<int> tag_ids;
for (const auto& detection : msg->detections) {
tag_ids.push_back(detection.id);
}
int target_tag_id = -1;
for (int id : priority_order) {
if (std::find(tag_ids.begin(), tag_ids.end(), id) != tag_ids.end()) {
target_tag_id = id;
break;
}
}
if (target_tag_id != -1)
{
geometry_msgs::msg::TransformStamped transform;
rclcpp::Time detection_time(msg->header.stamp.sec, msg->header.stamp.nanosec);
try{
std::string target_frame = "target:" + std::to_string(target_tag_id);
transform = _tf_buffer->lookupTransform(_local_frame, target_frame, detection_time, 20ms);
}
catch(const std::exception& e)
{
RCLCPP_ERROR(_node.get_logger(), "Error looking up transform: %s", e.what());
return;
}
//update the landing target position
_landing_target_position = Eigen::Vector2f(transform.transform.translation.x, transform.transform.translation.y);
//update yaw based on tag orientation
_yaw_setpoint = px4_ros2::quaternionToYaw(Eigen::Quaternionf(transform.transform.rotation.w, transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z));
//apply yaw offset
_yaw_setpoint += px4_ros2::degToRad(_yaw_offset_deg);
_integral_error_x = 0.0f;
_integral_error_y = 0.0f;
}
}
}The full code for this node can be found here