Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-02 19:53:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: retry sending home/origin if it fails

Browse Source 
Also grants us the ability to stream these messages
This commit is contained in:
Peter Barker 2019-02-25 12:43:47 +11:00 committed by Peter Barker
parent d02d897928
commit ace5192852
3 changed files with 30 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/GCS_MAVLink/GCS.cpp
View File

@ -49,16 +49,6 @@ void GCS::send_named_float(const char *name, float value) const
FOR_EACH_ACTIVE_CHANNEL(send_named_float(name, value));
}
void GCS::send_home() const
{
FOR_EACH_ACTIVE_CHANNEL(send_home());
}
void GCS::send_ekf_origin() const
{
FOR_EACH_ACTIVE_CHANNEL(send_ekf_origin());
}
/*
install an alternative protocol handler. This allows another
protocol to take over the link if MAVLink goes idle. It is used to

8
libraries/GCS_MAVLink/GCS.h
View File

@ -95,6 +95,8 @@ enum ap_message : uint8_t {
MSG_AOA_SSA,
MSG_LANDING,
MSG_ESC_TELEMETRY,
MSG_ORIGIN,
MSG_HOME,
MSG_NAMED_FLOAT,
MSG_LAST // MSG_LAST must be the last entry in this enum
};
@ -219,8 +221,8 @@ public:
void send_mount_status() const;
void send_named_float(const char *name, float value) const;
void send_gimbal_report() const;
void send_home() const;
void send_ekf_origin() const;
void send_home_position() const;
void send_gps_global_origin() const;
virtual void send_position_target_global_int() { };
void send_servo_output_raw();
static void send_collision_all(const AP_Avoidance::Obstacle &threat, MAV_COLLISION_ACTION behaviour);
@ -742,8 +744,6 @@ public:
void send_message(enum ap_message id);
void send_mission_item_reached_message(uint16_t mission_index);
void send_named_float(const char *name, float value) const;
void send_home() const;
void send_ekf_origin() const;
void send_parameter_value(const char *param_name,
ap_var_type param_type,

38
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -883,7 +883,6 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
// MSG_NAMED_FLOAT messages can't really be "streamed"...
// this list is ordered by AP_MESSAGE ID - the value being returned:
static const struct {
uint32_t mavlink_id;
ap_message msg_id;
@ -891,6 +890,8 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ MAVLINK_MSG_ID_HEARTBEAT, MSG_HEARTBEAT},
{ MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE},
{ MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION},
{ MAVLINK_MSG_ID_HOME_POSITION, MSG_HOME},
{ MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, MSG_ORIGIN},
{ MAVLINK_MSG_ID_SYS_STATUS, MSG_SYS_STATUS},
{ MAVLINK_MSG_ID_POWER_STATUS, MSG_POWER_STATUS},
{ MAVLINK_MSG_ID_MEMINFO, MSG_MEMINFO},
@ -2226,11 +2227,8 @@ void GCS_MAVLINK::send_named_float(const char *name, float value) const
mavlink_msg_named_value_float_send(chan, AP_HAL::millis(), float_name, value);
}
void GCS_MAVLINK::send_home() const
void GCS_MAVLINK::send_home_position() const
{
if (!HAVE_PAYLOAD_SPACE(chan, HOME_POSITION)) {
return;
}
if (!AP::ahrs().home_is_set()) {
return;
}
@ -2249,11 +2247,8 @@ void GCS_MAVLINK::send_home() const
AP_HAL::micros64());
}
void GCS_MAVLINK::send_ekf_origin() const
void GCS_MAVLINK::send_gps_global_origin() const
{
if (!HAVE_PAYLOAD_SPACE(chan, GPS_GLOBAL_ORIGIN)) {
return;
}
Location ekf_origin;
if (!AP::ahrs().get_origin(ekf_origin)) {
return;
@ -2764,7 +2759,10 @@ void GCS_MAVLINK::set_ekf_origin(const Location& loc)
ahrs.Log_Write_Home_And_Origin();
// send ekf origin to GCS
send_ekf_origin();
if (!try_send_message(MSG_ORIGIN)) {
// try again later
send_message(MSG_ORIGIN);
}
}
void GCS_MAVLINK::handle_set_gps_global_origin(const mavlink_message_t *msg)
@ -3451,8 +3449,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_get_home_position(const mavlink_command_l
if (!AP::ahrs().home_is_set()) {
return MAV_RESULT_FAILED;
}
send_home();
send_ekf_origin();
if (!try_send_message(MSG_HOME)) {
// try again later
send_message(MSG_HOME);
}
if (!try_send_message(MSG_ORIGIN)) {
// try again later
send_message(MSG_ORIGIN);
}
return MAV_RESULT_ACCEPTED;
}
@ -3950,6 +3954,16 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_global_position_int();
break;
case MSG_HOME:
CHECK_PAYLOAD_SIZE(HOME_POSITION);
send_home_position();
break;
case MSG_ORIGIN:
CHECK_PAYLOAD_SIZE(GPS_GLOBAL_ORIGIN);
send_gps_global_origin();
break;
case MSG_CURRENT_WAYPOINT:
case MSG_MISSION_ITEM_REACHED:
case MSG_NEXT_MISSION_REQUEST: