forked from Archive/PX4-Autopilot
mavlink: move GPS_RAW_INT to separate stream header
This commit is contained in:
Daniel Agar
parent
67a204206a
commit
d46ae7983f
|
|
@ -112,6 +112,7 @@ using matrix::wrap_2pi;
|
|||
#include "streams/FLIGHT_INFORMATION.hpp"
|
||||
#include "streams/GLOBAL_POSITION_INT.hpp"
|
||||
#include "streams/GPS_GLOBAL_ORIGIN.hpp"
|
||||
#include "streams/GPS_RAW_INT.hpp"
|
||||
#include "streams/GPS_STATUS.hpp"
|
||||
#include "streams/HIGH_LATENCY2.hpp"
|
||||
#include "streams/HIL_ACTUATOR_CONTROLS.hpp"
|
||||
|
|
@ -1329,89 +1330,6 @@ protected:
|
|||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamGPSRawInt : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
const char *get_name() const override
|
||||
{
|
||||
return MavlinkStreamGPSRawInt::get_name_static();
|
||||
}
|
||||
|
||||
static constexpr const char *get_name_static()
|
||||
{
|
||||
return "GPS_RAW_INT";
|
||||
}
|
||||
|
||||
static constexpr uint16_t get_id_static()
|
||||
{
|
||||
return MAVLINK_MSG_ID_GPS_RAW_INT;
|
||||
}
|
||||
|
||||
uint16_t get_id() override
|
||||
{
|
||||
return get_id_static();
|
||||
}
|
||||
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink)
|
||||
{
|
||||
return new MavlinkStreamGPSRawInt(mavlink);
|
||||
}
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
return _gps_sub.advertised() ? MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
|
||||
}
|
||||
|
||||
private:
|
||||
uORB::Subscription _gps_sub{ORB_ID(sensor_gps), 0};
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &) = delete;
|
||||
MavlinkStreamGPSRawInt &operator = (const MavlinkStreamGPSRawInt &) = delete;
|
||||
|
||||
protected:
|
||||
explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink)
|
||||
{}
|
||||
|
||||
bool send() override
|
||||
{
|
||||
sensor_gps_s gps;
|
||||
|
||||
if (_gps_sub.update(&gps)) {
|
||||
mavlink_gps_raw_int_t msg{};
|
||||
|
||||
msg.time_usec = gps.timestamp;
|
||||
msg.fix_type = gps.fix_type;
|
||||
msg.lat = gps.lat;
|
||||
msg.lon = gps.lon;
|
||||
msg.alt = gps.alt;
|
||||
msg.alt_ellipsoid = gps.alt_ellipsoid;
|
||||
msg.eph = gps.hdop * 100;
|
||||
msg.epv = gps.vdop * 100;
|
||||
msg.h_acc = gps.eph * 1e3f;
|
||||
msg.v_acc = gps.epv * 1e3f;
|
||||
msg.vel_acc = gps.s_variance_m_s * 1e3f;
|
||||
msg.hdg_acc = math::degrees(gps.c_variance_rad) * 1e5f; // degE5
|
||||
|
||||
if (PX4_ISFINITE(gps.vel_m_s) && (fabsf(gps.vel_m_s) >= 0.f)) {
|
||||
msg.vel = gps.vel_m_s * 100.f; // cm/s
|
||||
|
||||
} else {
|
||||
msg.vel = UINT16_MAX; // If unknown, set to: UINT16_MAX
|
||||
}
|
||||
|
||||
msg.cog = math::degrees(wrap_2pi(gps.cog_rad)) * 1e2f;
|
||||
msg.satellites_visible = gps.satellites_used;
|
||||
|
||||
mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
class MavlinkStreamCameraTrigger : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
|
|
@ -1624,7 +1542,9 @@ static const StreamListItem streams_list[] = {
|
|||
#if defined(GPS_GLOBAL_ORIGIN_HPP)
|
||||
create_stream_list_item<MavlinkStreamGpsGlobalOrigin>(),
|
||||
#endif // GPS_GLOBAL_ORIGIN_HPP
|
||||
#if defined(GPS_RAW_INT_HPP)
|
||||
create_stream_list_item<MavlinkStreamGPSRawInt>(),
|
||||
#endif // GPS_RAW_INT_HPP
|
||||
#if defined(GPS2_RAW_HPP)
|
||||
create_stream_list_item<MavlinkStreamGPS2Raw>(),
|
||||
#endif // GPS2_RAW_HPP
|
||||
|
|
|
|||
|
|
@ -0,0 +1,108 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef GPS_RAW_INT_HPP
|
||||
#define GPS_RAW_INT_HPP
|
||||
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
class MavlinkStreamGPSRawInt : public MavlinkStream
|
||||
{
|
||||
public:
|
||||
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSRawInt(mavlink); }
|
||||
|
||||
static constexpr const char *get_name_static() { return "GPS_RAW_INT"; }
|
||||
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_RAW_INT; }
|
||||
|
||||
const char *get_name() const override { return get_name_static(); }
|
||||
uint16_t get_id() override { return get_id_static(); }
|
||||
|
||||
unsigned get_size() override
|
||||
{
|
||||
return _sensor_gps_sub.advertised() ? (MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
|
||||
}
|
||||
|
||||
private:
|
||||
explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink) {}
|
||||
|
||||
uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps), 0};
|
||||
|
||||
bool send() override
|
||||
{
|
||||
sensor_gps_s gps;
|
||||
|
||||
if (_sensor_gps_sub.update(&gps)) {
|
||||
mavlink_gps_raw_int_t msg{};
|
||||
|
||||
msg.time_usec = gps.timestamp;
|
||||
msg.fix_type = gps.fix_type;
|
||||
msg.lat = gps.lat;
|
||||
msg.lon = gps.lon;
|
||||
msg.alt = gps.alt;
|
||||
msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless)
|
||||
msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless)
|
||||
|
||||
if (PX4_ISFINITE(gps.vel_m_s) && (fabsf(gps.vel_m_s) >= 0.f)) {
|
||||
msg.vel = gps.vel_m_s * 100.f; // cm/s
|
||||
|
||||
} else {
|
||||
msg.vel = UINT16_MAX; // If unknown, set to: UINT16_MAX
|
||||
}
|
||||
|
||||
msg.cog = math::degrees(wrap_2pi(gps.cog_rad)) * 1e2f;
|
||||
msg.satellites_visible = gps.satellites_used;
|
||||
msg.alt_ellipsoid = gps.alt_ellipsoid;
|
||||
msg.h_acc = gps.eph * 1e3f; // position uncertainty in mm
|
||||
msg.v_acc = gps.epv * 1e3f; // altitude uncertainty in mm
|
||||
msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm
|
||||
msg.hdg_acc = math::degrees(gps.c_variance_rad) * 1e5f; // Heading / track uncertainty in degE5
|
||||
|
||||
if (PX4_ISFINITE(gps.heading)) {
|
||||
if (fabsf(gps.heading) < FLT_EPSILON) {
|
||||
msg.yaw = 36000; // Use 36000 for north.
|
||||
|
||||
} else {
|
||||
msg.yaw = math::degrees(gps.heading) * 100.f; // centidegrees
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_msg_gps_raw_int_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
#endif // GPS_RAW_INT_HPP
|
||||
Loading…
Reference in New Issue