MAVLink Manual Control via Arduino / Teensy

hi @all,

Inspired on Burkards Sonar Boat project i want to build my own Sonar Boat. We have some discussion to implement the MAVLink functionality and use a APM or Pixhawk with ardurover firmware.

Burkard has developed a little remote with a OLED to get some information live from the Boat. This remote has a 5 Way Switch (Joystick) to control manual...

A 3DR Ground Modul, a five way switch and a Oled are connected to a Teensy (Teensyduino) 

So far so good :)

3691195025?profile=original

3691195227?profile=original

I try to implement the MAVLink Protocol to get some information about Battery, GPS Position. This will be succesfull. Here is an example https://vimeo.com/124046596 for receiving MAVLink information.

But I can't send manual commands from the remote controller to the Boat (APM) I want to control the roll and throttle value to control the Boat with the little 5 way switch.

I try to manual control with this Arduino code:

// MAVLink Fernbedienung & Statusdisplay
// Teensy 3.1
// 433Mhz Telemetrie Ground Modul
#include "../mavlink/include/mavlink.h" //Mavlink


#include <SoftwareSerial.h>
SoftwareSerial _MavLinkSerial(0, 1); //Telemetrie TX->Teensy RX=0, Telemetrie RX->Teensy TX=1
// Port für das Telemetriemodul

// | ANTENNE |
// +---------+---------------------------------------+
// | |
// | |
// | |
// | |
// +------+------+------+------+------+------+-------+
// | | | | | | | |
// | 3,3v | TX | RX | -- | -- | -- | GND |
// | |
// | |
// +-------------+--------------------+--------------+
// | |
// | USB Anschluss |
// | |
// +--------------------+

#define START 1
#define MSG_RATE 10 // Hertz



uint8_t system_id = 11; // ID 11 for this boot (10 = Rover) system_id ID of this system
uint8_t component_id = 200; // The component sending the message is the IMU component_id ID of this component (e.g. 200 for IMU)
uint8_t target = 11; //MAV_TYPE_SURFACE_BOAT 10= Rover, 11=boat The system to be controlled
float rollValue = 800; // roll (Ruder beim Boot)
float pitchValue = 500; // pitch (Beim Boot nicht genutzt)
float yawValue = 500; // yaw (Beim Boot nicht genutzt)
float throttleValue = 500; // Throttle (Gas beim Boot)
uint8_t roll_manual = 0; // roll control enabled auto:0, manual:1
uint8_t pitch_manual = 0; // pitch auto:0, manual:1
uint8_t yaw_manual = 0; // yaw auto:0, manual:1
uint8_t thrust_manual = 0; // thrust auto:0, manual:1
mavlink_message_t msg; // msg The MAVLink message to compress the data into
uint8_t buf[MAVLINK_MAX_PACKET_LEN];

uint8_t MavLink_Connected;
uint16_t hb_count;
int32_t gps_status = 0; // (ap_sat_visible * 10) + ap_fixtype
uint8_t ap_cell_count = 0;

unsigned long MavLink_Connected_timer;
unsigned long hb_timer;

int led = 13; //13 Pro Mini Teensy 3.1

void setup()
{
_MavLinkSerial.begin(57600);
MavLink_Connected = 0;
MavLink_Connected_timer=millis();
hb_timer = millis();
hb_count = 0;
}

void loop()
{
uint16_t len;
if(millis()-hb_timer > 1500)
{
hb_timer=millis();
if(!MavLink_Connected) { // Start requesting data streams from MavLink
digitalWrite(led,HIGH);
mavlink_msg_manual_control_pack(system_id, component_id, &msg, target, rollValue, pitchValue, throttleValue, yawValue, roll_manual, pitch_manual, yaw_manual, thrust_manual);
len = mavlink_msg_to_send_buffer(buf, &msg);
_MavLinkSerial.write(buf,len);
digitalWrite(led,LOW);
}
}

if((millis() - MavLink_Connected_timer) > 1500) { // if no HEARTBEAT from APM in 1.5s then we are not connected
MavLink_Connected=0;
hb_count = 0;
}

// MavLink_receive(); // Check MavLink communication

}

Second MAVLink command i don't know how i can do it... to switch the operation mode "Manual", "Auto"

I spend a lot of time so search in the web for some code examples.. but I can't find anything about manual control. 

I hope anybody can help me :)

regards Rene

MAVLink_MANUAL_COMMAND.zip

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –