zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

mavlink: GPS_GLOBAL_ORIGIN on by default, but only send on change or when requested

release/1.12
Daniel Agar 4 years ago
parent
8db37225d6
commit
a63e25f581
2 changed files with 59 additions and 37 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      src/modules/mavlink/mavlink_main.cpp
  2. 92
      src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp

4
src/modules/mavlink/mavlink_main.cpp
Unescape View File

@ -1522,6 +1522,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) @@ -1522,6 +1522,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("GPS2_RAW", 1.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 0.1f);
configure_stream_local("GPS_RAW_INT", 1.0f);
configure_stream_local("GPS_STATUS", 1.0f);
configure_stream_local("HOME_POSITION", 0.5f);
@ -1581,6 +1582,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) @@ -1581,6 +1582,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
configure_stream_local("GPS2_RAW", unlimited_rate);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
configure_stream_local("GPS_STATUS", 1.0f);
configure_stream_local("HOME_POSITION", 0.5f);
@ -1642,6 +1644,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) @@ -1642,6 +1644,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
configure_stream_local("GPS2_RAW", 1.0f);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS_RAW_INT", 1.0f);
configure_stream_local("HOME_POSITION", 0.5f);
configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.5f);
@ -1719,6 +1722,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) @@ -1719,6 +1722,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
configure_stream_local("GPS2_RAW", unlimited_rate);
configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
configure_stream_local("GPS_RAW_INT", unlimited_rate);
configure_stream_local("GPS_STATUS", 1.0f);
configure_stream_local("HIGHRES_IMU", 50.0f);

92
src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp
Unescape View File

@ -1,6 +1,6 @@ @@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -39,61 +39,79 @@ @@ -39,61 +39,79 @@
class MavlinkStreamGpsGlobalOrigin : public MavlinkStream
{
public:
const char *get_name() const override
{
return MavlinkStreamGpsGlobalOrigin::get_name_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGpsGlobalOrigin(mavlink); }
static constexpr const char *get_name_static()
{
return "GPS_GLOBAL_ORIGIN";
}
static constexpr const char *get_name_static() { return "GPS_GLOBAL_ORIGIN"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; }
static constexpr uint16_t get_id_static()
{
return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
}
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
uint16_t get_id() override
unsigned get_size() override
{
return get_id_static();
}
if (_vehicle_local_position_sub.advertised()) {
return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamGpsGlobalOrigin(mavlink);
return 0;
}
unsigned get_size() override
bool request_message(float param2, float param3, float param4, float param5, float param6, float param7) override
{
return _vehicle_local_position_sub.advertised() ?
(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
if (_valid) {
_force_next_send = true;
return true;
}
return false;
}
private:
explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
/* do not allow top copying this class */
MavlinkStreamGpsGlobalOrigin(MavlinkStreamGpsGlobalOrigin &) = delete;
MavlinkStreamGpsGlobalOrigin &operator = (const MavlinkStreamGpsGlobalOrigin &) = delete;
uint64_t _ref_timestamp{0};
double _ref_lat{static_cast<double>(NAN)};
double _ref_lon{static_cast<double>(NAN)};
float _ref_alt{NAN};
protected:
explicit MavlinkStreamGpsGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink)
{}
bool _valid{false};
bool _force_next_send{true};
bool send() override
{
vehicle_local_position_s vehicle_local_position;
if (_vehicle_local_position_sub.copy(&vehicle_local_position)
&& vehicle_local_position.xy_global && vehicle_local_position.z_global) {
mavlink_gps_global_origin_t msg{};
msg.latitude = static_cast<int32_t>(vehicle_local_position.ref_lat * 1e7); // double degree -> int32 degreeE7
msg.longitude = static_cast<int32_t>(vehicle_local_position.ref_lon * 1e7); // double degree -> int32 degreeE7
msg.altitude = static_cast<int32_t>(vehicle_local_position.ref_alt * 1e3f); // float m -> int32 mm
msg.time_usec = vehicle_local_position.timestamp; // int64 time since system boot
mavlink_msg_gps_global_origin_send_struct(_mavlink->get_channel(), &msg);
return true;
if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
if (vehicle_local_position.xy_global && vehicle_local_position.z_global) {
static constexpr double LLA_MIN_DIFF = 0.0000001; // ~11.132 mm at the equator
if (_force_next_send
|| (_ref_timestamp != vehicle_local_position.ref_timestamp)
|| (fabs(_ref_lat - vehicle_local_position.ref_lat) > LLA_MIN_DIFF)
|| (fabs(_ref_lon - vehicle_local_position.ref_lon) > LLA_MIN_DIFF)
|| (fabsf(_ref_alt - vehicle_local_position.ref_alt) > 0.001f)) {
mavlink_gps_global_origin_t msg{};
msg.latitude = round(vehicle_local_position.ref_lat * 1e7); // double degree -> int32 degreeE7
msg.longitude = round(vehicle_local_position.ref_lon * 1e7); // double degree -> int32 degreeE7
msg.altitude = roundf(vehicle_local_position.ref_alt * 1e3f); // float m -> int32 mm
msg.time_usec = vehicle_local_position.ref_timestamp; // int64 time since system boot
mavlink_msg_gps_global_origin_send_struct(_mavlink->get_channel(), &msg);
_ref_timestamp = vehicle_local_position.ref_timestamp;
_ref_lat = vehicle_local_position.ref_lat;
_ref_lon = vehicle_local_position.ref_lon;
_ref_alt = vehicle_local_position.ref_alt;
_force_next_send = false;
_valid = true;
return true;
}
}
}
return false;

Write Preview
Loading…
Cancel
Save