How to send waypoint to my flight card ?

[SALTU-19]

i have f4 flight card with inav and raspberry pi zero.I want to send waypoint to my flight card with set_wp.I have an error("message v2 with id 209 is not recognized").What am i gonna do ?

@christianrauch

[christianrauch]

I have not tested the MSP_SET_WP message before. If you provide a minimal source example to reproduce this issue (as plain text, not screenshots of text) I can try to reproduce this in my Betaflight setup.

You can increase the logger level to debug to get more informative output. Looking at the source code, receiving this error indicates that ok_id is false, which means that the direction byte is !. This is typically the case if the FCU does not support the requested MSP ID.

Your EOF message may further indicate a connection error. A request with an unsupported MSP ID should not cause connection issues. Have you encountered other connection issues over your serial connection?

Can you:

1. Test this with an MSPv1 connection instead of the MSPv2 connection.
2. Verify that this message works correctly with the iNav GUI with MSP version 1 and 2 connections.
3. Test the connection via a USB connection (USB-to-Serial adapter on FCU) to rule out disturbances with unshielded plain serial connections.

[DC-SWAT]

This message has a bug. Missed one uint8 param - action.

Fixed code:

// MSP_SET_WP: 209
struct SetWp : public Message {
    SetWp(FirmwareVariant v) : Message(v) {}

    virtual ID id() const override { return ID::MSP_SET_WP; }

    Value<uint8_t> number;
    Value<uint8_t> action;
    Value<int32_t> lat;
    Value<int32_t> lon;
    Value<int32_t> alt;

    Value<int16_t> p1;
    Value<int16_t> p2;
    Value<int16_t> p3;
    Value<uint8_t> nav_flag;

    virtual ByteVectorUptr encode() const override {
        ByteVectorUptr data = std::make_unique<ByteVector>();
        bool rc             = true;
        rc &= data->pack(number);
        rc &= data->pack(action);
        rc &= data->pack(lat);
        rc &= data->pack(lon);
        rc &= data->pack(alt);
        rc &= data->pack(p1);
        rc &= data->pack(p2);
        if(fw_variant == FirmwareVariant::INAV) {
            rc &= data->pack(p3);
        }
        rc &= data->pack(nav_flag);
        return data;
    }
};

[DC-SWAT]

Also I find a bug in sendMessage at decoding empty payload data.

decode_success = message.decode(data);

Change to:

decode_success = data.size() == 0 || message.decode(data);

SET_WP has empty payload data in answer. After there fixes it's works for me on INAV.

I fixed in this library and some other things, maybe will made pull request.

[christianrauch]

This message has a bug. Missed one uint8 param - action.

The original MSP_SET_WP message definition matches the common fields in the SetWp struct. The struct contains special fields for different firmwares, e.g. a third uint16_t is added if fw_variant == FirmwareVariant::INAV. The iNav message definition might have changed in newer firmware versions and we therefore have to version the message definition too.

@DC-SWAT Is there an official document with the iNav message definitions? Could you provide a PR that adapts the iNav specific definition? I am not using iNav, so I cannot test the new definition myself.

At the moment, we only differentiate between different firmware types, not versions. For simplicity, we could always update message definitions to the newest version of that firmware and thus completely neglecting backward compatibility to earlier firmware versions. Otherwise, you have to provide your own definition of MSP_SET_WP. I am not really interested in updating message definitions for every new version of every MultiWii derived firmware. Unfortunately, the set of available messages and their definitions differ between firmwares and versions.

decode_success = data.size() == 0 || message.decode(data);

SET_WP has empty payload data in answer. After there fixes it's works for me on INAV.

Thanks for the hint. Is this causing the EOF error?

I am using MSP_SET_RAW_RC, which also does not have a returned payload to decode (as it is a request), but I have not received any errors using it. I have to check this again. Ignoring the payload of received requests is the correct behaviour. None of the message definitions I have seen so far have a two-way payload. A message only requires either an encode (for sending requests to the FC) or a decode method (for receiving responses from the FC).

[SALTU-19]

#include <FlightController.hpp>
#include
#include <msp_msg.hpp>

#define MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT 0x01

struct SetWp : public Message {
SetWp(FirmwareVariant v) : Message(v) {}

virtual ID id() const override { return ID::MSP_SET_WP; }

Value<uint8_t> wp_no;
Value<uint8_t> action;
Value<uint32_t> lat;
Value<uint32_t> lon;
Value<uint32_t> alt;

Value<uint16_t> p1;
Value<uint16_t> p2;
Value<uint16_t> p3;
Value<uint8_t> nav_flag;

virtual ByteVectorUptr encode() const override {
    ByteVectorUptr data = std::make_unique<ByteVector>();
    bool rc             = true;
    rc &= data->pack(wp_no);
rc &= data->pack(action);
    rc &= data->pack(lat);
    rc &= data->pack(lon);
    rc &= data->pack(alt);
    rc &= data->pack(p1);
    rc &= data->pack(p2);
    if(fw_variant == FirmwareVariant::INAV) {
        rc &= data->pack(p3);
    }
    rc &= data->pack(nav_flag);
    return data;
}
};

int main(int argc, char* argv[]) {
msp::FirmwareVariant fw_variant = msp::FirmwareVariant::INAV;
const std::string device =
(argc > 1) ? std::string(argv[1]) : "/dev/ttySUB0";
const size_t baudrate = (argc > 2) ? std::stoul(argv[2]) : 115200;

fcu::FlightController fcu;
fcu.setLoggingLevel(msp::client::LoggingLevel::INFO);
// wait for connection
fcu.connect(device, baudrate);

msp::msg::SetWp setwp(fw_variant);

setwp.wp_no = 255;
setwp.action = MSP_NAV_STATUS_WAYPOINT_ACTION_WAYPOINT;
setwp.lat = 40811854;
setwp.lon = 29360025;
setwp.alt = 1500;
setwp.p1 = 80;
setwp.p2 = 0;
setwp.p3 = 0;
setwp.nav_flag = 0;

if (fcu.sendMessage(setwp) ) {
    std::cout << "Acknowled";
}
else
    std::cout << "wasted";
}

This is the final version of my code.

I still couldn't solve my problem. I can't set a waypoint.

[christianrauch]

@SALTU-19 Is comment https://github.com/christianrauch/msp/issues/49#issuecomment-685405532 relevant to you? Does it work if you change the MSP_SET_WP message definition accordingly?

[SALTU-19]

I edited it based on that comment but I still get an error:

Message v2 with ID 209 is not recognised! wastedbuffer returned EOF; reading char directly from port terminate called after throwing an instance of 'std::system_error' what(): read: Bad file descriptor Aborted

I can't fix this :(

[SALTU-19]

Also I find a bug in sendMessage at decoding empty payload data.

decode_success = message.decode(data);

Change to:

decode_success = data.size() == 0 || message.decode(data);

SET_WP has empty payload data in answer. After there fixes it's works for me on INAV.

I fixed in this library and some other things, maybe will made pull request.

@DC-SWAT i couldn't find the line that you've changed. could you tell me how can i find and change it more precisely?

[christianrauch]

@DC-SWAT i couldn't find the line that you've changed. could you tell me how can i find and change it more precisely?

I added that fix to the PR #48 .

[christianrauch]

@SALTU-19 I added the action field to MSP_SET_WP in PR #48 . Your C++ example code seems to have some syntax issues. In any case, I am not able to test this message as it is not supported by betaflight.

[DC-SWAT]

I used this info for MSP_SET_WP: https://docs.google.com/document/d/16ZfS_qwc-rJeA7N5Tx0DA6wtgxl6HdGgaz-jE3lHBWs/edit https://github.com/iNavFlight/inav/blob/master/src/main/fc/fc_msp.c#L2426

Also need change uint32 to int32 in your code, for MSP_SET_WP and also for MSP_SONAR_ALTITUDE. Because MSP_SONAR_ALTITUDE can return -1 as altitude, so for uint32 we got a very big value instead of -1 (no data).

[SALTU-19]

@SALTU-19 I added the action field to MSP_SET_WP in PR #48 . Your C++ example code seems to have some syntax issues. In any case, I am not able to test this message as it is not supported by betaflight.

Like what ? Syntax issues

[christianrauch]

Like what ? Syntax issues

Yes. For example your second #include is not including any header or the use of Value is missing the msp namespace. A compiler will not be able to compile this source file without changes.

[SALTU-19]

Like what ? Syntax issues

Yes. For example your second #include is not including any header or the use of Value is missing the msp namespace. A compiler will not be able to compile this source file without changes.

I think that part of the code was deleted during copying. it was #include <iostream> . Are there any other errors you see?

[SALTU-19]

I used this info for MSP_SET_WP: https://docs.google.com/document/d/16ZfS_qwc-rJeA7N5Tx0DA6wtgxl6HdGgaz-jE3lHBWs/edit https://github.com/iNavFlight/inav/blob/master/src/main/fc/fc_msp.c#L2426

Also need change uint32 to int32 in your code, for MSP_SET_WP and also for MSP_SONAR_ALTITUDE. Because MSP_SONAR_ALTITUDE can return -1 as altitude, so for uint32 we got a very big value instead of -1 (no data).

@DC-SWAT Have you ever used MSP_SET_WP. Can you share me a sample code?

[DC-SWAT]

Just fix includes and replace SetWp in your code:

#include <msp/Client.hpp>
#include <msp/FlightController.hpp>

// change
msp::msg::SetWp setwp(fw_variant);
// to
SetWp setwp(fw_variant);

But anyway you got false from sendMessage, another fix is ​​needed which I described above. Waypoint send normally, it's just wrong returned result.

[christianrauch]

I moved the fix for the MSP_SET_WP and the fix for your example code to a custom inav_wp branch https://github.com/christianrauch/msp/tree/inav_wp. Again, as I do not have a iNav FCU, I cannot test this. For now, I can only rely on the public documentation and the source code.

@SALTU-19 @DC-SWAT I would be happy if you could test this branch and send a PR that fixes remaining issues.

[SALTU-19]

I moved the fix for the MSP_SET_WP and the fix for your example code to a custom inav_wp branch https://github.com/christianrauch/msp/tree/inav_wp. Again, as I do not have a iNav FCU, I cannot test this. For now, I can only rely on the public documentation and the source code.

@SALTU-19 @DC-SWAT I would be happy if you could test this branch and send a PR that fixes remaining issues.

i tried this code today and my drone is heavily broken.What do you think about this ? it did not go to the waypoint I gave it. @christianrauch

Edit:A friend thinks that i try mspv2. What i am gonna do ?

[christianrauch]

Was the MSP_SET_WP message accepted by your FCU or did you get the same error message (is not recognised)? I cannot advise you how to use iNav, I can only try to solve issues related to the communication library (msp) in this repo.

If the new message definition is accepted by iNav, I will merge it into the repo and close this issue.

[SALTU-19]

Was the MSP_SET_WP message accepted by your FCU or did you get the same error message (is not recognised)? I cannot advise you how to use iNav, I can only try to solve issues related to the communication library (msp) in this repo.

If the new message definition is accepted by iNav, I will merge it into the repo and close this issue.

ı didn’t get error. It was accept but it did not go to the waypoint i gave

[christianrauch]

ı didn’t get error. It was accept

Thanks for checking this. This means that the wrong (or outdated) message definition caused the communication issue and that you can now send waypoints to iNav.

but it did not go to the waypoint i gave

There are many reasons beyond the MSP communication why the FCU does not behave as intended. If the waypoint was received and accepted by the FC, then there is nothing more what this library can do. You should check the FCU logs for any issues or warnings.

[SALTU-19]

ı didn’t get error. It was accept

Thanks for checking this. This means that the wrong (or outdated) message definition caused the communication issue and that you can now send waypoints to iNav.

but it did not go to the waypoint i gave

There are many reasons beyond the MSP communication why the FCU does not behave as intended. If the waypoint was received and accepted by the FC, then there is nothing more what this library can do. You should check the FCU logs for any issues or warnings.

I entered the speed, altitude in cm and is there a problem due to the entry of latitude and longitude ?

[christianrauch]

I entered the speed, altitude in cm and is there a problem due to the entry of latitude and longitude ?

I am not sure what you are asking me. I have never used iNav, let alone it's waypoint functionality. On how to use the waypoint functionality, you would have to check the iNav documentation or the user forums. As far as I can tell from your feedback, the waypoint message (MSP_SET_WP) was sent correctly to the FCU. How it is used by the FCU is outside the scope of this project.