Greetings,

Firmware Version: 2.71

Attached Files: Modified ArduPlane.pde

Premise

I've been working on modifying the ArduPlane's firmware so that it outputs a stream of data describing the plane's location, altitude, and attitude over Serial2 (uartC).

Technical Info

I made sure to disable the gcs3 initialization in the init_ardupilot() function so that the uartC port is available. To output the stream of data, I am simply using:

hal.uartC->println("data")

Where "data" is the specific variable I want to output. To output the GPS location and altitude data, added this line of code to the update_alt() and update_GPS() functions. The data is only outputted when valid new information is recieved from the GPS / barometer.

To output the attitude information, I added the above line of code to case 3 of the medium loop. This is the case dealing with telemetry so I guessed it might have slightly less of a load on it and I decided to add my code there. This is also after the euler angles have been updated, which are what I am sending out as the attitude data.

Problem

Here is the problem. This method of output works perfectly well most of the time. All my tests through HIL (hil_attitude) simulation were successful. Some of my tests in the air tests were successful as well, but others weren't. I am using a Gumstix Overo COM to record the ArduPlane output from uartC. I am pretty sure the Overo is not malfunctioning. Do any of the developers here have any other idea of what might be the cause of the inconsistencies in air?

Hypothesis

One thing I considered is the possibility of the ArduPilot scheduler getting in the way. I have briefly looked at the scheduler but don't exactly understand how it works. Does ArduPilot schedule an maximum time per function that can't be exceeded. This could explain why my code sometime fires and sometimes doesn't.

Once again, attached to this email is the modified ArduPlane.pde.

Views: 905

Attachments:

Reply to This

Replies to This Discussion

Hi Jonathan,

The simplest patch to do what you want on an APM2 is this:

--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -134,12 +134,12 @@ static void init_ardupilot()
         // baud rate
         hal.uartA->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD));
     }
-#else
+#endif
+
     // we have a 2nd serial port for telemetry
     hal.uartC->begin(map_baudrate(g.serial3_baud, SERIAL3_BAUD),
             128, SERIAL_BUFSIZE);
     gcs3.init(hal.uartC);
-#endif
 
     mavlink_system.sysid = g.sysid_this_mav;

That patch will enable the 2nd UART as a MAVLink enabled port. You can then attach to that port using any MAVLink aware client, such as pymavlink or mavproxy (which run nicely on embedded Linux boxes).

That will be much more reliable than the method you have been trying with directly writing to the serial port. It is much harder to write to a serial port on an APM2 while flying than you might think - you need to be sure the write won't block, as if it blocked then the flight control would stop, which could cause your plane to misbehave or crash.

The MAVLink code uses a queue system to ensure message writes never block.

Cheers, Tridge

A question out of curiosity...

I noticed a few places where serial block was disabled. Does this mean that, if needed, the scheduler (or some other part of the code) will stop the serial transfer before it's complete?

Another concern...

The interesting thing about this output stream is that it worked some of the time and other times didn't work. When it did work it appeared all the message were being sent flawlessly. When it didn't work, their would be a couple lines of garbage sent over the serial line and then nothing.

Hi Jonathan,

I noticed a few places where serial block was disabled. Does this mean that, if needed, the scheduler (or some other part of the code) will stop the serial transfer before it's complete?

Not quite. The way it works is that we never start a write to a serial port unless we know there is enough space in the ring buffer to hold the whole write. That ensures it can't block.

We also set the port non-blocking as a safety feature, but that shouldn't be triggering.

Cheers, Tridge

I've moved up to firmware 2.74b

Firmware: 2.74b

Update

I have gotten the ArduPilot to act like my Gumstix Overo is Ground Control Station 3 (gcs3). Therefore, all the attitude and location information is streamed to the Overo.

Problem

The Overo is suppose to act as an external camera board. So, DIGICAM_CONTROL messages should be forwarded to the Overo. I would like to do this by configuring the Overo as a subsystem which the ArduPilot would pass messages to as determined by the message component ID. Unfortunately, it doesn't look like subsystems are currently supported by ArduPilot or the Mission Planner (correct me if I'm wrong). So, I plan to do a simply hack, as documented below, that will forward all DIGICAM_CONTROL messages to gcs3, and subsequently the Overo.

Hack

Upon receiving a DIGICAM_CONTROL message, the AP_CAMERA object would create a pointer to the message and then call the gcs3 object to send a DIGICAM_CONTROL message. The gcs3 object would in turn grab the location of the message from the AP_CAMERA pointer and queue up a copy of the message to be sent.

Any thoughts or suggestions?

Hi Jonathan,

The APM:Plane code currently forwards unknown messages to the other link - see the last part of GCS_MAVLINK::handleMessage().It only forwards them if there is sufficient space in the transmit buffer of the other serial port to hold the message without blocking.

As you discovered it doesn't forward messages that are not for the APM target system/component pair. It probably should do this, but there is a problem doing that if there isn't sufficient space in the other serial buffer at the time the message arrives. Where would it put the message while waiting for the serial buffer to drain?

Upon receiving a DIGICAM_CONTROL message, the AP_CAMERA object would create a pointer to the message and then call the gcs3 object to send a DIGICAM_CONTROL message. The gcs3 object would in turn grab the location of the message from the AP_CAMERA pointer and queue up a copy of the message to be sent

Without seeing the implementation it is difficult to say for sure, but it sounds like you are assuming the incoming message is in memory that persists beyond the current stack frame. It isn't - the memory is on the stack of the caller, and as soon as the function returns any pointer to that memory is invalid.

There are several ways to solve this. The simplest is to copy the message to a static buffer, and then queue the message for sending out the gcs3 port as part of the normal message queueing in GCS_MAVLink.pde.

The key to all of this is memory management. We have very little free memory on an APM2, and we can't afford to do dynamic allocation for this sort of thing as we would quickly fragment the heap or run out of memory. On the PX4 the situation is quite different, and we have sufficient memory to allow for a queue of messages to be forwarded.

Cheers, Tridge

The APM:Plane code currently forwards unknown messages to the other link - see the last part of GCS_MAVLINK::handleMessage().It only forwards them if there is sufficient space in the transmit buffer of the other serial port to hold the message without blocking.

Aw, I see. What was the intent in doing this?

As you discovered it doesn't forward messages that are not for the APM target system/component pair. It probably should do this, but there is a problem doing that if there isn't sufficient space in the other serial buffer at the time the message arrives. Where would it put the message while waiting for the serial buffer to drain?

Is this the main reason why subsystems by component ID haven't been implemented? Are there developers currently trying to tackle this problem? Since I plan on using APM in multi-component UAV's, I might be interested in investigating this issue.

What if uartC could be wrapped in a component communication object instead of the GCS object (but similiar to the GCS object)? Then we could give uartC it's own MAVLink buffers for messages with component ID's that don't match the ArduPilot.

Is there anything besides the telemetry that is ever run through uartC?

How many active developers are working on the ArduPilot firmware and Mission Planner software? Who else might I talk to get a head start on this?

Aw, I see. What was the intent in doing this?

I added it as I'm doing image transfer from a Linux based pandaboard on a plane to the ground via MAVLink wrapped python objects. The protocol used is one that can handle packet loss, so losing some messages due to buffers being full was acceptable.

Is this the main reason why subsystems by component ID haven't been implemented? Are there developers currently trying to tackle this problem? Since I plan on using APM in multi-component UAV's, I might be interested in investigating this issue.

yep, it's all about memory. The APM2 has very little free memory. If you use a PX4 instead (which runs the same base APM software) then there are a lot more options as it has a lot more memory. The APM2 has 8k of RAM. The PX4 has 192k. I've switched to using PX4 for my planes for exactly this reason. You can have large serial buffers, object buffers etc and not have to worry all the time about running out of memory.

What if uartC could be wrapped in a component communication object instead of the GCS object (but similiar to the GCS object)? Then we could give uartC it's own MAVLink buffers for messages with component ID's that don't match the ArduPilot.

Putting a different object wrapper on it doesn't gain you any more memory. If you only want DIGICAM_CONTROL then you could have a static buffer for just that message, or you could allocate one general purpose MAVLink message buffer for message forwarding. Either way you need to think about what you want to happen when that buffer is full when another message arrives that needs forwarding.

You can reduce the problem a bit by running uartC at a higher baud rate (say 115200) compared to the rate of the telemetry link. Or you could instead push the problem onto the ground station by having a re-send of messages that aren't acknowledged. You could also lower the chance of the serial buffer being full by running the telemetry rate on uartC at lower stream rates.

How many active developers are working on the ArduPilot firmware and Mission Planner software? Who else might I talk to get a head start on this?

quite a few - maybe you should join the drones-discuss mailing list?

Cheers, Tridge

Putting a different object wrapper on it doesn't gain you any more memory.

Excuse me, I wasn't being clear about my intentions. A subsystem communication object would simply be the go-to-dump where MAVLink message with a component ID different from the ArduPilot's could be sent. The other component's on board the UAV could be connected through this uart port and receive the MAVLink commands intended for them.

The current Ground Control Station object seems to have limited options when sending a message. You are only allowed to give the message ID and the object takes care of all the details. This is fine for communicating with a GCS, but for passing MAVLink messages onto other components, its messy.

quite a few - maybe you should join the drones-discuss mailing list?

I will. Thanks for your help.

The current Ground Control Station object seems to have limited options when sending a message. You are only allowed to give the message ID and the object takes care of all the details. This is fine for communicating with a GCS, but for passing MAVLink messages onto other components, its messy.

It's done that way to save memory - the queue costs 1 byte per message. If you go via a different object structure you still need to think about what you do with a message arriving on uartA if it doesn't immediately fit on uartC.

That is the fundamental issue. You could decide to discard messages that don't fit, or you could have a fixed buffer size. What you can't do is just queue them all.

That's why I said changing the object structure doesn't gain any more memory. You just end up with the same problem in a different object. In an autopilot you can't wait for the serial buffer to drain, as the plane would lose stability while you wait.

Also note that discarding messages that don't fit may actually be a reasonable strategy. A telemetry radio is a lossy transport anyway, so you need to cope with loss whatever you do. So perhaps just doing the simple forwarding similar to the current fwd scheme but adjusted for different component IDs is all you need.

Cheers, Tridge

Reply to Discussion

RSS

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service