4899817366

Build Type

github

Committed by GitHub

Commit Message

Merge c0bb90862 into e4e9c71dd

Pull Request Pull Request #1772: Refactor MAVLinkParameters into client and server classes

Run Details

2192 of 2192 new or added lines in 34 files covered. (100.0%)

7708 of 24812 relevant lines covered (31.07%)

19.96 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/plugins/follow_me/follow_me_impl.cpp

#include "follow_me_impl.h"
#include "system.h"
#include "px4_custom_mode.h"
#include <cmath>

namespace mavsdk {

FollowMeImpl::FollowMeImpl(System& system) : PluginImplBase(system)
{
    // (Lat, Lon, Alt) => double, (vx, vy, vz) => float
    _last_location = _target_location = FollowMe::TargetLocation{};
    _system_impl->register_plugin(this);
}

FollowMeImpl::FollowMeImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    // (Lat, Lon, Alt) => double, (vx, vy, vz) => float
    _last_location = _target_location = FollowMe::TargetLocation{};
    _system_impl->register_plugin(this);
}

FollowMeImpl::~FollowMeImpl()
{
    if (_target_location_cookie) {
        _system_impl->remove_call_every(_target_location_cookie);
    }
    _system_impl->unregister_plugin(this);
}

void FollowMeImpl::init()
{
    _system_impl->register_mavlink_message_handler(
        MAVLINK_MSG_ID_HEARTBEAT,
        [this](const mavlink_message_t& message) { process_heartbeat(message); },
        static_cast<void*>(this));
}

void FollowMeImpl::deinit()
{
    _system_impl->unregister_all_mavlink_message_handlers(this);
}

void FollowMeImpl::enable()
{
    _system_impl->get_param_float_async(
        "FLW_TGT_HT",
        [this](MavlinkParameterSender::Result result, float value) {
            if (result == MavlinkParameterSender::Result::Success) {
                _config.follow_height_m = value;
            }
        },
        this);
    _system_impl->get_param_float_async(
        "FLW_TGT_DST",
        [this](MavlinkParameterSender::Result result, float value) {
            if (result == MavlinkParameterSender::Result::Success) {
                _config.follow_distance_m = value;
            }
        },
        this);
    _system_impl->get_param_float_async(
        "FLW_TGT_FA",
        [this](MavlinkParameterSender::Result result, float value) {
            if (result == MavlinkParameterSender::Result::Success) {
                _config.follow_angle_deg = value;
            }
        },
        this);
    _system_impl->get_param_float_async(
        "FLW_TGT_RS",
        [this](MavlinkParameterSender::Result result, float value) {
            if (result == MavlinkParameterSender::Result::Success) {
                _config.responsiveness = value;
            }
        },
        this);
    _system_impl->get_param_int_async(
        "FLW_TGT_ALT_M",
        [this](MavlinkParameterSender::Result result, int value) {
            if (result == MavlinkParameterSender::Result::Success) {
                _config.altitude_mode = static_cast<FollowMe::Config::FollowAltitudeMode>(value);
            }
        },
        this);
    _system_impl->get_param_float_async(
        "FLW_TGT_MAX_VEL",
        [this](MavlinkParameterSender::Result result, float value) {
            if (result == MavlinkParameterSender::Result::Success) {
                _config.max_tangential_vel_m_s = value;
            }
        },
        this);
}

void FollowMeImpl::disable()
{
    stop_sending_target_location();
}

FollowMe::Config FollowMeImpl::get_config() const
{
    return _config;
}

FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config& config)
{
    // Valdidate configuration
    if (!is_config_ok(config)) {
        LogErr() << debug_str << "set_config() failed. Last configuration is preserved.";
        return FollowMe::Result::SetConfigFailed;
    }

    auto height = config.follow_height_m;
    auto distance = config.follow_distance_m;
    auto responsiveness = config.responsiveness;
    auto altitude_mode = config.altitude_mode;
    auto max_tangential_vel_m_s = config.max_tangential_vel_m_s;

    LogDebug() << "Waiting for the system confirmation of the new configuration..";

    bool success = true;

    // Send configuration to Vehicle
    if (_config.follow_height_m != height) {
        if (_system_impl->set_param_float("FLW_TGT_HT", height) ==
            MavlinkParameterSender::Result::Success) {
            _config.follow_height_m = height;
        } else {
            success = false;
        }
    }

    if (_config.follow_distance_m != distance) {
        if (_system_impl->set_param_float("FLW_TGT_DST", distance) ==
            MavlinkParameterSender::Result::Success) {
            _config.follow_distance_m = distance;
        } else {
            success = false;
        }
    }

    if (_config.follow_angle_deg != config.follow_angle_deg) {
        if (_system_impl->set_param_float("FLW_TGT_FA", config.follow_angle_deg) ==
            MavlinkParameterSender::Result::Success) {
            _config.follow_angle_deg = config.follow_angle_deg;
        } else {
            success = false;
        }
    }

    if (_config.responsiveness != responsiveness) {
        if (_system_impl->set_param_float("FLW_TGT_RS", responsiveness) ==
            MavlinkParameterSender::Result::Success) {
            _config.responsiveness = responsiveness;
        } else {
            success = false;
        }
    }

    if (_config.altitude_mode != altitude_mode) {
        if (_system_impl->set_param_int("FLW_TGT_ALT_M", static_cast<int32_t>(altitude_mode)) ==
            MavlinkParameterSender::Result::Success) {
            _config.altitude_mode = altitude_mode;
        } else {
            success = false;
        }
    }

    if (_config.max_tangential_vel_m_s != max_tangential_vel_m_s) {
        if (_system_impl->set_param_float("FLW_TGT_MAX_VEL", max_tangential_vel_m_s) ==
            MavlinkParameterSender::Result::Success) {
            _config.max_tangential_vel_m_s = max_tangential_vel_m_s;
        } else {
            success = false;
        }
    }

    return (success ? FollowMe::Result::Success : FollowMe::Result::SetConfigFailed);
}

FollowMe::Result FollowMeImpl::set_target_location(const FollowMe::TargetLocation& location)
{
    bool schedule_now = false;
    {
        std::lock_guard<std::mutex> lock(_mutex);
        _target_location = location;
        // We're interested only in lat, long.
        _estimation_capabilities |= (1 << static_cast<int>(EstimationCapabilities::POS));

        if (_mode != Mode::ACTIVE) {
            return FollowMe::Result::NotActive;
        }
        // If set already, reschedule it.
        if (_target_location_cookie) {
            _system_impl->reset_call_every(_target_location_cookie);
            // We also need to send it right now.
            schedule_now = true;

        } else {
            // Register now for sending now and in the next cycle.
            _system_impl->add_call_every(
                [this]() { send_target_location(); }, SENDER_RATE, &_target_location_cookie);
        }
    }

    if (schedule_now) {
        send_target_location();
    }

    return FollowMe::Result::Success;
}

FollowMe::TargetLocation FollowMeImpl::get_last_location() const
{
    std::lock_guard<std::mutex> lock(_mutex);
    return _last_location;
}

bool FollowMeImpl::is_active() const
{
    std::lock_guard<std::mutex> lock(_mutex);
    return _mode == Mode::ACTIVE;
}

FollowMe::Result FollowMeImpl::start()
{
    FollowMe::Result result =
        to_follow_me_result(_system_impl->set_flight_mode(FlightMode::FollowMe));

    if (result == FollowMe::Result::Success) {
        // If location was set before, lets send it to vehicle
        std::lock_guard<std::mutex> lock(
            _mutex); // locking is not necessary here but lets do it for integrity
        if (is_target_location_set()) {
            _system_impl->add_call_every(
                [this]() { send_target_location(); }, SENDER_RATE, &_target_location_cookie);
        }
    }
    return result;
}

FollowMe::Result FollowMeImpl::stop()
{
    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (_mode == Mode::ACTIVE) {
            stop_sending_target_location();
        }
    }
    return to_follow_me_result(_system_impl->set_flight_mode(FlightMode::Hold));
}

bool FollowMeImpl::is_config_ok(const FollowMe::Config& config) const
{
    auto config_ok = false;

    if (config.follow_height_m < CONFIG_MIN_HEIGHT_M) {
        LogErr() << debug_str << "Err: Min height must be at least " << CONFIG_MIN_HEIGHT_M
                 << " meters";
    } else if (config.follow_distance_m < CONFIG_MIN_FOLLOW_DIST_M) {
        LogErr() << debug_str << "Err: Min Follow distance must be at least "
                 << CONFIG_MIN_FOLLOW_DIST_M << " meters";
    } else if (
        config.responsiveness < CONFIG_MIN_RESPONSIVENESS ||
        config.responsiveness > CONFIG_MAX_RESPONSIVENESS) {
        LogErr() << debug_str << "Err: Responsiveness must be in range ("
                 << CONFIG_MIN_RESPONSIVENESS << " to " << CONFIG_MAX_RESPONSIVENESS << ")";
    } else if (
        config.follow_angle_deg < CONFIG_MIN_FOLLOW_ANGLE ||
        config.follow_angle_deg > CONFIG_MAX_FOLLOW_ANGLE) {
        LogErr() << debug_str << "Err: Follow Angle must be in range " << CONFIG_MIN_FOLLOW_ANGLE
                 << " to " << CONFIG_MAX_FOLLOW_ANGLE << " degrees!";
    } else { // Config is OK
        config_ok = true;
    }

    return config_ok;
}

FollowMe::Result FollowMeImpl::to_follow_me_result(MavlinkCommandSender::Result result) const
{
    switch (result) {
        case MavlinkCommandSender::Result::Success:
            return FollowMe::Result::Success;
        case MavlinkCommandSender::Result::NoSystem:
            return FollowMe::Result::NoSystem;
        case MavlinkCommandSender::Result::ConnectionError:
            return FollowMe::Result::ConnectionError;
        case MavlinkCommandSender::Result::Busy:
            return FollowMe::Result::Busy;
        case MavlinkCommandSender::Result::Denied:
            // Fallthrough
        case MavlinkCommandSender::Result::TemporarilyRejected:
            return FollowMe::Result::CommandDenied;
        case MavlinkCommandSender::Result::Timeout:
            return FollowMe::Result::Timeout;
        default:
            return FollowMe::Result::Unknown;
    }
}

bool FollowMeImpl::is_target_location_set() const
{
    // If the target's latitude is NAN, we assume that location is not set.
    // We assume that mutex was acquired by the caller
    return std::isfinite(_target_location.latitude_deg);
}

void FollowMeImpl::send_target_location()
{
    // Don't send if we're not in FollowMe mode.
    if (!is_active()) {
        return;
    }

    SteadyTimePoint now = _time.steady_time();
    // needed by http://mavlink.org/messages/common#FOLLOW_TARGET
    uint64_t elapsed_msec =
        static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds

    std::lock_guard<std::mutex> lock(_mutex);
    //   LogDebug() << debug_str <<  "Lat: " << _target_location.latitude_deg << " Lon: " <<
    //   _target_location.longitude_deg <<
    //        " Alt: " << _target_location.absolute_altitude_m;
    const int32_t lat_int = int32_t(std::round(_target_location.latitude_deg * 1e7));
    const int32_t lon_int = int32_t(std::round(_target_location.longitude_deg * 1e7));
    const float alt = static_cast<float>(_target_location.absolute_altitude_m);

    const float pos_std_dev[] = {NAN, NAN, NAN};
    const float vel[] = {
        static_cast<float>(_target_location.velocity_x_m_s),
        static_cast<float>(_target_location.velocity_y_m_s),
        static_cast<float>(_target_location.velocity_z_m_s)};
    const float accel_unknown[] = {NAN, NAN, NAN};
    const float attitude_q_unknown[] = {1.f, NAN, NAN, NAN};
    const float rates_unknown[] = {NAN, NAN, NAN};
    uint64_t custom_state = 0;

    mavlink_message_t msg{};
    mavlink_msg_follow_target_pack(
        _system_impl->get_own_system_id(),
        _system_impl->get_own_component_id(),
        &msg,
        elapsed_msec,
        _estimation_capabilities,
        lat_int,
        lon_int,
        alt,
        vel,
        accel_unknown,
        attitude_q_unknown,
        rates_unknown,
        pos_std_dev,
        custom_state);

    if (!_system_impl->send_message(msg)) {
        LogErr() << debug_str << "send_target_location() failed..";
    } else {
        _last_location = _target_location;
    }
}

void FollowMeImpl::stop_sending_target_location()
{
    // We assume that mutex was acquired by the caller
    if (_target_location_cookie) {
        _system_impl->remove_call_every(_target_location_cookie);
        _target_location_cookie = nullptr;
    }
    _mode = Mode::NOT_ACTIVE;
}

void FollowMeImpl::process_heartbeat(const mavlink_message_t& message)
{
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(&message, &heartbeat);

    bool follow_me_active = false; // tells whether we're in FollowMe mode right now
    if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
        px4::px4_custom_mode px4_custom_mode;
        px4_custom_mode.data = heartbeat.custom_mode;

        if (px4_custom_mode.main_mode == px4::PX4_CUSTOM_MAIN_MODE_AUTO &&
            px4_custom_mode.sub_mode == px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET) {
            follow_me_active = true; // we're in FollowMe mode
        }
    }

    {
        std::lock_guard<std::mutex> lock(_mutex);
        if (!follow_me_active && _mode == Mode::ACTIVE) {
            // We're NOT in FollowMe mode anymore.
            // Lets stop sending target location updates
            stop_sending_target_location();
        } else if (follow_me_active && _mode == Mode::NOT_ACTIVE) {
            // We're in FollowMe mode now
            _mode = Mode::ACTIVE;
            return;
        }
    }
}

} // namespace mavsdk

1	#include "follow_me_impl.h"
2	#include "system.h"
3	#include "px4_custom_mode.h"
4	#include <cmath>
5
6	namespace mavsdk {
7
8	FollowMeImpl::FollowMeImpl(System& system) : PluginImplBase(system)	×
9	{
10	// (Lat, Lon, Alt) => double, (vx, vy, vz) => float
11	_last_location = _target_location = FollowMe::TargetLocation{};	×
12	_system_impl->register_plugin(this);	×
13	}	×
14
15	FollowMeImpl::FollowMeImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
16	{
17	// (Lat, Lon, Alt) => double, (vx, vy, vz) => float
18	_last_location = _target_location = FollowMe::TargetLocation{};	×
19	_system_impl->register_plugin(this);	×
20	}	×
21
22	FollowMeImpl::~FollowMeImpl()	×
23	{
24	if (_target_location_cookie) {	×
25	_system_impl->remove_call_every(_target_location_cookie);	×
26	}
27	_system_impl->unregister_plugin(this);	×
28	}	×
29
30	void FollowMeImpl::init()	×
31	{
32	_system_impl->register_mavlink_message_handler(	×
33	MAVLINK_MSG_ID_HEARTBEAT,
34	[this](const mavlink_message_t& message) { process_heartbeat(message); },	×
35	static_cast<void*>(this));
36	}	×
37
38	void FollowMeImpl::deinit()	×
39	{
40	_system_impl->unregister_all_mavlink_message_handlers(this);	×
41	}	×
42
43	void FollowMeImpl::enable()	×
44	{
45	_system_impl->get_param_float_async(	×
46	"FLW_TGT_HT",
47	[this](MavlinkParameterSender::Result result, float value) {	×
48	if (result == MavlinkParameterSender::Result::Success) {	×
49	_config.follow_height_m = value;	×
50	}
51	},	×
52	this);
53	_system_impl->get_param_float_async(	×
54	"FLW_TGT_DST",
55	[this](MavlinkParameterSender::Result result, float value) {	×
56	if (result == MavlinkParameterSender::Result::Success) {	×
57	_config.follow_distance_m = value;	×
58	}
59	},	×
60	this);
61	_system_impl->get_param_float_async(	×
62	"FLW_TGT_FA",
63	[this](MavlinkParameterSender::Result result, float value) {	×
64	if (result == MavlinkParameterSender::Result::Success) {	×
65	_config.follow_angle_deg = value;	×
66	}
67	},	×
68	this);
69	_system_impl->get_param_float_async(	×
70	"FLW_TGT_RS",
71	[this](MavlinkParameterSender::Result result, float value) {	×
72	if (result == MavlinkParameterSender::Result::Success) {	×
73	_config.responsiveness = value;	×
74	}
75	},	×
76	this);
77	_system_impl->get_param_int_async(	×
78	"FLW_TGT_ALT_M",
79	[this](MavlinkParameterSender::Result result, int value) {	×
80	if (result == MavlinkParameterSender::Result::Success) {	×
81	_config.altitude_mode = static_cast<FollowMe::Config::FollowAltitudeMode>(value);	×
82	}
83	},	×
84	this);
85	_system_impl->get_param_float_async(	×
86	"FLW_TGT_MAX_VEL",
87	[this](MavlinkParameterSender::Result result, float value) {	×
88	if (result == MavlinkParameterSender::Result::Success) {	×
89	_config.max_tangential_vel_m_s = value;	×
90	}
91	},	×
92	this);
93	}	×
94
95	void FollowMeImpl::disable()	×
96	{
97	stop_sending_target_location();	×
98	}	×
99
100	FollowMe::Config FollowMeImpl::get_config() const	×
101	{
102	return _config;	×
103	}
104
105	FollowMe::Result FollowMeImpl::set_config(const FollowMe::Config& config)	×
106	{
107	// Valdidate configuration
108	if (!is_config_ok(config)) {	×
109	LogErr() << debug_str << "set_config() failed. Last configuration is preserved.";	×
110	return FollowMe::Result::SetConfigFailed;	×
111	}
112
113	auto height = config.follow_height_m;	×
114	auto distance = config.follow_distance_m;	×
115	auto responsiveness = config.responsiveness;	×
116	auto altitude_mode = config.altitude_mode;	×
117	auto max_tangential_vel_m_s = config.max_tangential_vel_m_s;	×
118
119	LogDebug() << "Waiting for the system confirmation of the new configuration..";	×
120
121	bool success = true;	×
122
123	// Send configuration to Vehicle
124	if (_config.follow_height_m != height) {	×
125	if (_system_impl->set_param_float("FLW_TGT_HT", height) ==	×
126	MavlinkParameterSender::Result::Success) {
127	_config.follow_height_m = height;	×
128	} else {
129	success = false;	×
130	}
131	}
132
133	if (_config.follow_distance_m != distance) {	×
134	if (_system_impl->set_param_float("FLW_TGT_DST", distance) ==	×
135	MavlinkParameterSender::Result::Success) {
136	_config.follow_distance_m = distance;	×
137	} else {
138	success = false;	×
139	}
140	}
141
142	if (_config.follow_angle_deg != config.follow_angle_deg) {	×
143	if (_system_impl->set_param_float("FLW_TGT_FA", config.follow_angle_deg) ==	×
144	MavlinkParameterSender::Result::Success) {
145	_config.follow_angle_deg = config.follow_angle_deg;	×
146	} else {
147	success = false;	×
148	}
149	}
150
151	if (_config.responsiveness != responsiveness) {	×
152	if (_system_impl->set_param_float("FLW_TGT_RS", responsiveness) ==	×
153	MavlinkParameterSender::Result::Success) {
154	_config.responsiveness = responsiveness;	×
155	} else {
156	success = false;	×
157	}
158	}
159
160	if (_config.altitude_mode != altitude_mode) {	×
161	if (_system_impl->set_param_int("FLW_TGT_ALT_M", static_cast<int32_t>(altitude_mode)) ==	×
162	MavlinkParameterSender::Result::Success) {
163	_config.altitude_mode = altitude_mode;	×
164	} else {
165	success = false;	×
166	}
167	}
168
169	if (_config.max_tangential_vel_m_s != max_tangential_vel_m_s) {	×
170	if (_system_impl->set_param_float("FLW_TGT_MAX_VEL", max_tangential_vel_m_s) ==	×
171	MavlinkParameterSender::Result::Success) {
172	_config.max_tangential_vel_m_s = max_tangential_vel_m_s;	×
173	} else {
174	success = false;	×
175	}
176	}
177
178	return (success ? FollowMe::Result::Success : FollowMe::Result::SetConfigFailed);	×
179	}
180
181	FollowMe::Result FollowMeImpl::set_target_location(const FollowMe::TargetLocation& location)	×
182	{
183	bool schedule_now = false;	×
184	{
185	std::lock_guard<std::mutex> lock(_mutex);	×
186	_target_location = location;	×
187	// We're interested only in lat, long.
188	_estimation_capabilities \|= (1 << static_cast<int>(EstimationCapabilities::POS));	×
189
190	if (_mode != Mode::ACTIVE) {	×
191	return FollowMe::Result::NotActive;	×
192	}
193	// If set already, reschedule it.
194	if (_target_location_cookie) {	×
195	_system_impl->reset_call_every(_target_location_cookie);	×
196	// We also need to send it right now.
197	schedule_now = true;	×
198
199	} else {
200	// Register now for sending now and in the next cycle.
201	_system_impl->add_call_every(	×
202	[this]() { send_target_location(); }, SENDER_RATE, &_target_location_cookie);	×
203	}
204	}
205
206	if (schedule_now) {	×
207	send_target_location();	×
208	}
209
210	return FollowMe::Result::Success;	×
211	}
212
213	FollowMe::TargetLocation FollowMeImpl::get_last_location() const	×
214	{
215	std::lock_guard<std::mutex> lock(_mutex);	×
216	return _last_location;	×
217	}
218
219	bool FollowMeImpl::is_active() const	×
220	{
221	std::lock_guard<std::mutex> lock(_mutex);	×
222	return _mode == Mode::ACTIVE;	×
223	}
224
225	FollowMe::Result FollowMeImpl::start()	×
226	{
227	FollowMe::Result result =
228	to_follow_me_result(_system_impl->set_flight_mode(FlightMode::FollowMe));	×
229
230	if (result == FollowMe::Result::Success) {	×
231	// If location was set before, lets send it to vehicle
232	std::lock_guard<std::mutex> lock(	×
233	_mutex); // locking is not necessary here but lets do it for integrity	×
234	if (is_target_location_set()) {	×
235	_system_impl->add_call_every(	×
236	[this]() { send_target_location(); }, SENDER_RATE, &_target_location_cookie);	×
237	}
238	}
239	return result;	×
240	}
241
242	FollowMe::Result FollowMeImpl::stop()	×
243	{
244	{
245	std::lock_guard<std::mutex> lock(_mutex);	×
246	if (_mode == Mode::ACTIVE) {	×
247	stop_sending_target_location();	×
248	}
249	}
250	return to_follow_me_result(_system_impl->set_flight_mode(FlightMode::Hold));	×
251	}
252
253	bool FollowMeImpl::is_config_ok(const FollowMe::Config& config) const	×
254	{
255	auto config_ok = false;	×
256
257	if (config.follow_height_m < CONFIG_MIN_HEIGHT_M) {	×
258	LogErr() << debug_str << "Err: Min height must be at least " << CONFIG_MIN_HEIGHT_M	×
259	<< " meters";	×
260	} else if (config.follow_distance_m < CONFIG_MIN_FOLLOW_DIST_M) {	×
261	LogErr() << debug_str << "Err: Min Follow distance must be at least "	×
262	<< CONFIG_MIN_FOLLOW_DIST_M << " meters";	×
263	} else if (	×
264	config.responsiveness < CONFIG_MIN_RESPONSIVENESS \|\|	×
265	config.responsiveness > CONFIG_MAX_RESPONSIVENESS) {	×
266	LogErr() << debug_str << "Err: Responsiveness must be in range ("	×
267	<< CONFIG_MIN_RESPONSIVENESS << " to " << CONFIG_MAX_RESPONSIVENESS << ")";	×
268	} else if (	×
269	config.follow_angle_deg < CONFIG_MIN_FOLLOW_ANGLE \|\|	×
270	config.follow_angle_deg > CONFIG_MAX_FOLLOW_ANGLE) {	×
271	LogErr() << debug_str << "Err: Follow Angle must be in range " << CONFIG_MIN_FOLLOW_ANGLE	×
272	<< " to " << CONFIG_MAX_FOLLOW_ANGLE << " degrees!";	×
273	} else { // Config is OK
274	config_ok = true;	×
275	}
276
277	return config_ok;	×
278	}
279
280	FollowMe::Result FollowMeImpl::to_follow_me_result(MavlinkCommandSender::Result result) const	×
281	{
282	switch (result) {	×
283	case MavlinkCommandSender::Result::Success:	×
284	return FollowMe::Result::Success;	×
285	case MavlinkCommandSender::Result::NoSystem:	×
286	return FollowMe::Result::NoSystem;	×
287	case MavlinkCommandSender::Result::ConnectionError:	×
288	return FollowMe::Result::ConnectionError;	×
289	case MavlinkCommandSender::Result::Busy:	×
290	return FollowMe::Result::Busy;	×
291	case MavlinkCommandSender::Result::Denied:	×
292	// Fallthrough
293	case MavlinkCommandSender::Result::TemporarilyRejected:
294	return FollowMe::Result::CommandDenied;	×
295	case MavlinkCommandSender::Result::Timeout:	×
296	return FollowMe::Result::Timeout;	×
297	default:	×
298	return FollowMe::Result::Unknown;	×
299	}
300	}
301
302	bool FollowMeImpl::is_target_location_set() const	×
303	{
304	// If the target's latitude is NAN, we assume that location is not set.
305	// We assume that mutex was acquired by the caller
306	return std::isfinite(_target_location.latitude_deg);	×
307	}
308
309	void FollowMeImpl::send_target_location()	×
310	{
311	// Don't send if we're not in FollowMe mode.
312	if (!is_active()) {	×
313	return;	×
314	}
315
316	SteadyTimePoint now = _time.steady_time();	×
317	// needed by http://mavlink.org/messages/common#FOLLOW_TARGET
318	uint64_t elapsed_msec =
319	static_cast<uint64_t>(_time.elapsed_since_s(now) * 1000); // milliseconds	×
320
321	std::lock_guard<std::mutex> lock(_mutex);	×
322	// LogDebug() << debug_str << "Lat: " << _target_location.latitude_deg << " Lon: " <<
323	// _target_location.longitude_deg <<
324	// " Alt: " << _target_location.absolute_altitude_m;
325	const int32_t lat_int = int32_t(std::round(_target_location.latitude_deg * 1e7));	×
326	const int32_t lon_int = int32_t(std::round(_target_location.longitude_deg * 1e7));	×
327	const float alt = static_cast<float>(_target_location.absolute_altitude_m);	×
328
329	const float pos_std_dev[] = {NAN, NAN, NAN};	×
330	const float vel[] = {	×
331	static_cast<float>(_target_location.velocity_x_m_s),	×
332	static_cast<float>(_target_location.velocity_y_m_s),	×
333	static_cast<float>(_target_location.velocity_z_m_s)};	×
334	const float accel_unknown[] = {NAN, NAN, NAN};	×
335	const float attitude_q_unknown[] = {1.f, NAN, NAN, NAN};	×
336	const float rates_unknown[] = {NAN, NAN, NAN};	×
337	uint64_t custom_state = 0;	×
338
339	mavlink_message_t msg{};	×
340	mavlink_msg_follow_target_pack(	×
341	_system_impl->get_own_system_id(),	×
342	_system_impl->get_own_component_id(),	×
343	&msg,
344	elapsed_msec,
345	_estimation_capabilities,	×
346	lat_int,
347	lon_int,
348	alt,
349	vel,
350	accel_unknown,
351	attitude_q_unknown,
352	rates_unknown,
353	pos_std_dev,
354	custom_state);
355
356	if (!_system_impl->send_message(msg)) {	×
357	LogErr() << debug_str << "send_target_location() failed..";	×
358	} else {
359	_last_location = _target_location;	×
360	}
361	}
362
363	void FollowMeImpl::stop_sending_target_location()	×
364	{
365	// We assume that mutex was acquired by the caller
366	if (_target_location_cookie) {	×
367	_system_impl->remove_call_every(_target_location_cookie);	×
368	_target_location_cookie = nullptr;	×
369	}
370	_mode = Mode::NOT_ACTIVE;	×
371	}	×
372
373	void FollowMeImpl::process_heartbeat(const mavlink_message_t& message)	×
374	{
375	mavlink_heartbeat_t heartbeat;	×
376	mavlink_msg_heartbeat_decode(&message, &heartbeat);	×
377
378	bool follow_me_active = false; // tells whether we're in FollowMe mode right now	×
379	if (heartbeat.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {	×
380	px4::px4_custom_mode px4_custom_mode;
381	px4_custom_mode.data = heartbeat.custom_mode;	×
382
383	if (px4_custom_mode.main_mode == px4::PX4_CUSTOM_MAIN_MODE_AUTO &&	×
384	px4_custom_mode.sub_mode == px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET) {	×
385	follow_me_active = true; // we're in FollowMe mode	×
386	}
387	}
388
389	{
390	std::lock_guard<std::mutex> lock(_mutex);	×
391	if (!follow_me_active && _mode == Mode::ACTIVE) {	×
392	// We're NOT in FollowMe mode anymore.
393	// Lets stop sending target location updates
394	stop_sending_target_location();	×
395	} else if (follow_me_active && _mode == Mode::NOT_ACTIVE) {	×
396	// We're in FollowMe mode now
397	_mode = Mode::ACTIVE;	×
398	return;	×
399	}
400	}
401	}
402
403	} // namespace mavsdk

mavlink / MAVSDK / 4899817366

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous