MAVLink Manual Control via Arduino / Teensy

[mosmutzel]

Hi allerseits

Ich möchte eine kleine Fernbedienung mit einem Teensy 3.1 (Teensyduino) realisieren mit der ich zum einen den Mode (Auto, Manual, Guide ect.) und zum anderen die Steuerbefehle vor / zurück / Seite an ein Arduboot senden möchte. Eventuell später auch an meinen Quadcopter (TBS Discovery mit Pixhawk)

Das Auslesen vom MAVLink habe ich bereits erfolgreich gelöst. Nun wird auf einem kleinen Oled der Status der Batterie, GPS echt angezeigt. Nur bei den Steuer befehlen komme ich nicht weiter.
https://vimeo.com/124046596

Der Code für das Senden der MAVLink befehle schaut wie folgt aus:

#include "../libraries/mavlink/include/mavlink.h" //Mavlink

#include 

// Port für das Telemetriemodul

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

SoftwareSerial _MavLinkSerial(0, 1); //Telemetrie TX->Teensy RX=0, Telemetrie RX->Teensy TX=1 

#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;
   } 
}

Beim senden bekomme ich keine Fehlermeldungen o.Ä. allerdings wird auch nix gesteuert (z.b. das Ruder (roll))

Zum Thema Mode umschalten habe ich auch noch keinen Beispielcode gefunden.

hoffe Ihr könnt mir etwas weiter helfen.
LG René

 

[nusbr]

Bin auch gerade dabei, mich in dem Mavlink Protokoll zu verwirklichen. Leider bin ich noch nicht so weit wie du gekommen. Würdest du mir freundlicherweise den Code zur Verfügung stellen? Danke und lg Bruno