Hello,
I have the problem that I receive the high correctly, but when I want to receive the thrust (additionally also placed on channel 7) and channel 6, I get random values.
I have Mavlink set up as above, use an Aruino Mega and and use a Pixhawk 6X.
I hope maybe someone can help me because I just can not find the error.
#include <mavlink.h>
#include <SoftwareSerial.h>
//#include "common/mavlink_msg_gps_raw_int.h"
SoftwareSerial _MavLinkSerial(11, 10); // PIN 9=Telemetry TX->Pixhawk RX, PIN 10=Telemetry RX->Pixhawk TX
// 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() {
// MAVLink interface start
_MavLinkSerial.begin(57600);
Serial.begin(115200);
Serial.println("MAVLink starting.");
}
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_QUADROTOR; ///< 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)
{
// Record last HB update
previousMillisMAVLink = currentMillisMAVLink;
//Mav_Request_Data();
num_hbs_pasados++;
if (num_hbs_pasados >= num_hbs) {
// Request streams from Pixhawk
Serial.println("Streams requested!");
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];
uint8_t _system_id = 255; // id of computer which is sending the command (ground control software has id of 255)
uint8_t _component_id = 2; // seems like it can be any # except the number of what Pixhawk sys_id is
uint8_t _target_system = 1; // Id # of Pixhawk (should be 1)
uint8_t _target_component = 0; // Target component, 0 = all (seems to work with 0 or 1
uint8_t _req_stream_id = MAV_DATA_STREAM_ALL;
uint16_t _req_message_rate = 0x01; //number of times per second to request the data in hex
uint8_t _start_stop = 1; //1 = start, 0 = stop
// 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(_system_id, _component_id, &msg, _target_system, _target_component, _req_stream_id, _req_message_rate, _start_stop);
//uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
//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);
_MavLinkSerial.write(buf, len);
}
}
void comm_receive() {
mavlink_message_t msg;
mavlink_status_t status;
while (_MavLinkSerial.available() > 0) {
uint8_t c = _MavLinkSerial.read();
// Try to get a new message
if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
{
Serial.print("DEBUG msgid:"); Serial.println(msg.msgid);
// 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
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: // #30
{
mavlink_global_position_int_t Position;
mavlink_msg_global_position_int_decode(&msg, &Position);
Serial.print("Position high: "); Serial.println(Position.relative_alt);
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: // #35
{
mavlink_rc_channels_scaled_t RCCHANNEL;
mavlink_msg_rc_channels_scaled_decode(&msg, &RCCHANNEL);
Serial.print("Chanel 6 (3-Kanal Schalter): ");
int RAW_SERVO = RCCHANNEL.chan6_scaled;
Serial.println(RAW_SERVO);
Serial.print("Chanel 7 (Schub): ");
Serial.println(RCCHANNEL.chan3_scaled);
Serial.print("Drei Kanal: ");
Serial.println(mavlink_msg_rc_channels_scaled_get_chan6_scaled(&msg));
Serial.print("Schub: ");
Serial.println(mavlink_msg_rc_channels_scaled_get_chan3_scaled(&msg));
}
case MAVLINK_MSG_ID_MANUAL_CONTROL: // #69
{
mavlink_manual_control_t SCHUB;
mavlink_msg_manual_control_decode(&msg, &SCHUB);
//mavlink_msg_rc_channels_scaled_decode(&msg, &RCCHANNEL);
//mavlink_msg_manual_control_get_thrust(const mavlink_message_t* msg)
Serial.print("Schub2: ");
Serial.println(SCHUB.thrust);
Serial.print("Schub3: ");
Serial.println(mavlink_msg_manual_control_get_thrust(&msg));
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: // #69
{
mavlink_servo_output_raw_t SERVOX;
mavlink_msg_servo_output_raw_decode(&msg, &SERVOX);
Serial.print("Schub4: ");
Serial.println(SERVOX.servo7_raw);
Serial.print("Schub5: ");
Serial.println(mavlink_msg_servo_output_raw_get_servo7_raw(&msg));
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: // #35
{
mavlink_rc_channels_raw_t chs;
mavlink_msg_rc_channels_raw_decode(&msg, &chs);
Serial.print("Chanel 6 (3-Kanal Schalter): ");
Serial.println(chs.chan3_raw);
}
break;
}
}
}
}
