hi bro can you PM you’re code. i’m also working with ultrasonic sensors Arduino nano and pix hawk please help
i have no error in code and connection i’m using multiple (uart) ultrasonic sensor for collusion detection i need only sensor data…
#include “mavlink.h”
#include <SoftwareSerial.h>
#define MIN_DISTANCE 30//30cm
#define MAX_DISTANCE 500//100cm
//uint16_t un16distance;
unsigned char data1[4]={};//, data2[4]={};
uint16_t distance, distance2;
#define TX_PIN 10
#define RX_PIN 11
SoftwareSerial mavlinkSerial(TX_PIN,RX_PIN); // RX,TX
SoftwareSerial mySerial1(4,5); // RX, TX
//SoftwareSerial mySerial2(14,12); // RX, TX
#define ORIENT_FORWARD 0
#define ORIENT_FORWARD_RIGHT 1
#define ORIENT_RIGHT 2
#define ORIENT_BACK_RIGHT 3
#define ORIENT_BACK 4
#define ORIENT_BACK_LEFT 5
#define ORIENT_LEFT 6
#define ORIENT_FORWARD_LEFT 7
#define ORIENT_UP 24
#define ORIENT_DOWN 25
#define DEST_SYS_ID 1 //device id
//#define MAV_COMP_ID_PERIPHERAL 158
int distancetemp;
void setup(){
mavlinkSerial.begin(57600);
Serial.begin(19200);
mySerial1.begin(9600);
// mySerial2.begin(9600);
}
void loop(){
sensordata();
sendHeartbeat();
sendDistanceSensor(distance,ORIENT_FORWARD);// forward
}
void sensordata(){
// do{
// for(int i=0;i<4;i++)
// {
// data1[i]=mySerial1.read();
// }
//// Serial.print(“reading data”);
// }while(mySerial1.read()==0xff);
// mySerial1.flush();
// if(data1[0]==0xff)
// {
// int sum;
// sum=(data1[0]+data1[1]+data1[2])&0x00FF;
//
// if(sum==data1[3])
// {
// distance=(data1[1]<<8)+data1[2];
//// Serial.print(“calculating distance”);
// if(distance>280)
// {
// Serial.print(“distance:”);
// Serial.print(distance/10);
// }else Serial.println(“ERROR”);
// }
distance = 500;
delay(1000);
}
// sending heart beat
void sendHeartbeat()
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
mavlink_msg_heartbeat_pack[DEST_SYS_ID,MAV_COMP_ID_PERIPHERAL,&msg,MAV_TYPE_GCS,MAV_AUTOPILOT_INVALID,0,0,0];
uint16_t len = mavlink_msg_to_send_buffer(buf,&msg);
mavlinkSerial.write(buf,len);
Serial.println(">send Heartbeat");
}
// sending data
void sendDistanceSensor(uint16_t distance, uint8_t orientation)
{
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
// https://mavlink.io/en/messages/common.html#DISTANCE_SENSOR
mavlink_msg_distance_sensor_pack(DEST_SYS_ID, // The sysid of message should same the system id of the pixhawk (SYSID_THISMAV)
MAV_COMP_ID_PERIPHERAL , // component id MAV_COMP_ID_PATHPLANNER (195) or MAV_COMP_ID_PERIPHERAL (158)
&msg,
0, //ms, Timestamp (time since system boot).
MIN_DISTANCE, //cm, Minimum distance the sensor can measure
MAX_DISTANCE, //cm, Maximum distance the sensor can measure
distance/10, //cm, Current distance reading
1, //Type from MAV_DISTANCE_SENSOR enum, Type of distance sensor. (0:MAV_DISTANCE_SENSOR_LASER, 1:MAV_DISTANCE_SENSOR_ULTRASOUND)
orientation+1, //Onboard ID of the sensor
orientation, //Type from MAV_SENSOR_ORIENTATION enum
0,
0,0,0,0// Measurement variance. Max standard deviation is 6cm. 255 if unknown.
);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
mavlinkSerial.write(buf, len);
Serial.print("("); Serial.print(orientation); Serial.print("): "); Serial.print(distance/10);Serial.print("cm\n");
delay(100);
}
this is my code hope you replay soon 
im commented the ultrasonic function and for testing ill send 500 mm as data
Hello Juan Pedro. I have a question for my project. How can I contact with you on other platforms. I searched you on Linkedin but I can’t find your profile. Please help me!
OK. Thank you so much for your reply.
Why does this work
mavlink_msg_param_request_read_send(MAVLINK_COMM_0, apm_mav_system, apm_mav_component, "BATT2_CRT_VOLT",-1);
//but this is ignored by AP? - produces a packet 244 - but AP does not set the rate.
mavlink_msg_message_interval_send(MAVLINK_COMM_0, MAVLINK_MSG_ID_VFR_HUD, 500000);
Message interval requires ArduCopter version greater than 4.0
Do you have that?
@amilcarlucas thanks for trying to help, I solved it by using mavlink_msg_command_long_send - which worked great. (and yes, I am well past AP/AC4.0) 
this way I can get the actual MAV_CMD_SET_MESSAGE_INTERVAL (511)
I was thrown off by the existence of the convenience function mavlink_msg_message_interval_send which looked like what I needed, but then it produces a command 244 (which does not seem to be used anywhere)
I am having trouble reading the second Arduino board when I put though the Arduino serial port the information that comes back is scrambled
is it a wire connection issue or a coding issue ?
#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>
SoftwareSerial mySerial(11, 12); // RX, TX
SoftwareSerial pxSerial(9,10); // RX, TX
#endif
// Call the library
#include <mavlink.h>
#include <SoftwareSerial.h>
// Launch the serial port in setup
void setup()
{
// MAVLink interface start
Serial.begin(57600);
}
// Loop your program
void loop()
{
// MAVLink config
/* 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_GROUND_ROVER; ///< This system is an airplane / fixed wing
// Define the system type, → on-board controller
uint8_t system_type = MAV_TYPE_GROUND_ROVER;
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(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();
unsigned long previousMillisMAVLink;
unsigned long next_interval_MAVLink;
unsigned long num_hbs_pasados;
int num_hbs;
if (currentMillisMAVLink - previousMillisMAVLink >= next_interval_MAVLink)
{
// Timing variables
unsigned long previousMillisMAVLink = currentMillisMAVLink;
#ifdef SOFT_SERIAL_DEBUGGING
pxSerial.write(buf,len);
//mySerial.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
mySerial.println(“Streams requested!”);
#endif
Mav_Request_Data();
num_hbs_pasados = 0;
}
}
// Check reception buffer
comm_receive();
}
void Mav_Request_Data()
{
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
*/
- 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
}
}
void comm_receive() {
mavlink_message_t msg;
mavlink_status_t status;
while(Serial.available()>0) {
uint8_t c = Serial.read();
// 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
//mySerial.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
mySerial.print("PX SYS STATUS: ");
mySerial.print("[Bat (V): ");
mySerial.print(sys_status.voltage_battery);
mySerial.print("], [Bat (A): ");
mySerial.print(sys_status.current_battery);
mySerial.print("], [Comms loss (%): ");
mySerial.print(sys_status.drop_rate_comm);
mySerial.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, ¶m_value);
#ifdef SOFT_SERIAL_DEBUGGING
mySerial.println("PX PARAM_VALUE");
mySerial.println(param_value.param_value);
mySerial.println(param_value.param_count);
mySerial.println(param_value.param_index);
mySerial.println(param_value.param_id);
mySerial.println(param_value.param_type);
mySerial.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
//mySerial.println("PX RAW IMU");
//mySerial.println(raw_imu.xacc);
#endif
}
break;
default:
break;
}
}
}
}
we are running into the same issue how did you solve ?
Is it possible to change flight modes using the Mavlink library and instructing the Pixhawk using the arduino to change flight modes??
Of course that is possible
Hey @Siddhant_Rao, I have managed to do this, I can share my code later when I’m home if you like.
Cheers,
Paul
Hey @Siddhant_Rao, hopefully this helps.
My project is split into a number of source files, so I will post all of the relevant bits. Let me know if you need anything else.
Also, if you haven’t already, check the doco here Get and Set FlightMode — Dev documentation
/*
* mavlink_fns.h
*
* header file for my various mavlink functions.
*
*/
#ifndef MAVLINK_FNS_H
#define MAVLINK_FNS_H
#include "Arduino.h" // helps with the "types" used here.
// Below mavlink includes draw from the c_library_v1 library I added
//#include <common/mavlink.h> // The Mavlink library for the "common"
#include <ardupilotmega/mavlink.h> // The Mavlink library for the "ar
// MAVLink Specific Debugs
//#define MAVLINK_DEBUG // uncomment if you want the various MAVLINK
// MAVLink IDs - https://ardupilot.org/dev/docs/mavlink-basics.html
#define FMX_SYS_ID 1 // MAVLink System ID of this device. For
// I am setting this to 1, as all electro
// the AutoPilot, and it has sysID = 1.
#define FMX_COMP_ID 100 // MAVLink Component ID of this device. F
// I am setting this to 100, as it can be
#define AP_SYS_ID 1 // MAVLink System ID of the autopilot.
#define AP_COMP_ID 1 // MAVLink Component ID of the autopilot.
The next file mavlink_fns.cpp has a lot of other code in it, here is the relevant stuff.
/*
* mavlink_fns.cpp
*
* mavlink specific functio
*
*
*/
#include "mavlink_fns.h"
..
..
*============================
* mavlink_set_flightmode_ap()
*
* Change the ArduPilot Flightmode of the AP.
* We issue a COMMAND_LONG containing the command MAV_CMD_DO_SET_MODE (#176) https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_MODE
* As discussed in ArduPilot doco - https://ardupilot.org/dev/docs/mavlink-get-set-flightmode.html
*
* The Flightmodes are defined in ardupilotmega.h from the MAVLink library.
* ROVER_MODE_MANUAL=0
* ROVER_MODE_ACRO=1
* ROVER_MODE_STEERING=3
* ROVER_MODE_HOLD=4
* ROVER_MODE_LOITER=5
* ROVER_MODE_FOLLOW=6
* ROVER_MODE_SIMPLE=7
* ROVER_MODE_AUTO=10
* ROVER_MODE_RTL=11
* ROVER_MODE_SMART_RTL=12
* ROVER_MODE_GUIDED=15
* ROVER_MODE_INITIALIZING=16
* ROVER_MODE_ENUM_END=17
*============================*/
void mavlink_set_flightmode_ap(float desired_flightmode)
{
debugPrintln("mavlink_set_flightmode_ap() - START");
Serial.print("mavlink_set_flightmode_ap() - Desired Flightmode:");Serial.println(desired_flightmode);
// Prep source and dest MAVLink addressing info, to be used in below actions.
uint8_t _system_id = FMX_SYS_ID; // MAVLink System ID of this device.
uint8_t _component_id = FMX_COMP_ID; // MAVLink Component ID of this device.
uint8_t _target_system = AP_SYS_ID; // MAVLink System ID of the autopilot.
uint8_t _target_component = AP_COMP_ID; // MAVLink Component ID of the autopilot.
// Build the COMMAND_LONG / MAV_CMD_DO_SET_MODE message.
// components of the MAVLink COMMAND_LONG message - https://mavlink.io/en/messages/common.html#COMMAND_LONG
uint16_t _cl_command = MAV_CMD_DO_SET_MODE; // https://mavlink.io/en/messages/common.html#MAV_CMD_DO_SET_MODE
uint8_t _cl_confirmation = 0; // always 0 for first transmission, then incremented. https://mavlink.io/en/services/command.html#COMMAND_LONG
float _cl_param1 = 1; // always MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1
float _cl_param2 = desired_flightmode; // Flightmode
float _cl_param3 = 0; // Not used, so set to zero.
float _cl_param4 = 0; // Not used, so set to zero.
float _cl_param5 = 0; // Not used, so set to zero.
float _cl_param6 = 0; // Not used, so set to zero.
float _cl_param7 = 0; // Not used, so set to zero.
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack and send the message
mavlink_msg_command_long_pack(_system_id, _component_id, &msg, _target_system, _target_component, _cl_command, _cl_confirmation, _cl_param1, _cl_param2, _cl_param3, _cl_param4, _cl_param5, _cl_param6, _cl_param7);
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); // put message into our send buffer and also get it's size in bytes.
Serial1.write(buf, len); //Write data to serial port byte by byte.
debugPrintln("mavlink_set_flightmode_ap() - END");
} // END - mavlink_set_flightmode_ap()
1 Like
Now the other thing to keep in mind is the AutoPilot may not change modes for some underlying condition, and I’m sure others can explain that better than I can, but this is the code to issue the commands. I tend to look at the AutoPilots responses straight after issuing a command to see if it is successful or if there is some sort of error response.
I have got a number of MAVLink commands working from my Adafruit Feather M4 CAN board (not using CAN), so I might try and pull them together in a stand alone test program and share it here.
So far I have successfully managed the following over MAVLink;
- Arming & Disarming the AP
- Changing flightmode
- Setting an individual parameter
- Reading an individual parameter
- sending a heartbeat
- Requesting/Disabling the AP to stream groups of parameters or individual parameters
- Uploading a mission (that was the most complex by far)
- Reading a mission
- Clearing a mission
The documentation from here MAVLink Interface — Dev documentation and its sub pages is great once you get going, but it is a steep learn.
Let me know how you go.
Cheers,
Paul
thank you a lot. I will test it soon on my hardware and ask for help if needed
I’ve built a wrapper around some of my Arduino MAVLink functions so you can test them and hopefully they are helpful to learn from. It’s not perfect, but it works and does a reasonable job. I will add more functionality to it when I have time.
Its available here GitHub - pauljeffress/Pulsar-MAVLink-Tester
I have put some explanation in the README there too.
Cheers,
Paul
Do you solve this problem? now i am working on this project. Can you help me ? I’ll be grateful for this.
Can anybody help me? I try to control the speed of the stepper motor by Arducopter / pixhawk through Arduino. Now I work on a Water surface vehicle project. I want after getting the signal from the receiver Arducopter/ pixhawk to control the Arduino for speed control of the stepper motor.