APM MAVlink mit Arduino auslesen?

WDZaphod

Erfahrener Benutzer
#1
Hallo zusammen,

ich versuche gerade, mit einem Arduino Pro (16MHZ, 5V) den MAVlink-Datenstrom meines APM 2.5 zu lesen / decodieren. MIttendrin kam jetzt die Idee, einfach mal zu fragen: Hat das hier schonmal jemand gemacht?
Um das Rad nicht 2x zu erfinden...
Konkret will ich die Höhe des fliegenden Quads auslesen.

Grüße,
Michael
 

matala

Erfahrener Benutzer
#3
Hallo zusammen,
Konkret will ich die Höhe des fliegenden Quads auslesen.
hallo

wenn du den stromsensor angeschlossen und unter hardware eingestellt hast--
dann unter logs alle eingestellt hast--
kannst du die höhe mit dem APMLogVisualizer auslesen

glück auf
matala
 

WDZaphod

Erfahrener Benutzer
#4
Mmh, ich will die Daten live auslesen, und den Arduino in's Quad packen :D
Einbauort nähe MinimOSD, da sind die passenden Strippen ja vorhanden.
Konkret will ich mir ein Variometer bauen, welches jedesmal einen hohen Piepton abgibt, wenn man 1m steigt, und einen tiefen Ton, wenn man 1m sinkt. An der Häufigkeit der Töne und deren Frequenz kann man dann hören, ob man steigt oder sinkt, und wie schnell.
Das Audio-Signal soll direkt in den VTx gehen, dann piept es im Kopfhörer an der Fatshark :)
 

WDZaphod

Erfahrener Benutzer
#5
@olex:

Ja, der ist allerdings völliger Overkill :eek:
Ich will eigentlich nur einen Wert abgreifen, den müsste man doch so einfach irgendwie finden?
Meines Wissens gibt es da verschieden Zeilen, die sich am Anfang durch eine Kennung unterscheiden, und je nach Kennung dann weiter hinten in bestimmten Bytes die benötigten werte stehen haben?
Sowas sollte man doch ein einem 10-Zeiler abhandeln können, ohne zig Libs zu laden?

Ich hatte mal sowas programmiert, um Werte vom Fahrzeugbus (obd2) auszulesen:

Code:
void loop() {   pointer=0; zeichen=0; for(int x = 0;x<29;x++) {buffer[x]=0;}  // Variablen auf 0 setzen, buffer leeren
                while (zeichen != 13) { // Solange das letzte Zeichen kein LF war...
                                        buffer[pointer] = Serial.read(); LastMessage = millis();
                                        zeichen = (buffer[pointer]); 
                                        if ( zeichen !=32 && zeichen !=13 && zeichen !=10 && zeichen !=62 ) { pointer++; }
                                       } 
    
// Sequenzen erkennen:
if ( buffer[2]=='C' && buffer[3]=='4' && buffer[4]=='B' && buffer[5]=='0' && buffer[6]=='9' && buffer[7]=='0' && buffer[8]=='0' && buffer[9]=='0' ) MachWas1();     
if ( buffer[2]=='C' && buffer[3]=='4' && buffer[4]=='B' && buffer[5]=='0' && buffer[6]=='1' && buffer[7]=='0' && buffer[8]=='2' && buffer[9]=='2' ) MachWas2();   
if ( buffer[2]=='C' && buffer[3]=='4' && buffer[4]=='B' && buffer[5]=='0' && buffer[6]=='1' && buffer[7]=='0' && buffer[8]=='0' && buffer[9]=='0' ) MachWas3();  
}
 

Yups

Erfahrener Benutzer
#6
Hey!

Schau bei autoquad.org nach dem mavtohott. Das Bau ich mir gerade für dem apm um. Schreib mir doch mal ne PN.
 

Roberto

Erfahrener Benutzer
#7
So wie ich das sehe muss beim Mavlink auch ein Wert angefordert werden. Die Kommunikation ist also bidirektional. Ausserdem musst Du noch eine Checksumme bilden und mitschicken. Anders sieht das aus, wenn schon minimosd mit der apm "redet". Deinen 2. Arduino brauchst Du dann nur noch mit RX an die APM TX Leitung zu hängen und auf Dein Datenpaket warten (mit passender Baudrate..).
Alternativ kannst Du sicherlich auch das Variometersignal über einen der vielen, freien arduino pins als pwm ausgeben lassen und dann mit dem anderen einlesen undundundundundund.......

LG
Rob
 
Zuletzt bearbeitet:

WDZaphod

Erfahrener Benutzer
#8
@Yups: Gerade mal angesehen, danke für den Link! Alledings ist da auch ganz schön Code am Start, da müsste man ja erst mal 95% rauswerfen :rolleyes:
Laut allem, was ich bisher über das Protokoll gelesen habe, sollte das erheblich einfacher gehen?

@Roberto: Den MinimOSD hab ich dran, das wäre also kein Problem.
Was ich mir vorstelle: Ich lausche am Bus mit passender Baudrate, und fülle so lange ein Byte-Array, bis ein Zeilenendezeichen kommt.
Dann checke ich, ob vorne am Array die passende Kennnung steht (es werden anscheinend verschiedene Zeilen mit unterschiedlicher Länge und unterschiedlichem Inhalt geschickt), wenn nein, Array verwerfen und auf die nächste Zeile warten.
Wenn die mit dem richtigen Anfang kommt, werden die passenden Bytes genommen, und in die Höhe umgerechnet.
Der Rest ist dann einfach...
 

Roberto

Erfahrener Benutzer
#9
Da musst Du Dich mal in das Navlink Protokoll reinknien. Zeilenende erkennen könnte ggf ein Problem sein, besser auf den Anfang des gesuchten Datenblocks warten. Selbst wenn mal ein Datenblock flöten geht, kannst Du immer noch die Höhenänderung aus den vorhanden Daten errechnen, wenn Du sie mit Timestamp gespeichert hast. Das Variometer beim minimosd zeigt bei mit nur +- 3m/s. Keine Ahnung, was da tatsächlich gesendet wird. Vielleicht werden auch schon bessere variometerdaten gesendet? Hier würde ich auch mal schauen: https://code.google.com/p/mwc-ng/ da wird ein abgespecktes mavlink verwendet.
 

Yups

Erfahrener Benutzer
#10
Ja richtig, das ist alles schon recht umfangreich.
Allerdings geht es sicherlich wesentlich schneller die entscheidenden Sachen da zu kopieren als alles neu zu entwickeln.

Alternativ kannst du dir auch die jdrones IO Board software anschauen. Soweit ich weiss sendet die sogar einen request an den APM.
http://code.google.com/p/arducodes/downloads/detail?name=jD-IOBoard-191012.zip&can=2&q=
... und löst damit mein Hauptproblem. Der APM sendet nur Daten wenn ich den MissionPlanner verbinde, also einen mavlink request sende. Normalerweise kein Problem, da ich den Kopter meistens im Wegpunktmodus benutze. Aber um mal kurz zu fliegen ist das ziemlich nervig.

Im Quellcode vom Arducopter findet man eine Beschreibung der Mavlink Parameter:

ArduCopter\Source\ArduCopter-2.9.1b\libraries\GCS_MAVLink\message_definitions

Dies ist der entscheidende Teil aus dem Quellcode:

Code:
#define MAVLINK_COMM_NUM_BUFFERS 1
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS

// this code was moved from libraries/GCS_MAVLink to allow compile
// time selection of MAVLink 1.0
BetterStream	*mavlink_comm_0_port;
BetterStream	*mavlink_comm_1_port;

mavlink_system_t mavlink_system = {12,1,0,0};

#include "Mavlink_compat.h"
#include "include/mavlink/v1.0/mavlink_types.h"
#include "include/mavlink/v1.0/common/mavlink.h" 

static int packet_drops = 0;
static int parse_error = 0;

void request_mavlink_rates()
{

//serial debug entfernt
          }
          break;
          
          case MAVLINK_MSG_ID_SYS_STATUS:
          { 
            // AQ sends   (0,0,0,idle %, batt, batt remaining, drops, 0,0,0,0) 
            m2h_vbat_A = (mavlink_msg_sys_status_get_voltage_battery(&msg) / 100.0f);    // It will arive in mV, but Hott uses V * 100
            //m2h_battery_remaining_A = mavlink_msg_sys_status_get_battery_remaining(&msg);    // not used in HoTT 
            m2h_abat = mavlink_msg_sys_status_get_current_battery(&msg);   
          }
          break;
/*
 * @param fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
 * @param lat Latitude in 1E7 degrees
 * @param lon Longitude in 1E7 degrees
 * @param alt Altitude in 1E3 meters (millimeters) above MSL
 * @param eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 * @param epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535
 * @param vel GPS ground speed (m/s * 100). If unknown, set to: 65535
 * @param cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535
 * @param satellites_visible Number of satellites visible. If unknown, set to 255
 */
        case MAVLINK_MSG_ID_GPS_RAW_INT:    // is send by AutoQad
          {      
            m2h_fix_type = mavlink_msg_gps_raw_int_get_fix_type(&msg); //1
            m2h_gps_lon = mavlink_msg_gps_raw_int_get_lon(&msg);    // latitude 2
            m2h_gps_lat = mavlink_msg_gps_raw_int_get_lat(&msg);    // longitude 3
            m2h_gps_alt = mavlink_msg_gps_raw_int_get_alt(&msg);    // gps altitude 4
            m2h_gps_vel = mavlink_msg_gps_raw_int_get_vel(&msg);    // speed in cm/s 5
            m2h_gps_cog = mavlink_msg_gps_raw_int_get_cog(&msg);    // direction of movement 6
            m2h_satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(&msg);            
          }
          break;

        case MAVLINK_MSG_ID_ATTITUDE:
          {
            m2h_pitch = ToDeg(mavlink_msg_attitude_get_pitch(&msg));
            m2h_roll = ToDeg(mavlink_msg_attitude_get_roll(&msg));
            m2h_yaw = ToDeg(mavlink_msg_attitude_get_yaw(&msg));
          }
          break;
/**
 * @brief Pack a scaled_pressure message on a channel
 * @param system_id ID of this system
 * @param component_id ID of this component (e.g. 200 for IMU)
 * @param chan The MAVLink channel this message was sent over
 * @param msg The MAVLink message to compress the data into
 * @param time_boot_ms Timestamp (microseconds since UNIX epoch or microseconds since system boot)  -- AutoQuad
 * @param press_abs Absolute pressure (hectopascal)                             -- AutoQuad AQ_PRESSURE*0.01f
 * @param press_diff Differential pressure 1 (hectopascal)                      -- AutoQuad 0.0f
 * @param temperature Temperature measurement (0.01 degrees celsius)            -- AutoQuad
 * @return length of the message in bytes (excluding serial stream start sign)
 */          
        case MAVLINK_MSG_ID_SCALED_PRESSURE:
        {
           m2h_abs = mavlink_msg_scaled_pressure_get_press_abs(&msg);       // absolute presure          
           m2h_temp = mavlink_msg_scaled_pressure_get_temperature(&msg);    // internal pressure sensor temp
          }
          break;
/**
 * @return RC channel 1 value, in microseconds
 */
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
        {
           throttle = mavlink_msg_rc_channels_raw_get_chan3_raw(&msg);
        }
        break;
        
/**
* @param target_system System ID
 * @param latitude global position * 1E7
 * @param longitude global position * 1E7
 * @param altitude global position * 1000
 */
        case MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT:
          {
           m2h_gps_lat_home = mavlink_msg_set_global_position_setpoint_int_get_latitude(&msg);
           m2h_gps_lon_home = mavlink_msg_set_global_position_setpoint_int_get_longitude(&msg);
           m2h_ceiling = mavlink_msg_set_global_position_setpoint_int_get_altitude(&msg);
           if ( (m2h_gps_lat_home !=0) &&  (m2h_gps_lon_home !=0)) m2h_got_home = 1;
          }
          break;

        case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
          {
           m2h_rel_alt = mavlink_msg_global_position_int_get_relative_alt(&msg);
                     }
          break;

          /* I know, totaly misused this message with other variables but the Android app doesn't take any other 'alien' message */
        case MAVLINK_MSG_ID_VFR_HUD:
          { 
           m2h_gps_lat_home = mavlink_msg_vfr_hud_get_airspeed(&msg);
           m2h_gps_lon_home = mavlink_msg_vfr_hud_get_groundspeed(&msg);
           m2h_ceiling = mavlink_msg_vfr_hud_get_alt(&msg);
           m2h_heading = mavlink_msg_vfr_hud_get_heading(&msg);
           if ( (m2h_gps_lat_home !=0) &&  (m2h_gps_lon_home !=0)) m2h_got_home = 1;
          }
          break;
          
        case MAVLINK_MSG_ID_STATUSTEXT:
          {   
           #ifdef SERDB            
             DPL(mavlink_msg_statustext_get_severity(&msg));
           #endif
          }  
          break;
// * * * end messages * * *       
       default:
       //Do nothing
       break;
      }
    }
    delayMicroseconds(138);
    //next one
  }
  // Update global packet drops counter
  packet_drops += status.packet_rx_drop_count;
  parse_error += status.parse_error;
  
}

------------------------------

Ich habe mal meine leicht angepasste APM Version von Mav2Hott angehängt.
Da gibt es leider ein dickes Problem das ich nicht gelöst bekomme. Gleich vorweg: ich habe nur ein paar "basics" im Programmieren und fummel mich da eher durch als dass ich alles durschaue.

Unter "Functions" wir die Funktion "checkBattery" alle 1Hz durchlaufen. Das möchte ich nutzen um anhand der der int16 variable m2h_abat = strom die verbrauchte kapazität zu errechnen.

Code:
  m2h_cbat = (m2h_cbat + (((int16_t)(m2h_abat))  / 3600));                                  // cumulate battery capacity
Das funktioniert prinzipiell auch, nur hängt sich dabei das jdrones ioboard (arduino pro mini) fast komplett auf.
Wenn ich die Zeile deaktiviere läuft alles wie gewohnt. Was mache ich falsch?


Danke & Gruß
Yannick
 

Anhänge

Zuletzt bearbeitet:
FPV1

Banggood

Oben Unten