New MAVLink message implementation to ArduPilot

[Karl]

Quick word on my goal: I'm implementing an "advanced fail safe messaging system" that has the goal of putting the user in the control loop. If something goes wrong (GPS loss, low battery...) the areal vehicle will propose solutions to the user (return to launch, fire parachute, switch to manual control...), the ground station will prompt a window where the user can click on a solution within a time window, otherwise the UAV will take a default action.

I'm having a hard time implementing a new MAVLink message in ArduPilot. I successfully updated the MAVLink headers and ArduPilot firmware with everything needed to send my new message to the ground station (I'm using QGroundControl). The ArduPilot compiles well and the code I added, but I don't have any feedback and on the QGroundControl side I can't see the passed custom message (I've updated the headers there too). I'm wondering if my implementation is correct.

In CGS_Common.cpp I added the following static method, which I based on GCS_MAVLINK::send_statustext_all:

void GCS_MAVLINK::send_afs_message(MAV_SEVERITY severity, uint8_t timeout, AFS_SOLUTIONS *solutions, const prog_char_t *str, ...)
{
    for (uint8_t i=0; i<MAVLINK_COMM_NUM_BUFFERS; i++) {
        if ((1U<<i) & mavlink_active) {
            mavlink_channel_t chan = (mavlink_channel_t)(MAVLINK_COMM_0+i);
            if (comm_get_txspace(chan) >= MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_STATUSTEXT_LEN) {
                char msg2[50];
                va_list arg_list;
                va_start(arg_list, str);
                hal.util->vsnprintf_P((char *)msg2, sizeof(msg2), str, arg_list);
                va_end(arg_list);
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, PSTR("Sending AFS message...")); // Debug
                mavlink_msg_afs_message_send(chan,
                                             1, // ID that must be included with reply
                                             severity,
                                             timeout,
                                             msg2,
                                             (const uint8_t*)solutions);
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, PSTR("AFS message sent")); // Debug
            }
        }
    }
}

Right now I'd like to know if my message is actually sent by ArduPilot, currently I use MAVProxy/SITL to simulate the areal vehicle. I don't see my custom message in the MAVProxy console, I assume that is because MAVProxy does not know how to handle the custom message as I have not implemented it there... but does it still forward them?

[Tom Pittenger]

Karl,

Holy smokes, you're using va_list in a loop?!?! Please don't do that.

Next, are you editing the xml file? You can't just edit a header. The xml triggers compiling a bunch of supporting source code that is required to transmit it. It's all automatic.

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Karl]

Hi Tom. I was using GCS_MAVLINK::send_statustext_all as reference, I didn't know what va_list does... something like an environment to copy from PROGMEM to a variable I guess?

I updated the MAVLink using the proper procedure: first the XML, then generate.sh, copied the generated header files to ArduPilot and QGroundControl and implemented some code to interact with the new functions.

[Tom Pittenger]

Oh geez, if  GCS_MAVLINK::send_statustext_all is doing that then we should change that. The generate.sh is no longer needed for ArduPilot, it all done automatially. The generated code is no longer committed, only the xml.

[Karl]

GCS_MAVLINK::send_statustext_all works well enough for me.

I copied the modified ardupilotmega.xml file to the \libraries\GCS_MAVLink\message_definitions folder, but I still don't see my message. I kind of suspect that the message is not even sent out, that's why I'm asking here, but I have no proof.

[Tom Pittenger]

Try raising the severity. Then revert to the old method and see if that works.

[Karl]

You mean MAV_SEVERITY_INFO? My test messages for the GCS_MAVLINK::send_afs_message are already at the maximum severity level (MAV_SEVERITY_CRITICAL), but it really doesn't change anything, it only affects how you interpret it from the GC side...

[Karl]

Does the GCS_MAVLINK::send_statustext_all message have to be enabled for it to work? I can't find the source of the issue, I've been stuck on this for more than a week... Nothing indicates that the message is sent out. Having no means of debugging (as I run the thing in simulation) makes it really difficult to pinpoint the issue.

I found one issue: in the code above the MAVLINK_MSG_ID_STATUSTEXT_LEN should've been MAVLINK_MSG_ID_AFS_MESSAGE_LEN, but that did not help.

[Randy Mackay]

Karl,

 

You’re using Copter and some fairly recent version of the code (i.e. “Copter-3.4-dev”)?

 

I guess the first thing is to ensure that at least some known-to-be-working messages are getting through to your GCS.  So for examples, you should be able to disable the GPS (in SITL you can set SIM_GPS_TYPE=0) and then try to arm in Loiter and it should fail and you should see one of the following messages (which come from arming_checks.cpp):

PreArm: Waiting for Nav Checks

PreArm: Need 3D Fix

PreArm: High GPS HDOP

 

I somehow vaguely recall that there’s an issue with text messages over UDP at least with mission planner.  It’s a vague memory and I don’t know why it would be a problem nor where the issue is/was.

 

It’s an interesting thing you’re doing and makes a lot of sense.  If it were me I’d probably implement it by creating a new FAILSAFE flight mode and then add new behaviours to all the existing failsafe parameters (i.e. FS_EKF_ACTION, FS_BATT_ENABLE, etc) to allow the user to specify that the desired action is to switch into this new flight mode.  That new flight mode would then implement doing whatever more complex action you want.  I’d probably also implement new mavlink messages to handle whatever back-and-forth communication between the flight controller and the GCS is required.

 

-Randy

--

[Karl]

Hi Randy,

I'm using ArduPlane V3.4.0. Messages go through as expected, no problems whatsoever. This is the response from MAVProxy when I try to arm in loiter mode while the GPS is down:

APM: PreArm: AHRS not healthy!
APM: PreArm: Bad GPS Position

I just noticed that the lost packet count in MAVProxy increases every time I send my custom message, so I guess the packet is lost somehow... How can that be? Where is it lost? Is it an implementation issue? Does MAVProxy need to know the message definition as well? I have only generated new headers from my custom ardupilotmega.xml for ardupilot and QGC, but not MAVProxy.

I'm currently at the stage of getting my custom back-and-forth messaging going. I'm building the fail safe system it in the outback challenge library (APM_OBC), but a better way would probably be to do what you said: a separate library allowing to customize all the fail safe elements separately. For example I have a parachute in the plane, but for most UAVs this is probably not an option. That being said, my distress message is currently lost...

[Karl]

In MAVProxy I entered status in the console after a message of mine was sent and this was the output:

100.0, Pcz : 9.99999997475e-07}
5078: ATTITUDE {time_boot_ms : 1294188, roll : 0.438226550817, pitch : 0.0074303
0617014, yaw : 1.10801875591, rollspeed : 0.00536821922287, pitchspeed : 0.09197
95036316, yawspeed : 0.195034593344}
100: BAD_DATA {unknown MAVLink message ID 198, data:['fe', '3a', 'c3', '1', '1',
 'c6', '1', '2', 'a', '43', '72', '69', '74', '69', '63', '61', '6c', '20', '73'
, '61', '66', '65', '74', '79', '20', '69', '73', '73', '75', '65', '3a', '20',
'47', '50', '53', '20', '73', '69', '67', '6e', '61', '6c', '20', '6c', '6f', '7
3', '74', '2e', '0', 'a8', 'a0', '4c', '0', '80', 'a1', '4b', '0', '10', '0', '0
', '0', '0', '0', '1', '65', 'ef']}
2: CAMERA_FEEDBACK {time_usec : 1455292337035000, target_system : 0, cam_idx : 0
, img_idx : 0, lat : 465175852, lng : 65729872, alt_msl : 510.940002441, alt_rel
 : 137.559997559, roll : 3.15000009537, pitch : -0.180000007153, yaw : 261.60000
6104, foc_len : 0.0, flags : 0}pos_vert_variance : 0.0307541135699, compass_vari
2: COMMAND_ACK {command : 11, result : 0}ce : 0.171113982797}stom_mode : 11, sys

It seems MAVProxy sees the special message, but cannot interpret it. However I don't seem to get anything in QGroundControl where I tried to output all messages, but none with the ID 198 came by... Maybe I'm doing something wrong when sending out the message.

On Monday, 8 February 2016 17:06:18 UTC+1, Karl wrote: