aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

GCS_MAVLink: move handling of incoming statutext messages up

Browse Source
This commit is contained in:
Peter Barker 2017-07-12 17:21:15 +10:00 committed by Francisco Ferreira
parent 208607d668
commit 129d7220e6
4 changed files with 27 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/GCS_MAVLink/GCS.h
View File

@ -103,6 +103,8 @@ public:
// accessor for uart
AP_HAL::UARTDriver *get_uart() { return _port; }
virtual uint8_t sysid_my_gcs() const = 0;
static const struct AP_Param::GroupInfo var_info[];
// set to true if this GCS link is active
@ -265,7 +267,8 @@ protected:
void handle_device_op_write(mavlink_message_t *msg);
void handle_timesync(mavlink_message_t *msg);
void handle_statustext(mavlink_message_t *msg);
private:
float adjust_rate_for_stream_trigger(enum streams stream_num);

21
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -1685,6 +1685,22 @@ void GCS_MAVLINK::handle_timesync(mavlink_message_t *msg)
);
}
void GCS_MAVLINK::handle_statustext(mavlink_message_t *msg)
{
DataFlash_Class *df = DataFlash_Class::instance();
if (df == nullptr) {
return;
}
// ignore any statustext messages not from our GCS:
if (msg->sysid != sysid_my_gcs()) {
return;
}
mavlink_statustext_t packet;
mavlink_msg_statustext_decode(msg, &packet);
char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G','C','S',':'};
memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN);
df->Log_Write_Message(text);
}
/*
handle messages which don't require vehicle specific data
@ -1739,7 +1755,12 @@ void GCS_MAVLINK::handle_common_message(mavlink_message_t *msg)
case MAVLINK_MSG_ID_MISSION_SET_CURRENT:
handle_common_mission_message(msg);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
handle_statustext(msg);
break;
}
}
void GCS_MAVLINK::handle_common_mission_message(mavlink_message_t *msg)

1
libraries/GCS_MAVLink/GCS_Dummy.h
View File

@ -15,6 +15,7 @@ class GCS_MAVLINK_Dummy : public GCS_MAVLINK
protected:
AP_Mission *get_mission() override { return nullptr; }
uint8_t sysid_my_gcs() const override { return 1; }
};

1
libraries/GCS_MAVLink/examples/routing/routing.cpp
View File

@ -22,6 +22,7 @@ protected:
uint32_t telem_delay() const override { return 0; }
AP_Mission *get_mission() override { return nullptr; }
uint8_t sysid_my_gcs() const override { return 1; }
private: