GPS_INJECT CMD 232

[AutonauticsTeam]

Hi

Looking to send GPS corrections wondering if there is a method to this. I have looked around and there is a lot regarding sending Fake GPS. I am looking to construct the 232 mavlink message and then send it.

time_usec	uint64_t	Timestamp (micros since boot or Unix epoch)
gps_id	uint8_t	ID of the GPS for multiple GPS inputs
ignore_flags	uint16_t	Flags indicating which fields to ignore (see GPS_INPUT_IGNORE_FLAGS enum). All other fields must be provided.
time_week_ms	uint32_t	GPS time (milliseconds from start of GPS week)
time_week	uint16_t	GPS week number
fix_type	uint8_t	0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK
lat	int32_t	Latitude (WGS84), in degrees * 1E7
lon	int32_t	Longitude (WGS84), in degrees * 1E7
alt	float	Altitude (AMSL, not WGS84), in m (positive for up)
hdop	float	GPS HDOP horizontal dilution of position in m
vdop	float	GPS VDOP vertical dilution of position in m
vn	float	GPS velocity in m/s in NORTH direction in earth-fixed NED frame
ve	float	GPS velocity in m/s in EAST direction in earth-fixed NED frame
vd	float	GPS velocity in m/s in DOWN direction in earth-fixed NED frame
speed_accuracy	float	GPS speed accuracy in m/s
horiz_accuracy	float	GPS horizontal accuracy in m
vert_accuracy	float	GPS vertical accuracy in m
satellites_visible	uint8_t	Number of satellites visible.

That is the message. Is there a simple way to complete this eg mavcmd? or is this not currently supported?

Thanks a lot in advance and sorry if I have missed something posted earlier

TG

[vooon]

It is not supported, you should extend fake_gps plugin or write your own.

Also message called GPS_INPUT.

[AutonauticsTeam]

Hi vooon thanks for getting back to me. I have looked through fakegps and it looks like what we want to do. Is it possible to publish to the mavlink/to topic with a type 233 and our raw RTCM3 data packed? Thanks for your help

[vooon]

Better don't do that. Publishing to mavlink/to have several problems, much easier is to write mavros plugin (where all necessary environment already provided and problems solved).

[AutonauticsTeam]

Ok thanks vooon, I will attempt to do this.

the structure of 233 requires three parts which are flags length and the payload. The payload is obviously the RTCM3 raw data however are the flags and length set by mavros and mavlink or must these be set by my plugin?

Thanks for all your help!

[vooon]

Should be set by plugin. Mavros does nothing with message contents, so you must fill all fields before send.

[AutonauticsTeam]

ok thanks very much for your help!

[vooon]

As i see from GPS_RTCM_DATA, you need custom GpsRtcmData.msg:

Header header

uint8[] data # must be smaller than 4*180

In callback you need:

using mavlink::common::msg::GPS_RTCM_DATA;

GPS_RTCM_DATA m{};

bool fragmented = data.size() > m.data.size():

m.flag = (rtcm_seq << 3) | (pkt_frag << 1) | ((fragmented) ? 1 : 0);
// copy portion of data
// fill m.len
UAS_FCU(m_uas)->send_message_ignore_drop(m);
// repeat if request have more data, pkt_frag++

[AutonauticsTeam]

Thanks for this vooon! So we feed a rostopic with the data and then transform this in the plugin to be sent as a mavlink msg? I understand the fragmented however what is the difference between the fragment id and sequence id?

[vooon]

As i understands, FCU should rebuild RTCM packet before send it to GPS. So you need packet sequence number (same for all fragments, [0-31]) and fragment number [0-3].

If message smaller than payload size, it may be send in one message; otherwise you have to split to segments. After that message was sent, you need to update seq (keep in mind, that it is 5-bit).

[AutonauticsTeam]

Hi vooon fantastic, we have completed the following below and tested.

Thanks for all your help

namespace mavros {
namespace std_plugins {
class RTCM3Plugin : public plugin::PluginBase {
public:
    RTCM3Plugin() : PluginBase(),
        nh("~")
    { }

    void initialize(UAS &uas_)
    {
        PluginBase::initialize(uas_);
        //ROS_INFO_NAMED("rtcm3", "RTCM3Plugin::initialize");
        //ROS_INFO_NAMED("rtcm3", "rtcm3");
        rtcm3_sub = nh.subscribe("/rtcm_stream", 1000, &RTCM3Plugin::rtcm3_cb, this);
    }
    Subscriptions get_subscriptions() {
        return{};
    }

private:
    ros::NodeHandle nh;
    ros::Subscriber rtcm3_sub;
    int mlim = 180;
    int rtcm_seq = 0;
    int flag;
void rtcm3_cb(const mavros_msgs::RTCM3::ConstPtr &req)
{
int i,j,msz,p = 0;
//using mavlink::common::msg::GPS_RTCM_DATA;
//GPS_RTCM_DATA m{};
mavlink::common::msg::GPS_RTCM_DATA m{};
std::vector<unsigned char> data; //data from ROS
data = req->data; //ROS to data var
msz = data.size(); // fill length of msg
bool fragmented = (msz / mlim) ? 1 : 0;
p = msz / mlim;
if(msz % mlim != 0){p++;}
if(p <= 4){int pkt_frag = 0;
    if (data.size()>mlim){//if msg in is greater than msg limit
    //int pkt_frag = 0;
    int pb = 0;
                for (i=0;i<p;i++){
                    m.flags = (rtcm_seq << 3) | (pkt_frag << 1) | fragmented;
                    for (j=i*mlim; j<(i+1)*mlim && j < msz; j++){m.data[pb] = data[pb];} // FILL ARRAY FROM VECTOR
                    pkt_frag += 1;
                    m.len = pb;
                    UAS_FCU(m_uas)->send_message_ignore_drop(m);
                }
        }
        else{
            for (j=0; j<msz; j++){m.data[j] = data[j];}// FILL ARRAY FROM VECTOR
            m.len = msz;
            m.flags = (rtcm_seq << 3) | (pkt_frag << 1) | fragmented;
            UAS_FCU(m_uas)->send_message_ignore_drop(m);
        }
rtcm_seq++;
if(rtcm_seq > 31){rtcm_seq = 0;}
}

else
{
     ROS_INFO("Error RTCM3 Parse exceeded fragmentation limit, (Corrupt or too Large)");
}
}
};
}   // namespace std_plugins
}   // namespace mavros
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::RTCM3Plugin, mavros::plugin::PluginBase)

[vooon]

Looks too messy. mlim should not be used, GPS_RTCM_DATA::data.size() (which is 180). Next, you may use iterator of req->data.begin(), then use it to copy fragment to mavlink message.

uint8_t common_flags = (rtcm_seq << 3) | ((req->data.size() > m.data.size()) ? 1 : 0);
...
m.flags = common_falgs | (pkt_frag << 1);

[vooon]

Next, check that data size is less than allowed first, warn if not and return.

[vooon]

Seq limit:

rtcm_seq++;
rtcm_seq &= 0x1f; // limit to 5 bit.

[vooon]

Pseudo code:

const size_t PKT_FRAG_MAX = 4;
const auto m_data_max = m.data.size();

auto it = req->data.cbegin();
auto end = req->data.cend();

for (uitn8_t pkt_frag = 0; pkt_frag < PKT_FRAG_MAX && it != end; pkt_frag++) {

  size_t len = std::min(m_data_max, std::distance(it, end));

  // ensure that array is zeroed
  if (len < m_data_max)
    m.data.fill(0);

  m.flags = common_flags | (pkt_frag << 1);
  m.len = len;
  std::copy(it, it+len, m.data.begin());
  it += len;

  // send

}

[AutonauticsTeam]

Sorry have been really busy, thanks for the feedback vooon, I will get to implementing this in the coming days.

[TSC21]

@AutonauticsTeam what's the state of this matter? Do you have usable code that you can add on a PR and all can benefit from it?

[TSC21]

Closing as stale.