From 27e69d73d50def78f33ab5b864fac34faa19df7a Mon Sep 17 00:00:00 2001 From: "mich146@hotmail.com" Date: Fri, 15 Apr 2011 13:24:05 +0000 Subject: [PATCH] ACM mavlink update/log fix git-svn-id: https://arducopter.googlecode.com/svn/trunk@1881 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/ArduCopterMega.pde | 3 +- ArduCopterMega/GCS.h | 30 ++++- ArduCopterMega/GCS_Mavlink.pde | 194 ++++++++++++++++++------------ ArduCopterMega/Log.pde | 15 ++- ArduCopterMega/Mavlink_Common.h | 19 ++- ArduCopterMega/defines.h | 2 + ArduCopterMega/global_data.h | 52 -------- ArduCopterMega/global_data.pde | 20 --- 8 files changed, 165 insertions(+), 170 deletions(-) delete mode 100644 ArduCopterMega/global_data.h delete mode 100644 ArduCopterMega/global_data.pde diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index fefbf13730..6047113f14 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -52,7 +52,6 @@ version 2.1 of the License, or (at your option) any later version. // Local modules #include "defines.h" #include "Parameters.h" -#include "global_data.h" #include "GCS.h" #include "HIL.h" @@ -1170,4 +1169,4 @@ adjust_altitude() } } } -} \ No newline at end of file +} diff --git a/ArduCopterMega/GCS.h b/ArduCopterMega/GCS.h index 8276f41fe4..1798f28234 100644 --- a/ArduCopterMega/GCS.h +++ b/ArduCopterMega/GCS.h @@ -65,12 +65,13 @@ public: /// void send_text(uint8_t severity, const char *str) {} +#define send_text_P(severity, msg) send_text(severity, msg) /// Send a text message with a PSTR() /// /// @param severity A value describing the importance of the message. /// @param str The text to be sent. /// - void send_text_P(uint8_t severity, const prog_char_t *str) {} + void send_text(uint8_t severity, const prog_char_t *str) {} /// Send acknowledgement for a message. /// @@ -140,7 +141,7 @@ public: void init(BetterStream *port); void send_message(uint8_t id, uint32_t param = 0); void send_text(uint8_t severity, const char *str); - void send_text_P(uint8_t severity, const prog_char_t *str); + void send_text(uint8_t severity, const prog_char_t *str); void acknowledge(uint8_t id, uint8_t sum1, uint8_t sum2); void data_stream_send(uint16_t freqMin, uint16_t freqMax); private: @@ -152,6 +153,7 @@ private: AP_Var *_queued_parameter; ///< next parameter to be sent in queue uint16_t _queued_parameter_index; ///< next queued parameter's index + uint16_t _queued_parameter_count; ///< saved count of parameters for queued send /// Count the number of reportable parameters. /// @@ -175,6 +177,30 @@ private: uint16_t rawControllerStreamRate; uint16_t rcStreamRate; uint16_t extraStreamRate[3]; + + // waypoints + uint16_t requested_interface; // request port to use + uint16_t waypoint_request_i; // request index + uint16_t waypoint_dest_sysid; // where to send requests + uint16_t waypoint_dest_compid; // " + bool waypoint_sending; // currently in send process + bool waypoint_receiving; // currently receiving + uint16_t waypoint_count; + uint32_t waypoint_timelast_send; // milliseconds + uint32_t waypoint_timelast_receive; // milliseconds + uint16_t waypoint_send_timeout; // milliseconds + uint16_t waypoint_receive_timeout; // milliseconds + float junk; //used to return a junk value for interface + + // data stream rates + uint16_t streamRateRawSensors; + uint16_t streamRateExtendedStatus; + uint16_t streamRateRCChannels; + uint16_t streamRateRawController; + uint16_t streamRatePosition; + uint16_t streamRateExtra1; + uint16_t streamRateExtra2; + uint16_t streamRateExtra3; }; #endif // GCS_PROTOCOL_MAVLINK diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 7604c99796..8269140d24 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -5,7 +5,21 @@ #include "Mavlink_Common.h" GCS_MAVLINK::GCS_MAVLINK() : -packet_drops(0) +packet_drops(0), +// parameters +// note, all values not explicitly initialised here are zeroed +waypoint_send_timeout(1000), // 1 second +waypoint_receive_timeout(1000), // 1 second +// stream rates +streamRateRawSensors(0), +streamRateExtendedStatus(0), +streamRateRCChannels(0), +streamRateRawController(0), +//streamRateRawSensorFusion(0), +streamRatePosition(0), +streamRateExtra1(0), +streamRateExtra2(0), +streamRateExtra3(0) { } @@ -20,6 +34,7 @@ GCS_MAVLINK::init(BetterStream * port) mavlink_comm_1_port = port; chan = MAVLINK_COMM_1; } + _queued_parameter = NULL; } void @@ -45,55 +60,58 @@ GCS_MAVLINK::update(void) _queued_send(); // stop waypoint sending if timeout - if (global_data.waypoint_sending && (millis() - global_data.waypoint_timelast_send) > global_data.waypoint_send_timeout){ + if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout")); - global_data.waypoint_sending = false; + waypoint_sending = false; } // stop waypoint receiving if timeout - if (global_data.waypoint_receiving && (millis() - global_data.waypoint_timelast_receive) > global_data.waypoint_receive_timeout){ + if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout")); - global_data.waypoint_receiving = false; + waypoint_receiving = false; } } void GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) { - if (freqLoopMatch(global_data.streamRateRawSensors,freqMin,freqMax)) + if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { + if (freqLoopMatch(streamRateRawSensors,freqMin,freqMax)) send_message(MSG_RAW_IMU); - if (freqLoopMatch(global_data.streamRateExtendedStatus,freqMin,freqMax)) { + if (freqLoopMatch(streamRateExtendedStatus,freqMin,freqMax)) { send_message(MSG_EXTENDED_STATUS); send_message(MSG_GPS_STATUS); send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working +// send_message(MSG_NAV_CONTROLLER_OUTPUT); } - if (freqLoopMatch(global_data.streamRatePosition,freqMin,freqMax)) { + if (freqLoopMatch(streamRatePosition,freqMin,freqMax)) { send_message(MSG_LOCATION); } - if (freqLoopMatch(global_data.streamRateRawController,freqMin,freqMax)) { + if (freqLoopMatch(streamRateRawController,freqMin,freqMax)) { // This is used for HIL. Do not change without discussing with HIL maintainers send_message(MSG_SERVO_OUT); } - if (freqLoopMatch(global_data.streamRateRCChannels,freqMin,freqMax)) { + if (freqLoopMatch(streamRateRCChannels,freqMin,freqMax)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); } - if (freqLoopMatch(global_data.streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info + if (freqLoopMatch(streamRateExtra1,freqMin,freqMax)){ // Use Extra 1 for AHRS info send_message(MSG_ATTITUDE); } - if (freqLoopMatch(global_data.streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info + if (freqLoopMatch(streamRateExtra2,freqMin,freqMax)){ // Use Extra 2 for additional HIL info send_message(MSG_VFR_HUD); } - if (freqLoopMatch(global_data.streamRateExtra3,freqMin,freqMax)){ + if (freqLoopMatch(streamRateExtra3,freqMin,freqMax)){ // Available datastream + } } } @@ -110,12 +128,12 @@ GCS_MAVLINK::send_text(uint8_t severity, const char *str) } void -GCS_MAVLINK::send_text_P(uint8_t severity, const prog_char_t *str) +GCS_MAVLINK::send_text(uint8_t severity, const prog_char_t *str) { mavlink_statustext_t m; uint8_t i; for (i=0; icompid, g.waypoint_total + 1); // + home - global_data.waypoint_timelast_send = millis(); - global_data.waypoint_sending = true; - global_data.waypoint_receiving = false; - global_data.waypoint_dest_sysid = msg->sysid; - global_data.waypoint_dest_compid = msg->compid; - global_data.requested_interface = chan; + waypoint_timelast_send = millis(); + waypoint_sending = true; + waypoint_receiving = false; + waypoint_dest_sysid = msg->sysid; + waypoint_dest_compid = msg->compid; + requested_interface = chan; break; } @@ -320,7 +355,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); // Check if sending waypiont - if (!global_data.waypoint_sending) break; + if (!waypoint_sending) break; // decode mavlink_waypoint_request_t packet; @@ -331,7 +366,12 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) tell_command = get_wp_with_index(packet.seq); // set frame of waypoint - uint8_t frame = MAV_FRAME_GLOBAL; // reference frame + uint8_t frame; + if (tell_command.options & WP_OPTION_ALT_RELATIVE) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame + } else { + frame = MAV_FRAME_GLOBAL; // reference frame + } float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; // time that the mav should loiter in milliseconds @@ -344,7 +384,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // command needs scaling x = tell_command.lat/1.0e7; // local (x), global (latitude) y = tell_command.lng/1.0e7; // local (y), global (longitude) - z = tell_command.alt/1.0e2; // local (z), global (altitude) + if (tell_command.options & WP_OPTION_ALT_RELATIVE) { + z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt + } else { + z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) + } } switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields @@ -391,7 +435,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) param1,param2,param3,param4,x,y,z); // update last waypoint comm stamp - global_data.waypoint_timelast_send = millis(); + waypoint_timelast_send = millis(); break; } @@ -408,7 +452,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) uint8_t type = packet.type; // ok (0), error(1) // turn off waypoint send - global_data.waypoint_sending = false; + waypoint_sending = false; break; } @@ -425,6 +469,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) _queued_parameter = AP_Var::first(); _queued_parameter_index = 0; + _queued_parameter_count = _count_parameters(); + requested_interface = chan; break; } @@ -483,17 +529,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) packet.count = MAX_WAYPOINTS; } g.waypoint_total.set_and_save(packet.count - 1); - global_data.waypoint_timelast_receive = millis(); - global_data.waypoint_receiving = true; - global_data.waypoint_sending = false; - global_data.waypoint_request_i = 0; + waypoint_timelast_receive = millis(); + waypoint_receiving = true; + waypoint_sending = false; + waypoint_request_i = 0; break; } case MAVLINK_MSG_ID_WAYPOINT: { // Check if receiving waypiont - if (!global_data.waypoint_receiving) break; + if (!waypoint_receiving) break; // decode mavlink_waypoint_t packet; @@ -501,7 +547,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (mavlink_check_target(packet.target_system,packet.target_component)) break; // check if this is the requested waypoint - if (packet.seq != global_data.waypoint_request_i) break; + if (packet.seq != waypoint_request_i) break; // store waypoint uint8_t loadAction = 0; // 0 insert in list, 1 exec now @@ -511,12 +557,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (packet.frame) { - case MAV_FRAME_GLOBAL: case MAV_FRAME_MISSION: + case MAV_FRAME_GLOBAL: { tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 tell_command.alt = packet.z*1.0e2; // in as m converted to cm + tell_command.options = 0; break; } @@ -525,19 +572,17 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) tell_command.lat = 1.0e7*ToDeg(packet.x/ (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = -packet.z*1.0e2 + home.alt; + tell_command.alt = packet.z*1.0e2; + tell_command.options = 1; break; } case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { - if (!home_is_set) { - send_text_P(SEVERITY_MEDIUM, PSTR("Refusing relative alt wp: home not set")); - return; - } tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2 + home.alt; + tell_command.alt = packet.z*1.0e2; + tell_command.options = 1; break; } } @@ -586,15 +631,13 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) - // save waypoint - and prevent re-setting home - if (packet.seq != 0) set_wp_with_index(tell_command, packet.seq); // update waypoint receiving state machine - global_data.waypoint_timelast_receive = millis(); - global_data.waypoint_request_i++; + waypoint_timelast_receive = millis(); + waypoint_request_i++; - if (global_data.waypoint_request_i > g.waypoint_total) + if (waypoint_request_i > g.waypoint_total) { uint8_t type = 0; // ok (0), error(1) @@ -605,7 +648,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) type); send_text_P(SEVERITY_LOW,PSTR("flight plan received")); - global_data.waypoint_receiving = false; + waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter } @@ -657,7 +700,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); } else if (var_type == AP_Var::k_typeid_int8) { - ((AP_Int8 *)vp)->set(packet.param_value+rounding_addition); + ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); } else { // we don't support mavlink set on this parameter break; @@ -846,7 +889,7 @@ GCS_MAVLINK::_queued_send() { // Check to see if we are sending parameters - while (NULL != _queued_parameter) { + if (NULL != _queued_parameter && (requested_interface == chan) && mavdelay > 1) { AP_Var *vp; float value; @@ -865,13 +908,12 @@ GCS_MAVLINK::_queued_send() chan, (int8_t*)param_name, value, - _count_parameters(), + _queued_parameter_count, _queued_parameter_index); _queued_parameter_index++; - break; } - + mavdelay = 0; @@ -882,19 +924,19 @@ GCS_MAVLINK::_queued_send() // request waypoints one by one // XXX note that this is pan-interface - if (global_data.waypoint_receiving && - (global_data.requested_interface == chan) && - global_data.waypoint_request_i <= g.waypoint_total && + if (waypoint_receiving && + (requested_interface == chan) && + waypoint_request_i <= g.waypoint_total && mavdelay > 15) { // limits to 3.33 hz mavlink_msg_waypoint_request_send( chan, - global_data.waypoint_dest_sysid, - global_data.waypoint_dest_compid, - global_data.waypoint_request_i); + waypoint_dest_sysid, + waypoint_dest_compid, + waypoint_request_i); mavdelay = 0; } } #endif - + diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 4adb106522..61043cada5 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -449,7 +449,6 @@ void Log_Write_GPS( long log_Time, long log_Lattitude, long log_Longitude, long DataFlash.WriteLong(log_Ground_Speed); DataFlash.WriteLong(log_Ground_Course); DataFlash.WriteByte(END_BYTE); - DataFlash.WriteByte(END_BYTE); } // Write an raw accel/gyro data packet. Total length : 28 bytes @@ -532,7 +531,7 @@ void Log_Read_Performance() long pm_time; int logvar; - Serial.print("PM:"); + Serial.printf_P(PSTR("PM:")); pm_time = DataFlash.ReadLong(); Serial.print(pm_time); Serial.print(comma); @@ -554,7 +553,7 @@ void Log_Read_Cmd() byte logvarb; long logvarl; - Serial.print("CMD:"); + Serial.printf_P(PSTR("CMD:")); for(int i = 1; i < 4; i++) { logvarb = DataFlash.ReadByte(); Serial.print(logvarb, DEC); @@ -585,7 +584,7 @@ void Log_Read_Startup() // Read an attitude packet void Log_Read_Attitude() { - Serial.printf_P(PSTR("ATT: %d, %d, %d\n"), + Serial.printf_P(PSTR("ATT: %d, %d, %u\n"), DataFlash.ReadInt(), DataFlash.ReadInt(), (uint16_t)DataFlash.ReadInt()); @@ -594,7 +593,7 @@ void Log_Read_Attitude() // Read a mode packet void Log_Read_Mode() { - Serial.print("MOD:"); + Serial.printf_P(PSTR("MOD:")); Serial.println(flight_mode_strings[DataFlash.ReadByte()]); } @@ -618,7 +617,7 @@ void Log_Read_GPS() void Log_Read_Raw() { float logvar; - Serial.print("RAW:"); + Serial.printf_P(PSTR("RAW:")); for (int y = 0; y < 6; y++) { logvar = (float)DataFlash.ReadLong() / t7; Serial.print(logvar); @@ -631,8 +630,8 @@ void Log_Read_Raw() void Log_Read(int start_page, int end_page) { byte data; - byte log_step; - int packet_count; + byte log_step = 0; + int packet_count = 0; int page = start_page; diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 1893ba4b97..38c2ef74de 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -5,7 +5,6 @@ #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK -uint16_t system_type = MAV_FIXED_WING; byte mavdelay = 0; // what does this do? @@ -31,7 +30,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui case MSG_HEARTBEAT: mavlink_msg_heartbeat_send( chan, - system_type, + mavlink_system.type, MAV_AUTOPILOT_ARDUPILOTMEGA); break; @@ -149,10 +148,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui // This is used for HIL. Do not change without discussing with HIL maintainers mavlink_msg_rc_channels_scaled_send( chan, - g.rc_1.norm_output(), - g.rc_2.norm_output(), - g.rc_3.norm_output(), - g.rc_4.norm_output(), + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), 0, 0, 0, @@ -199,10 +198,10 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui chan, (float)airspeed / 100.0, (float)g_gps->ground_speed / 100.0, - dcm.yaw_sensor, + (dcm.yaw_sensor / 100) % 360, + nav_throttle, current_loc.alt / 100.0, - climb_rate, - nav_throttle); + climb_rate); break; } @@ -258,7 +257,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui break; } - defualt: + default: break; } } diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 515dbc52ba..f62a80980d 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -271,3 +271,5 @@ // parameters get the first 1KiB of EEPROM, remainder is for waypoints #define WP_START_BYTE 0x400 // where in memory home WP is stored + all other WP #define WP_SIZE 15 +#define ONBOARD_PARAM_NAME_LENGTH 15 +#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe diff --git a/ArduCopterMega/global_data.h b/ArduCopterMega/global_data.h deleted file mode 100644 index ee15837779..0000000000 --- a/ArduCopterMega/global_data.h +++ /dev/null @@ -1,52 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef __GLOBAL_DATA_H -#define __GLOBAL_DATA_H - -#define ONBOARD_PARAM_NAME_LENGTH 15 -#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe - -#define FIRMWARE_VERSION 18 // <-- change on param data struct modifications - -#ifdef __cplusplus - -/// -// global data structure -// This structure holds all the vehicle parameters. -// TODO: bring in varibles floating around in ArduPilotMega.pde -// -struct global_struct -{ - // waypoints - uint16_t requested_interface; // request port to use - uint16_t waypoint_request_i; // request index - uint16_t waypoint_dest_sysid; // where to send requests - uint16_t waypoint_dest_compid; // " - bool waypoint_sending; // currently in send process - bool waypoint_receiving; // currently receiving - uint16_t waypoint_count; - uint32_t waypoint_timelast_send; // milliseconds - uint32_t waypoint_timelast_receive; // milliseconds - uint16_t waypoint_send_timeout; // milliseconds - uint16_t waypoint_receive_timeout; // milliseconds - float junk; //used to return a junk value for interface - - // data stream rates - uint16_t streamRateRawSensors; - uint16_t streamRateExtendedStatus; - uint16_t streamRateRCChannels; - uint16_t streamRateRawController; - uint16_t streamRatePosition; - uint16_t streamRateExtra1; - uint16_t streamRateExtra2; - uint16_t streamRateExtra3; - - // struct constructor - global_struct(); -} global_data; - -extern "C" const char *param_nametab[]; - -#endif // __cplusplus - -#endif // __GLOBAL_DATA_H diff --git a/ArduCopterMega/global_data.pde b/ArduCopterMega/global_data.pde deleted file mode 100644 index 5dcd11cfb5..0000000000 --- a/ArduCopterMega/global_data.pde +++ /dev/null @@ -1,20 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -global_struct::global_struct() : - // parameters - // note, all values not explicitly initialised here are zeroed - waypoint_send_timeout(1000), // 1 second - waypoint_receive_timeout(1000), // 1 second - - // stream rates - streamRateRawSensors(0), - streamRateExtendedStatus(0), - streamRateRCChannels(0), - streamRateRawController(0), - //streamRateRawSensorFusion(0), - streamRatePosition(0), - streamRateExtra1(0), - streamRateExtra2(0), - streamRateExtra3(0) -{ -}