// locks onto a particular target sysid and sets it's position data stream to at least 1hz
void Tracker::mavlink_check_target(const mavlink_message_t* msg)
{
// exit immediately if the target has already been set
if (target_set) {
return;
}
// decode
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
// exit immediately if this is not a vehicle we would track
if ((packet.type == MAV_TYPE_ANTENNA_TRACKER) ||
(packet.type == MAV_TYPE_GCS) ||
(packet.type == MAV_TYPE_ONBOARD_CONTROLLER) ||
(packet.type == MAV_TYPE_GIMBAL)) {
return;
}
// set our sysid to the target, this ensures we lock onto a single vehicle
if (g.sysid_target == 0) {
g.sysid_target = msg->sysid;
}
// send data stream request to target on all channels
// Note: this doesn't check success for all sends meaning it's not guaranteed the vehicle's positions will be sent at 1hz
for (uint8_t i=0; i < num_gcs; i++) {
if (gcs_chan[i].initialised) {
// request position
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
msg->sysid,
msg->compid,
MAV_DATA_STREAM_POSITION,
g.mavlink_update_rate,
1); // start streaming
}
// request air pressure
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
msg->sysid,
msg->compid,
MAV_DATA_STREAM_RAW_SENSORS,
g.mavlink_update_rate,
1); // start streaming
}
}
}
// flag target has been set
target_set = true;
}
void Aircraft::requestMavlinkStreams()
{
const int maxStreams = 6;
const uint8_t MAVStreams[maxStreams] =
{
MAV_DATA_STREAM_RAW_SENSORS,
MAV_DATA_STREAM_EXTENDED_STATUS,
MAV_DATA_STREAM_POSITION,
MAV_DATA_STREAM_EXTRA1,
MAV_DATA_STREAM_EXTRA2,
MAV_DATA_STREAM_EXTRA3
};
const uint16_t MAVRates[maxStreams] =
{
1, // MAV_DATA_STREAM_RAW_SENSORS
2, // MAV_DATA_STREAM_EXTENDED_STATUS
2, // MAV_DATA_STREAM_POSITION
5, // MAV_DATA_STREAM_EXTRA1
2, // MAV_DATA_STREAM_EXTRA2
2 // MAV_DATA_STREAM_EXTRA3
};
for (int i = 0; i < maxStreams; i++)
{
mavlink_msg_request_data_stream_send(MAVLINK_COMM_0, m_mavSystem, m_mavComponent, MAVStreams[i], MAVRates[i], 1);
}
}
void Mavlink::makeRateRequest()
{
const int maxStreams = 6;
const unsigned short MAVStreams[maxStreams] = {MAV_DATA_STREAM_RAW_SENSORS,
MAV_DATA_STREAM_EXTENDED_STATUS,
MAV_DATA_STREAM_RC_CHANNELS,
MAV_DATA_STREAM_POSITION,
MAV_DATA_STREAM_EXTRA1,
MAV_DATA_STREAM_EXTRA2};
const unsigned int MAVRates[maxStreams] = {0x02, 0x02, 0x05, 0x02, 0x05, 0x02};
for (int i=0; i < maxStreams; i++) {
mavlink_msg_request_data_stream_send(MAVLINK_COMM_0,
apm_mav_system, apm_mav_component,
MAVStreams[i], MAVRates[i], 1);
}
}
void GCS_Tracker::request_datastream_airpressure(const uint8_t sysid, const uint8_t compid)
{
for (uint8_t i=0; i < num_gcs(); i++) {
if (gcs().chan(i).initialised) {
// request air pressure
if (HAVE_PAYLOAD_SPACE((mavlink_channel_t)i, DATA_STREAM)) {
mavlink_msg_request_data_stream_send(
(mavlink_channel_t)i,
sysid,
compid,
MAV_DATA_STREAM_RAW_SENSORS,
tracker.g.mavlink_update_rate,
1); // start streaming
}
}
}
}
void Mavlink::makeRateRequest() {
//I suppose that I ask ArduPilot to send me 6 data streams, each at different time rate. The higher number, the higher rate.
//I have troubles makind this work - for some reason, the stream with attitude (roll/pitch/yaw) seems to be glitchy - sometimes I don't receive anything for 500ms, otherwise there is update as soon as 100ms.
const int maxStreams = 6;
const unsigned short MAVStreams[maxStreams] = {MAV_DATA_STREAM_RAW_SENSORS,
MAV_DATA_STREAM_EXTENDED_STATUS,
MAV_DATA_STREAM_RC_CHANNELS,
MAV_DATA_STREAM_POSITION,
MAV_DATA_STREAM_EXTRA1,
MAV_DATA_STREAM_EXTRA2};
const unsigned int MAVRates[maxStreams] = {0x04, 0x04, 0x01, 0x03, 0x08, 0x08};
for (int i=0; i < maxStreams; i++) {
mavlink_msg_request_data_stream_send(MAVLINK_COMM_0,
apm_mav_system, apm_mav_component,
MAVStreams[i], MAVRates[i], 1);
}
}
int main (void)
{
char textBuffer[50];
uint8_t batReqs=0;
uint8_t heartbeats=0;
uint8_t statusFlag=0;
uint8_t gpsFlag=0;
uint8_t batFlag=0;
setCurBuf(0);
setPwrBuf(0,0);
setAltBuf(0);
setAirBuf(0);
setGPSBuf(0,0,0,0,0,0);
TWISlaveInit();
#ifdef TIMEIT
TimerInit();
#endif
mavlink_system.sysid = 1;
mavlink_system.compid = 50;
USART_Init();
mavlink_message_t msg;
mavlink_status_t status;
while(1)
{
uint8_t byte=usartGetChar();
if(mavlink_parse_char(MAVLINK_COMM_0, byte,&msg,&status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
heartbeats++;
for (int n=1;n<10;n++)
PORTB ^= (1<<PB5);
// After a couple of heartbeats, try up to 3 times to get the battery capacity
if((heartbeats>2)&&(!batFlag)&&(batReqs<3))
{
mavlink_msg_param_request_read_send(MAVLINK_COMM_0,0x01,0x00,capName,-1);
batReqs++;
}
// if you go 2 heartbeats without getting the status or GPS message, re-request the telemetry stream
if(((!statusFlag)||(!gpsFlag))&&(heartbeats>2))
{
mavlink_msg_request_data_stream_send(MAVLINK_COMM_0, 0x01, 0x00, 0x01, 0x200, 0x01);
heartbeats=0;
}
else
{
statusFlag=0;
gpsFlag=0;
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE:
mavlink_msg_param_value_get_param_id(&msg,textBuffer);
float value=mavlink_msg_param_value_get_param_value(&msg);
if(strncmp(textBuffer,capName,16)==0)
{
batteryCapacity=(uint16_t)value;
batFlag=1;
}
break;
case MAVLINK_MSG_ID_SYS_STATUS:
setPwrBuf(mavlink_msg_sys_status_get_voltage_battery(&msg),mavlink_msg_sys_status_get_battery_remaining(&msg));
setCurBuf(mavlink_msg_sys_status_get_current_battery(&msg));
statusFlag=1;
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
setAltBuf(mavlink_msg_gps_raw_int_get_alt(&msg));
#ifdef TIMEIT
TCNT1=0;
#endif
setGPSBuf(mavlink_msg_gps_raw_int_get_lat(&msg),mavlink_msg_gps_raw_int_get_lon(&msg),mavlink_msg_gps_raw_int_get_alt(&msg),
mavlink_msg_gps_raw_int_get_vel(&msg),mavlink_msg_gps_raw_int_get_cog(&msg),
mavlink_msg_gps_raw_int_get_satellites_visible(&msg));
#ifdef TIMEIT
timeVal=TCNT1;
#endif
setAirBuf(mavlink_msg_gps_raw_int_get_vel(&msg));
gpsFlag=1;
break;
case MAVLINK_MSG_ID_STATUSTEXT:
mavlink_msg_statustext_get_text(&msg,textBuffer);
// Set status for Powerbox when the low battery message is received
if(strncmp(textBuffer,lowBattery,strlen(lowBattery))==0)
alarm |= (1<<LOW_BATTERY);
break;
default:
break;
}
}
}
}
