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.
Replies
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 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