I'm trying to add an I2C sensor to my APM using Ardupilot 2.20, but am having trouble getting MAVLink to send the additional sensor data. I built a test procedure in the CLI that shows I'm getting good data, but can't figure out how to create a MAVLink message type to even see the data as text. What I really want is to get the sensor to pop up in QGroundControl as a plottable datastream in the Engineer perspective. Does anyone know the best way to add this functionality to APM/MAVLink?

Views: 2045

Reply to This

Replies to This Discussion

For those who are interested, I found a solution. By using the MAVLink library function

  mavlink_msg_named_value_float_send(chan, name, value)

(located in ~/sketchbook/libraries/GCS_MAVLink/include/common/mavlink_message_named_value_float.h), or the similar function for integers, it is easy to transmit any sensor values as a time series to the ground station, and QGroundControl will read the message and add the name you provide (limited to 10 characters by the function above) to the realtime plot.

To integrate with ArduPilotMega 2.20, I added a message definition to the defines.h file:

  #define MSG_NEW_SENSOR_DATA 0x25

and used this value in the switch of mavlink_send_message(chan, id, param, packet_drops):

  case MSG_NEW_SENSOR_DATA:

    {

      mavlink_msg_named_value_float_send(chan, "New Sensor", global_variable_with_new_sensor_data);

      break;

    }

Then, I made a call to the higher-order member of the gcs object:

  gcs.send_message(MSG_NEW_SENSOR_DATA)

Because my sensor functions at 0.5 Hz, I put this function call in my own super-slow loop, called from inside the one-second-loop in ArduPilotMega.pde, but you can put it in whichever speed loop you want. Just make sure reading your sensor can't block the autopilot code for too long, or you will have some major problems.

Doing it this ways allows me to use the gcs object's private definitions of chan, param, and packet_drops. The MAVLink handbook recommends only using mavlink_msg_named_value_float_send for rapid prototyping and testing, so I will be using the function as a basis for my own custom MAVLink message, but this should be enough to get others started and avoid some of my own frustrations. Enjoy!

Robert,

I'm curious - What i2C sensor are you adding?

I have just created an i2C sonar modification to help avoid noise on the analog signal for an Arducopter.

Its still a work in progress - I expect to integrate it in the AC2 code and fly it later this week

-Andrew

Andrew,

I'm working with a SensAir K32 CO2 sensor. My research group is developing a mobile atmospheric monitoring system. I just looked at your post about the noise with the ESC and will be interested in seeing your modifications. It's good to know in case we encounter similar problems with our I2C bus.

Thanks,

Rob

Rob
The noise issue was with the original analog signal from the sonar not the i2c.
This is why I've put together a i2c version of the sonar
I've mounted a arduino mini on the sonar. It reads the data as pwm and preprocesses it there then connects to the APM via i2c.
Test flights tomorrow if our winter weather behaves here.
Andrew

Robert,

I am trying to get a K30 Air sensor to work with the APM 2. Did you guys successfully get this project off the ground? 

Hi,

Wow, great, thanks!

I will find myself an unused analog input and add a light sensor :)

Regards

Soren

Hi Robert,

By following your method below, did your new sensor data show up in the 'Values" widget? I want to display my sensor data as a value along with Groundspeed etc etc

Robert Hansen said:

For those who are interested, I found a solution. By using the MAVLink library function

  mavlink_msg_named_value_float_send(chan, name, value)

(located in ~/sketchbook/libraries/GCS_MAVLink/include/common/mavlink_message_named_value_float.h), or the similar function for integers, it is easy to transmit any sensor values as a time series to the ground station, and QGroundControl will read the message and add the name you provide (limited to 10 characters by the function above) to the realtime plot.

To integrate with ArduPilotMega 2.20, I added a message definition to the defines.h file:

  #define MSG_NEW_SENSOR_DATA 0x25

and used this value in the switch of mavlink_send_message(chan, id, param, packet_drops):

  case MSG_NEW_SENSOR_DATA:

    {

      mavlink_msg_named_value_float_send(chan, "New Sensor", global_variable_with_new_sensor_data);

      break;

    }

Then, I made a call to the higher-order member of the gcs object:

  gcs.send_message(MSG_NEW_SENSOR_DATA)

Because my sensor functions at 0.5 Hz, I put this function call in my own super-slow loop, called from inside the one-second-loop in ArduPilotMega.pde, but you can put it in whichever speed loop you want. Just make sure reading your sensor can't block the autopilot code for too long, or you will have some major problems.

Doing it this ways allows me to use the gcs object's private definitions of chan, param, and packet_drops. The MAVLink handbook recommends only using mavlink_msg_named_value_float_send for rapid prototyping and testing, so I will be using the function as a basis for my own custom MAVLink message, but this should be enough to get others started and avoid some of my own frustrations. Enjoy!

Reply to Discussion

RSS

Groups

Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service