Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Landing: Add MAVLink reporting interface

Browse Source
This commit is contained in:
Michael du Breuil 2017-04-12 23:57:10 -07:00 committed by Tom Pittenger
parent 5a15413513
commit 1e735324d4
4 changed files with 49 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
libraries/AP_Landing/AP_Landing.cpp
View File

@ -263,6 +263,21 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing
}
}
// send out any required mavlink messages
bool AP_Landing::send_landing_message(mavlink_channel_t chan) {
if (!flags.in_progress) {
return false;
}
switch (type) {
case TYPE_DEEPSTALL:
return deepstall.send_deepstall_message(chan);
case TYPE_STANDARD_GLIDE_SLOPE:
default:
return false;
}
}
bool AP_Landing::is_flaring(void) const
{
if (!flags.in_progress) {

2
libraries/AP_Landing/AP_Landing.h
View File

@ -20,6 +20,7 @@
#include <AP_Common/AP_Common.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <AP_Navigation/AP_Navigation.h>
#include <GCS_MAVLink/GCS.h>
#include "AP_Landing_Deepstall.h"
/// @class AP_Landing
@ -72,6 +73,7 @@ public:
void handle_flight_stage_change(const bool _in_landing_stage);
int32_t constrain_roll(const int32_t desired_roll_cd, const int32_t level_roll_limit_cd);
bool get_target_altitude_location(Location &location);
bool send_landing_message(mavlink_channel_t chan);
// helper functions
bool restart_landing_sequence(void);

28
libraries/AP_Landing/AP_Landing_Deepstall.cpp
View File

@ -364,6 +364,24 @@ int32_t AP_Landing_Deepstall::get_target_airspeed_cm(void) const
}
}
bool AP_Landing_Deepstall::send_deepstall_message(mavlink_channel_t chan) const
{
CHECK_PAYLOAD_SIZE2(DEEPSTALL);
mavlink_msg_deepstall_send(
chan,
landing_point.lat,
landing_point.lng,
stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_exit.lat : 0.0f,
stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_exit.lng : 0.0f,
stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_entry.lat : 0.0f,
stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? arc_entry.lng : 0.0f,
landing_point.alt * 0.01,
stage >= DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT ? predicted_travel_distance : 0.0f,
stage == DEEPSTALL_STAGE_LAND ? crosstrack_error : 0.0f,
stage);
return true;
}
const DataFlash_Class::PID_Info& AP_Landing_Deepstall::get_pid_info(void) const
{
return ds_PID.get_pid_info();
@ -414,7 +432,7 @@ void AP_Landing_Deepstall::build_approach_path(void)
}
float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height) const
float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const float height)
{
bool reverse = false;
@ -446,11 +464,13 @@ float AP_Landing_Deepstall::predict_travel_distance(const Vector3f wind, const f
float estimated_forward = cosf(estimated_crab_angle) * forward_speed_ms + cosf(theta) * wind_length;
predicted_travel_distance = estimated_forward * height / down_speed + stall_distance;
#ifdef DEBUG_PRINTS
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Predict: %f %f", stall_distance, estimated_forward * height / down_speed + stall_distance);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Predict: %f %f", stall_distance, predicted_travel_distance);
#endif // DEBUG_PRINTS
return estimated_forward * height / down_speed + stall_distance;
return predicted_travel_distance;
}
bool AP_Landing_Deepstall::verify_breakout(const Location &current_loc, const Location &target_loc,
@ -488,7 +508,7 @@ float AP_Landing_Deepstall::update_steering()
ab.normalize();
Vector2f a_air = location_diff(arc_exit, current_loc);
float crosstrack_error = a_air % ab;
crosstrack_error = a_air % ab;
float sine_nu1 = constrain_float(crosstrack_error / MAX(L1_period, 0.1f), -0.7071f, 0.7107f);
float nu1 = asinf(sine_nu1);

11
libraries/AP_Landing/AP_Landing_Deepstall.h
View File

@ -20,12 +20,13 @@
#include <AP_Common/AP_Common.h>
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#include <AP_Navigation/AP_Navigation.h>
#include <GCS_MAVLink/GCS.h>
#include <PID/PID.h>
class AP_Landing;
/// @class AP_Landing
/// @brief Class managing ArduPlane landing methods
/// @class AP_Landing_Deepstall
/// @brief Class managing Plane Deepstall landing methods
class AP_Landing_Deepstall
{
private:
@ -81,6 +82,8 @@ private:
float L1_xtrack_i; // L1 integrator for navigation
PID ds_PID;
int32_t last_target_bearing; // used for tracking the progress on loitering
float crosstrack_error; // current crosstrack error
float predicted_travel_distance; // distance the aircraft is perdicted to travel during deepstall
//public AP_Landing interface
void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
@ -97,11 +100,13 @@ private:
bool is_throttle_suppressed(void) const;
bool is_flying_forward(void) const;
bool send_deepstall_message(mavlink_channel_t chan) const;
const DataFlash_Class::PID_Info& get_pid_info(void) const;
//private helpers
void build_approach_path();
float predict_travel_distance(const Vector3f wind, const float height) const;
float predict_travel_distance(const Vector3f wind, const float height);
bool verify_breakout(const Location &current_loc, const Location &target_loc, const float height_error) const;
float update_steering(void);