diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index db34c15389..c1546d95a4 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -131,8 +131,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - // Available datastream - //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); + send_message(MSG_RAW_ADC); } } } diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h index 3de4438cc2..4c2147b2c4 100644 --- a/ArduPlane/Mavlink_Common.h +++ b/ArduPlane/Mavlink_Common.h @@ -344,6 +344,20 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ break; } + + case MSG_RAW_ADC: + { + CHECK_PAYLOAD_SIZE(AP_ADC); + mavlink_msg_ap_adc_send(chan, + analogRead(BATTERY_PIN1), + analogRead(BATTERY_PIN2), + analogRead(BATTERY_PIN3), + analogRead(BATTERY_PIN4), + analogRead(AN4), + analogRead(AN5)); + break; + } + default: break; } @@ -351,7 +365,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ } -#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of +#define MAX_DEFERRED_MESSAGES 18 // should be at least equal to number of // switch types in mavlink_try_send_message() static struct mavlink_queue { uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index b48deed2da..3929123647 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -176,6 +176,7 @@ #define MSG_GPS_RAW 0x64 #define MSG_SERVO_OUT 0x70 +#define MSG_RAW_ADC 0x71 #define MSG_PIN_REQUEST 0x80 #define MSG_PIN_SET 0x81