search

mavlink / MAVSDK

API and library for MAVLink compatible systems written in C++17
https://mavsdk.mavlink.io
BSD 3-Clause "New" or "Revised" License
614 stars 502 forks source link

Not sending message rc_channels_override #2220

Closed AnatolyAG closed 6 months ago

AnatolyAG commented 7 months ago

Hi there. I use MAVSDK version: v2.1.0

I want to emulate controlling the drone as if through a joystick. On python I used vehicle.channels.overrides['3'] = 2000 to control the elevation, and overwriting other channels. vehicle.channels.overrides['1'] = 1500 vehicle.channels.overrides['2'] = 1500 vehicle.channels.overrides['4'] = 1500 for flight control. Now I am trying to do the same thing using c++. I made a function that sends messages

void send_rc_channels_override(
    mavsdk::MavlinkPassthrough &mavlink_passthrough,
    uint8_t channel,
    uint16_t value)
{
    mavlink_passthrough.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel)
        {
            mavlink_message_t message;
    uint16_t chan1_raw = 1;
    uint16_t chan2_raw = 1;  
    uint16_t chan3_raw = 1; 
    uint16_t chan4_raw = 1;  
    uint16_t chan5_raw = 1;  
    uint16_t chan6_raw = 1;  
    uint16_t chan7_raw = 1;  
    uint16_t chan8_raw = 1;  
    uint16_t chan9_raw = 1;  
    uint16_t chan10_raw = 0 ; 
    uint16_t chan11_raw = 0;  
    uint16_t chan12_raw = 0;  
    uint16_t chan13_raw = 0;  
    uint16_t chan14_raw = 0;  
    uint16_t chan15_raw = 0;  
    uint16_t chan16_raw = 0;  
    uint16_t chan17_raw = 0;  
    uint16_t chan18_raw = 0;

    mavlink_msg_rc_channels_override_pack_chan(
        mavlink_address.system_id, //uint8_t system_id,
        mavlink_address.component_id,//uint8_t component_id,
        0,  //uint8_t chan,
        &message, //mavlink_message_t * msg,
        mavlink_passthrough.get_target_sysid(), //uint8_t target_system, 
        mavlink_passthrough.get_target_compid(), // target_component, 
        chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw,
        chan10_raw, chan11_raw, chan12_raw, chan13_raw, chan14_raw, chan15_raw, chan16_raw, chan17_raw, chan18_raw);

    std::cout << "channel - " << channel << " id - " << (int)message.msgid << std::endl;

    return message;
 });

}

I called this function 

while (std::abs(dron_alt_curr - dron_alt_set) > 0.01)
    { //
        std::cout << "Try fly\n";
        send_rc_channels_override(mavlink_passthrough, 3, 2000);
        sleep_for(milliseconds(100));
    }

and using the sniffer example I subscribe to the messages I want on the other port.

mavsdk.intercept_incoming_messages_async([](mavlink_message_t& message) {
        if ((int)message.msgid == 0)
            return true;
        if (message.msgid != MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE)
        {
            return true;
        }

        std::cout << "Got message " << (int)message.msgid << '\n';

        std::cout << "  System ID: " << (int)message.sysid << std::endl;
        std::cout << "  Component ID: " << (int)message.compid << std::endl;
        std::cout << "  Sequence: " << (int)message.seq << std::endl;
        std::cout << "  Message length: " << (int)message.len << std::endl;
        // std::cout << "  Message name: " << messagename << std::endl;
        std::cout << "  Payload:" << std::endl;
        for (int i = 0; i < message.len; ++i) {
            std::cout << "    Byte " << i << ": " << (int)message.payload64[i] << std::endl;
        }
        return true;
    });

But the emulator does not react to my messages and after a couple of seconds goes from armed to disarmed state. Maybe I'm doing something wrong, maybe there are examples of how to do it correctly? Or is there another way for solve my problem?

julianoes commented 7 months ago

One nitpick:

mavlink_msg_rc_channels_override_pack_chan(
        mavlink_address.system_id, //uint8_t system_id,
        mavlink_address.component_id,//uint8_t component_id,
        0,  //uint8_t chan,
        &message, //mavlink_message_t * msg,

0 for chan should be channel from the function arguments.

Next step for you is to check whether the message arrive on the PX4 side. Have you verified that?

AnatolyAG commented 7 months ago

Yes, I use channel from argument, I was the one who tried to go through the channels manually.

I usually do this

void send_rc_channels_override(
    mavsdk::MavlinkPassthrough &mavlink_passthrough,
    uint8_t channel,
    uint16_t value)
{

    mavlink_passthrough.queue_message([&](MavlinkAddress mavlink_address, uint8_t channel)
                                      {
            mavlink_message_t message;
    uint16_t chan1_raw = 1;
    uint16_t chan2_raw = 1;  
    uint16_t chan3_raw = 1; 
    uint16_t chan4_raw = 1;  
    uint16_t chan5_raw = 1;  
    uint16_t chan6_raw = 1;  
    uint16_t chan7_raw = 1;  
    uint16_t chan8_raw = 1;  
    uint16_t chan9_raw = 1;  
    uint16_t chan10_raw = 0 ; 
    uint16_t chan11_raw = 0;  
    uint16_t chan12_raw = 0;  
    uint16_t chan13_raw = 0;  
    uint16_t chan14_raw = 0;  
    uint16_t chan15_raw = 0;  
    uint16_t chan16_raw = 0;  
    uint16_t chan17_raw = 0;  
    uint16_t chan18_raw = 0;

    mavlink_msg_rc_channels_override_pack_chan(
        mavlink_address.system_id, //uint8_t system_id,
        mavlink_address.component_id,//uint8_t component_id,
        channel,  //uint8_t chan,
        &message, //mavlink_message_t * msg,
        mavlink_passthrough.get_target_sysid(), //uint8_t target_system, 
        mavlink_passthrough.get_target_compid(), // target_component, 
        chan1_raw, chan2_raw, chan3_raw, chan4_raw, chan5_raw, chan6_raw, chan7_raw, chan8_raw, chan9_raw,
        chan10_raw, chan11_raw, chan12_raw, chan13_raw, chan14_raw, chan15_raw, chan16_raw, chan17_raw, chan18_raw);

    std::cout << "channel - " << channel << " id - " << (int)message.msgid << std::endl;

    return message;
 });

}

and I subscribe message 

mavsdk.intercept_incoming_messages_async([](mavlink_message_t& message) {
        if ((int)message.msgid == 0)
            return true;
        if (message.msgid != MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE)
        {
           return true;
        }

        std::cout << "Got message " << (int)message.msgid << '\n';

       std::cout << "  System ID: " << (int)message.sysid << std::endl;
        std::cout << "  Component ID: " << (int)message.compid << std::endl;
        std::cout << "  Sequence: " << (int)message.seq << std::endl;
        std::cout << "  Message length: " << (int)message.len << std::endl;
        // std::cout << "  Message name: " << messagename << std::endl;
        std::cout << "  Payload:" << std::endl;
        for (int i = 0; i < message.len; ++i) {
            std::cout << "    Byte " << i << ": " << (int)message.payload64[i] << std::endl;
        }
        return true;
    });

filtering message message.msgid != MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE

snifer connect to drone by another port, maybe its wrong way? but i dont know how i can see recived message in dron. I use mission planer , and sketch 'ArduCopter'. From python (use dronekit) its very good control by channel override, but from c++ I can't get a result.

this is output from message sender

~/code/dron_cpp$ ./build/test 
[12:03:31|Info ] MAVSDK version: v2.1.0 (mavsdk_impl.cpp:26)
[12:03:31|Debug] New system ID: 1 Comp ID: 1 (mavsdk_impl.cpp:737)
[12:03:31|Debug] Component Autopilot (1) added. (system_impl.cpp:388)
[12:03:31|Warn ] Vehicle type changed (new type: 2, old type: 0) (system_impl.cpp:230)
[12:03:31|Debug] Discovered 1 component(s) (system_impl.cpp:563)
Current flight mode: Altctl
Check until vehicle is ready to arm11111
Almost arming -> go arm
Arming...
Taking off...
Try fly
channel -  id - 70
[12:03:31|Debug] MAVLink: info: Arming motors (system_impl.cpp:256)
[12:03:31|Debug] New system ID: 245 Comp ID: 190 (mavsdk_impl.cpp:737)
[12:03:31|Debug] Component Ground station (190) added. (system_impl.cpp:388)
[12:03:31|Debug] Discovered 1 component(s) (system_impl.cpp:563)
Try fly
channel -  id - 70
Try fly
channel -  id - 70
Try fly
channel -  id - 70
Try fly
channel -  id - 70
Try fly
channel -  id - 70
[12:03:32|Debug] New system ID: 255 Comp ID: 190 (mavsdk_impl.cpp:737)
[12:03:32|Debug] Component Ground station (190) added. (system_impl.cpp:388)
[12:03:32|Debug] Discovered 1 component(s) (system_impl.cpp:563)
Try fly
channel -  id - 70

how you can see channel - empty std::cout << "channel - " << channel << " id - " << (int)message.msgid << std::endl; - this line code channel - id - 70 - result output

this is output from sniffer

~/code/MAVSDK/examples/sniffer$ ./build/sniffer 
[12:02:30|Info ] MAVSDK version: v2.1.0 (mavsdk_impl.cpp:26)
[12:02:30|Debug] New system ID: 1 Comp ID: 1 (mavsdk_impl.cpp:737)
[12:02:30|Debug] Component Autopilot (1) added. (system_impl.cpp:388)
[12:02:30|Warn ] Vehicle type changed (new type: 2, old type: 0) (system_impl.cpp:230)
[12:02:30|Debug] Discovered 1 component(s) (system_impl.cpp:563)
[12:03:31|Debug] MAVLink: info: Arming motors (system_impl.cpp:256)
[12:03:31|Debug] Component Unsupported component (195) added. (system_impl.cpp:388)
[12:03:41|Debug] MAVLink: info: Disarming motors (system_impl.cpp:256)

Im remove filtering message msgid != MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE and get output from sniffer

this output from test code 

~/code/dron_cpp$ ./build/test 
[12:12:24|Info ] MAVSDK version: v2.1.0 (mavsdk_impl.cpp:26)
[12:12:24|Debug] New system ID: 1 Comp ID: 1 (mavsdk_impl.cpp:737)
[12:12:24|Debug] Component Autopilot (1) added. (system_impl.cpp:388)
[12:12:24|Warn ] Vehicle type changed (new type: 2, old type: 0) (system_impl.cpp:230)
[12:12:24|Debug] Discovered 1 component(s) (system_impl.cpp:563)
Current flight mode: Altctl
Check until vehicle is ready to arm11111
Almost arming -> go arm
Arming...
[12:12:24|Debug] MAVLink: info: Arming motors (system_impl.cpp:256) ---- can sync with snifer
Taking off...
Try fly
channel -  id - 70
Try fly
channel -  id - 70
Try fly
channel -  id - 70
Try fly
channel -  id - 70
Try fly
channel -  id - 70
[12:12:24|Debug] New system ID: 245 Comp ID: 190 (mavsdk_impl.cpp:737)
[12:12:24|Debug] Component Ground station (190) added. (system_impl.cpp:388)
[12:12:24|Debug] Discovered 1 component(s) (system_impl.cpp:563)
[12:12:25|Debug] New system ID: 255 Comp ID: 190 (mavsdk_impl.cpp:737)
[12:12:25|Debug] Component Ground station (190) added. (system_impl.cpp:388)
[12:12:25|Debug] Discovered 1 component(s) (system_impl.cpp:563)
Try fly
channel -  id - 70
...........

Try fly
channel -  id - 70
[12:12:34|Debug] MAVLink: info: Disarming motors (system_impl.cpp:256)  ---- can sync with sniffer
Try fly
channel -  id - 70
Try fly
channel -  id - 70
Try fly
channel -  id - 70
......

this outpu sniffer

Got message 253
  System ID: 1
  Component ID: 1
  Sequence: 163
  Message length: 14
  Payload:
    Byte 0: 1836204294
    Byte 1: 1869901677
    Byte 2: 0
    Byte 3: 0
    Byte 4: 0
    Byte 5: 0
    Byte 6: 0
    Byte 7: 0
    Byte 8: 760237908
    Byte 9: 4277302
    Byte 10: 0
    Byte 11: 0
    Byte 12: 0
    Byte 13: 0
[12:12:24|Debug] MAVLink: info: Arming motors (system_impl.cpp:256)   ---- can sync with test ouptut
Got message 4
  System ID: 1
  Component ID: 195
  Sequence: 0
  Message length: 4
  Payload:
    Byte 0: 1775070543
    Byte 1: 0
    Byte 2: 0
    Byte 3: 0
[12:12:25|Debug] Component Unsupported component (195) added. (system_impl.cpp:388)
Got message 4
  System ID: 1
  Component ID: 195
  Sequence: 1
  Message length: 4
  Payload:
    Byte 0: 1780048374 ,    Byte 1: 0,     Byte 2: 0,     Byte 3: 0
Got message 4
  System ID: 1
  Component ID: 195
  Sequence: 2
  Message length: 4
  Payload:
    Byte 0: 1780081510,     Byte 1: 0,    Byte 2: 0,    Byte 3: 0
Got message 111
  System ID: 255
  Component ID: 190
  Sequence: 202
  Message length: 14
  Payload:
    Byte 0: 322169088    Byte 1: 1191547481    Byte 2: 0    Byte 3: 0
    Byte 4: 0    Byte 5: 0    Byte 6: 0    Byte 7: 0    Byte 8: 760237908
    Byte 9: 4277302    Byte 10: 0    Byte 11: 0    Byte 12: 0    Byte 13: 0
Got message 111
  System ID: 1
  Component ID: 1
  Sequence: 172
  Message length: 14
  Payload:
    Byte 0: 0    Byte 1: 1931251481    Byte 2: 0    Byte 3: 0
    Byte 4: 0    Byte 5: 0    Byte 6: 0    Byte 7: 0    Byte 8: 760237908
    Byte 9: 4277302    Byte 10: 0    Byte 11: 0    Byte 12: 0    Byte 13: 0
Got message 253
  System ID: 1
  Component ID: 1
  Sequence: 175
  Message length: 17
  Payload:
    Byte 0: 1936278534    Byte 1: 1830840174    Byte 2: 115    Byte 3: 0
    Byte 4: 0    Byte 5: 0    Byte 6: 0    Byte 7: 0    Byte 8: 760237908
    Byte 9: 4277302    Byte 10: 0    Byte 11: 0    Byte 12: 0
    Byte 13: 0    Byte 14: 0    Byte 15: 0    Byte 16: 0
[12:12:34|Debug] MAVLink: info: Disarming motors (system_impl.cpp:256)
Got message 22
  System ID: 1
  Component ID: 1
  Sequence: 176
  Message length: 25
  Payload:
    Byte 0: 1051598246    Byte 1: 1599360845    Byte 2: 1448036447    Byte 3: 9    
    Byte 4: 0                      Byte 5: 0                      Byte 6: 0                     Byte 7: 0
    Byte 8: 760237908      Byte 9: 4277302          Byte 10: 0                   Byte 11: 0
    Byte 12: 0                    Byte 13: 0                    Byte 14: 0                   Byte 15: 0
    Byte 16: 0                    Byte 17: 0                    Byte 18: 0                   Byte 19: 0 
    Byte 20: 0                    Byte 21: 0                    Byte 22: 0                   Byte 23: 0    Byte 24: 0
Got message 4
  System ID: 1
  Component ID: 195
  Sequence: 3
  Message length: 4
  Payload:
    Byte 0: 1785050240    Byte 1: 0    Byte 2: 1448036447    Byte 3: 9
Got message 4
  System ID: 1
  Component ID: 195
  Sequence: 4
  Message length: 4
  Payload:
    Byte 0: 1785102422    Byte 1: 0    Byte 2: 1448036447    Byte 3: 9
Got message 66
  System ID: 255
  Component ID: 190
  Sequence: 211
  Message length: 6
  Payload:
    Byte 0: -1091239934    Byte 1: 3    Byte 2: 1448036447    Byte 3: 9    Byte 4: 0    Byte 5: 0

maybe there are other ways?

julianoes commented 6 months ago

So, the messages are arriving but PX4 doesn't use them? Or ArduPilot?

AnatolyAG commented 6 months ago

So apparently it doesn't work for the ArduPilot.

I use Mission planer - this is output when start

Setting SIM_SPEEDUP=1.000000
Suggested EK3_DRAG_BCOEF_* = 16.288, EK3_DRAG_MCOEF = 0.209
Home: 50.450725 30.461211 alt=160.800000m hdg=0.000000
Starting sketch 'ArduCopter'
Starting SITL input
Using Irlock at port : 9005
bind port 5760 for 0
Serial port 0 on TCP port 5760
Loaded defaults from ....\Mission Planner\sitl\default_params/copter.parm
bind port 5762 for 2
Serial port 2 on TCP port 5762
bind port 5763 for 3
Serial port 3 on TCP port 5763
Smoothing reset at 0.001
validate_structures:498: Validating structures
Loaded defaults from .....\Mission Planner\sitl\default_params/copter.parm
New connection on serial port 0

I connect to 5762 port - control script and connect to 5763 port - sniffer script.

I can't use this library for this case?

julianoes commented 6 months ago

Are the messages arriving, yes or no?

AnatolyAG commented 6 months ago

Yeah, apparently the message goes out and comes in, but the message code is wrong. I may have phrased the question incorrectly.

julianoes commented 6 months ago

I don't think I can help you on the message code. That's what you have to figure out between passthrough and ArduPilot.

AnatolyAG commented 6 months ago

Thanks. I realized that the library works in a peculiar way with ArduPilot.

JonasVautherin commented 6 months ago

What do you mean by peculiar? Is there something to fix in MAVSDK? Can you elaborate a bit, in case someone else has the same issue?

AnatolyAG commented 6 months ago

Don't mind me, it's a translation problem. Let me describe again what I wanted to do and what I got.

I wanted to programmatically control the drone like an operator controls FPV, I see it this way, I write code that sends messages to the autopilot drone as if the operator from the remote control. For experiments I chose mission planer, in it you can run emulation of different drones, I chose quadrocopter. Accordingly, it can be controlled by rewriting the control channels (which in my understanding emulates a joystick or remote control). I wrote the code in python using the dronekit library, in AltHold mode. in this way vehicle.channels.overrides['3'] = 2000 - I got the drone to the desired altitude, as the drone climbed I checked the current altitude from telemetry and upon reaching it using the channels 

vehicle.channels.overrides['2'] # Pitch control.
vehicle.channels.overrides['1'] 0 # roll control.

I controlled the flight of the drone, as a result it is possible to emulate a fast flight and react to the current environment if you read and analyze the data from the sensors. Then I tried using MAVSDK library to repeat the same actions and failed, for experiments I used the examples that come with the library, with a small modification. From the noticed confusions :

  1. Never once worked, everywhere disabled 
    while (telemetry.health_all_ok() != true) {
    std::cout << "Vehicle is getting ready to arm\n";
    sleep_for(seconds(1));
}
  2. Attempts to send a message to overwrite the channel as described above failed.
  3. Using Offboard Mod - only managed to take off and at the same time it is changing the control mode.
  4. Using Manual Control mode - failed to launch even in the example, right after the start there is a change of mode and the drone goes into disarm mode.

Here is a short what was the task and how it ended. If you can suggest something, I will be grateful. If something I did wrong - I apologize, I'm just learning well and apparently how to do it correctly did not find, so do not kick too much.