added support for AP_ADC MAVLink packet

this adds AP_ADC, which sends raw ADC 16 bit values for all 6 ADC
channels at the Extra3 MAVLink stream rate. Extra3 was previously
unused
This commit is contained in:
Andrew Tridgell 2011-09-10 19:08:18 +10:00
parent 7a0f19d49d
commit cba8953ce4
3 changed files with 17 additions and 3 deletions

View File

@ -131,8 +131,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
} }
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
// Available datastream send_message(MSG_RAW_ADC);
//Serial.printf("mav8 %d\n", (int)streamRateExtra3.get());
} }
} }
} }

View File

@ -344,6 +344,20 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_
break; 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: default:
break; 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() // switch types in mavlink_try_send_message()
static struct mavlink_queue { static struct mavlink_queue {
uint8_t deferred_messages[MAX_DEFERRED_MESSAGES]; uint8_t deferred_messages[MAX_DEFERRED_MESSAGES];

View File

@ -176,6 +176,7 @@
#define MSG_GPS_RAW 0x64 #define MSG_GPS_RAW 0x64
#define MSG_SERVO_OUT 0x70 #define MSG_SERVO_OUT 0x70
#define MSG_RAW_ADC 0x71
#define MSG_PIN_REQUEST 0x80 #define MSG_PIN_REQUEST 0x80
#define MSG_PIN_SET 0x81 #define MSG_PIN_SET 0x81