Servers by jDrones

Arduino (UNO) - MAVLink - PX4 no apparent connection + problem understanding the code from step by step guide

mavlink
arduino
pixhawk
(Gianluca Gregnol) #1

Good evening community,

first of all i’m here to learn and understand as much i can on MAVLink.

I’m trying to connect and Arduino Uno with the PX4 to read different parameters (attitude, speed,…).
I used Software serial to entablish a connection between the UNO and the PX4 and a standard serial from the UNO to my pc to read variables value in the serial monitor.

CODE BELOW

The physical connections are as follow:

  1. PC - UNO : USB;
  2. UNO - PX4 : telem 2 (pin 9: Rx (yellow), pin 10: Tx (green)+ GND (black) on UNO);
  3. PX4 is powered via USB through an external power bank.

for the connection 2 i followed this picture:
http://ardupilot.org/copter/_images/3drRadioV1_pixhawk1.jpg

This is my connection:

Current PX4 setting for serial port are as in the picture below.

picture%202

I have a problem with the code posted here:
https://discuss.ardupilot.org/t/mavlink-and-arduino-step-by-step/25566

From his code i only commented or deleted the LEDs parts that were not useful for me.
I upload in the UNO the MAVLink library from the zip file in the link above (mavlink.zip).
“request” and “receive” variables are counters i added to verify how many times functions are executed.

The problem is that the UNO seems not to be able to reach the PX4, as once the “Streams requested!” message is displayed (line 93), no data is displayed, even if from the code it should (for example in lines 292-293 it should print the roll value, but as you can see from the picture below, nothing is printed).

I cannot undestand if the code has problems or if my connections are not correct.

If you need more information or any detail, just ask.
Before writing here i spent about 3 weeks undestanding the code as much as i could and trying to solve the problem.
As i have no more ideas on how to proceed, i though to ask here.
Thank you.

the code can be also seen from this site (feel free to modify it. it will be store):

https://www.codepile.net/pile/zMNdOB6n

CODE USED

/*  MAVLInk_DroneLights
    by Juan Pedro López
*/
// In case we need a second serial port for debugging
#define SOFT_SERIAL_DEBUGGING   // Comment this line if no serial debugging is needed
#ifdef SOFT_SERIAL_DEBUGGING

// Library to use serial debugging with a second board
#include <SoftwareSerial.h>

#define RxPin0 9
#define TxPin0 10

int request = 0;
int receive = 0;

SoftwareSerial pxSerial = SoftwareSerial(RxPin0, TxPin0);  // RX, TX || UNO - PX4

#endif

#include "mavlink.h"
//#include "common/mavlink_msg_request_data_stream.h"


// Mavlink variables
unsigned long previousMillisMAVLink = 0;     // will store last time MAVLink was transmitted and listened
unsigned long next_interval_MAVLink = 1000;  // next interval to count
const int num_hbs = 60;                      // # of heartbeats to wait before activating STREAMS from Pixhawk. 60 = one minute.
int num_hbs_pasados = num_hbs;

void setup() {

  pinMode(RxPin0, INPUT);
  pinMode(TxPin0, OUTPUT);

  // MAVLink interface start
  // Serial.begin(57600);

#ifdef SOFT_SERIAL_DEBUGGING
  // [DEB] Soft serial port start
  Serial.begin(57600);
  pxSerial.begin(57600);
  Serial.println("MAVLink starting.");
#endif
}

void loop() {

  // MAVLink
  /* The default UART header for your MCU */
  int sysid = 1;                   ///< ID 20 for this airplane. 1 PX, 255 ground station
  int compid = 158;                ///< The component sending the message
  int type = MAV_TYPE_FIXED_WING;   ///< This system is an airplane / fixed wing

  // Define the system type, in this case an airplane -> on-board controller
  // uint8_t system_type = MAV_TYPE_FIXED_WING;
  uint8_t system_type = MAV_TYPE_GENERIC;
  uint8_t autopilot_type = MAV_AUTOPILOT_INVALID;

  uint8_t system_mode = MAV_MODE_PREFLIGHT; ///< Booting up
  uint32_t custom_mode = 0;                 ///< Custom mode, can be defined by user/adopter
  uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight
  // Initialize the required buffers
  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  //mavlink_msg_heartbeat_pack(sysid,compid, &msg, type, autopilot_type, system_mode, custom_mode, system_state);
  mavlink_msg_heartbeat_pack(1, 0, &msg, type, autopilot_type, system_mode, custom_mode, system_state);

  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  // Send the message with the standard UART send function
  // uart0_send might be named differently depending on
  // the individual microcontroller / library in use.
  unsigned long currentMillisMAVLink = millis();
  if (currentMillisMAVLink - previousMillisMAVLink >= next_interval_MAVLink) {
    previousMillisMAVLink = currentMillisMAVLink;

#ifdef SOFT_SERIAL_DEBUGGING
    pxSerial.write(buf, len);
    //Serial.println("Ardu HB");
#else
    Serial.write(buf, len);
#endif

    //Mav_Request_Data();
    num_hbs_pasados++;
    if (num_hbs_pasados >= num_hbs) {
      // Request streams from Pixhawk
#ifdef SOFT_SERIAL_DEBUGGING
      Serial.println("Streams requested!");
#endif
      Mav_Request_Data();
      num_hbs_pasados = 0;
    }

  }

  // Check reception buffer
  comm_receive();

  //  Serial.print("receive:  ");
  //  Serial.print(receive);
  //  Serial.print("       request:  ");
  //  Serial.println(request);
}

void Mav_Request_Data()
{

  request = request + 1;

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];


  // STREAMS that can be requested
  /*
     Definitions are in common.h: enum MAV_DATA_STREAM

     MAV_DATA_STREAM_ALL=0, // Enable all data streams
     MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.
     MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS
     MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW
     MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.
     MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.
     MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot
     MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot
     MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot
     MAV_DATA_STREAM_ENUM_END=13,

     Data in PixHawk available in:
      - Battery, amperage and voltage (SYS_STATUS) in MAV_DATA_STREAM_EXTENDED_STATUS
      - Gyro info (IMU_SCALED) in MAV_DATA_STREAM_EXTRA1
  */

  // To be setup according to the needed information to be requested from the Pixhawk
  const int  maxStreams = 2;
  const uint8_t MAVStreams[maxStreams] = {MAV_DATA_STREAM_EXTENDED_STATUS, MAV_DATA_STREAM_EXTRA1};
  const uint16_t MAVRates[maxStreams] = {0x02, 0x05};


  for (int i = 0; i < maxStreams; i++) {
    /*
       mavlink_msg_request_data_stream_pack(system_id, component_id,
          &msg,
          target_system, target_component,
          MAV_DATA_STREAM_POSITION, 10000000, 1);

       mavlink_msg_request_data_stream_pack(uint8_t system_id, uint8_t component_id,
          mavlink_message_t* msg,
          uint8_t target_system, uint8_t target_component, uint8_t req_stream_id,
          uint16_t req_message_rate, uint8_t start_stop)

    */
    mavlink_msg_request_data_stream_pack(2, 200, &msg, 1, 0, MAVStreams[i], MAVRates[i], 1);
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
#ifdef SOFT_SERIAL_DEBUGGING
    pxSerial.write(buf, len);
#else
    Serial.write(buf, len);
#endif
  }

  // Request: PARAM_REQUEST_LIST. Only for full log recording
  /*
     Primitive: mavlink_msg_param_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
                   uint8_t target_system, uint8_t target_component)
  */
  /*
    // Configure
    uint8_t system_id=2;
    uint8_t component_id=200;
    // mavlink_message_t* msg;
    uint8_t target_system=1;
    uint8_t target_component=0;

    // Pack
    mavlink_msg_param_request_list_pack(system_id, component_id, &msg,
      target_system, target_component);
    uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

    // Send
    #ifdef SOFT_SERIAL_DEBUGGING
      pxSerial.write(buf,len);
    #else
      Serial.write(buf, len);
    #endif
  */
}



void comm_receive() {

  receive = receive + 1;

  mavlink_message_t msg;
  mavlink_status_t status;

  // Echo for manual debugging
  // Serial.println("---Start---");

#ifdef SOFT_SERIAL_DEBUGGING
  while (pxSerial.available() > 0) {
    uint8_t c = pxSerial.read();
#else
  while (Serial.available() > 0) {
    uint8_t c = Serial.read();
#endif

    // Try to get a new message
    if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) {

      // Handle message
      switch (msg.msgid) {
        case MAVLINK_MSG_ID_HEARTBEAT:  // #0: Heartbeat
          {
            // E.g. read GCS heartbeat and go into
            // comm lost mode if timer times out
#ifdef SOFT_SERIAL_DEBUGGING
            Serial.println("PX HB");
#endif
          }
          break;

        case MAVLINK_MSG_ID_SYS_STATUS:  // #1: SYS_STATUS
          {
            /* Message decoding: PRIMITIVE
                  mavlink_msg_sys_status_decode(const mavlink_message_t* msg, mavlink_sys_status_t* sys_status)
            */
            //mavlink_message_t* msg;
            mavlink_sys_status_t sys_status;
            mavlink_msg_sys_status_decode(&msg, &sys_status);
#ifdef SOFT_SERIAL_DEBUGGING
            Serial.print("PX SYS STATUS: ");
            Serial.print("[Bat (V): ");
            Serial.print(sys_status.voltage_battery);
            Serial.print("], [Bat (A): ");
            Serial.print(sys_status.current_battery);
            Serial.print("], [Comms loss (%): ");
            Serial.print(sys_status.drop_rate_comm);
            Serial.println("]");
#endif
          }
          break;

        case MAVLINK_MSG_ID_PARAM_VALUE:  // #22: PARAM_VALUE
          {
            /* Message decoding: PRIMITIVE
                  mavlink_msg_param_value_decode(const mavlink_message_t* msg, mavlink_param_value_t* param_value)
            */
            //mavlink_message_t* msg;
            mavlink_param_value_t param_value;
            mavlink_msg_param_value_decode(&msg, &param_value);
#ifdef SOFT_SERIAL_DEBUGGING
            Serial.println("PX PARAM_VALUE");
            Serial.println(param_value.param_value);
            Serial.println(param_value.param_count);
            Serial.println(param_value.param_index);
            Serial.println(param_value.param_id);
            Serial.println(param_value.param_type);
            Serial.println("------ Fin -------");
#endif
          }
          break;

        case MAVLINK_MSG_ID_RAW_IMU:  // #27: RAW_IMU
          {
            /* Message decoding: PRIMITIVE
                  static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavlink_raw_imu_t* raw_imu)
            */
            mavlink_raw_imu_t raw_imu;
            mavlink_msg_raw_imu_decode(&msg, &raw_imu);
#ifdef SOFT_SERIAL_DEBUGGING
            //Serial.println("PX RAW IMU");
            //Serial.println(raw_imu.xacc);
#endif
          }
          break;

        case MAVLINK_MSG_ID_ATTITUDE:  // #30
          {
            /* Message decoding: PRIMITIVE
                  mavlink_msg_attitude_decode(const mavlink_message_t* msg, mavlink_attitude_t* attitude)
            */
            mavlink_attitude_t attitude;
            mavlink_msg_attitude_decode(&msg, &attitude);
#ifdef SOFT_SERIAL_DEBUGGING
            Serial.println("PX ATTITUDE");
            Serial.println(attitude.roll);
            //            if(attitude.roll>1) leds_modo = 0;
            //            else if(attitude.roll<-1) leds_modo = 2;
            //            else leds_modo=1;
#endif
          }
          break;


        default:
#ifdef SOFT_SERIAL_DEBUGGING
          Serial.print("--- Otros: ");
          Serial.print("[ID: ");
          Serial.print(msg.msgid);
          Serial.print("], [seq: ");
          Serial.print(msg.seq);
          Serial.println("]");
#endif
          break;
      }
    }
  }
}
(Juan Pedro) #2

Hi,

Send me a PM. I can try to help.

Regards,
Juan Pedro

(Gianluca Gregnol) #3

thank you Juan, I’m sending it to you within today. :pray:

Gianluca

(Gianluca Gregnol) #4

UPDATE in case anyone is having the same problem in the future.

PROBLEM SOLVED

The image i used to set the connections ( http://ardupilot.org/copter/_images/3drRadioV1_pixhawk1.jpg ) shows clearly a Pixhawk 4, but i have a Pixhawk 2 cube… i wrongly assumed that the telemetry scheme ports would have been the same for the 2 models, but that’s not true.
Instead, if you have a Pixhawk 2 cube, the scheme is basically inverted. So if you have a model like mine just use the schematic below insted of the one i reported above.

Juan Pedro’s code works perfectly (the one in his post and the one posted here) and after i changed the connections, i was able to see the stream from the serial monitor.

1 Like