Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: factor vehicle's mavlink send_heartbeat

Browse Source
This commit is contained in:
Peter Barker 2018-03-22 20:38:37 +11:00 committed by Francisco Ferreira
parent a0a1ca4d95
commit fa3b3964e8
3 changed files with 36 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
ArduSub/GCS_Mavlink.cpp
View File

@ -25,16 +25,14 @@ void Sub::gcs_send_deferred(void)
* pattern below when adding any new messages
*/
NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
MAV_TYPE GCS_MAVLINK_Sub::frame_type() const
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = motors.armed() ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
return MAV_TYPE_SUBMARINE;
}
// set system as critical if any failsafe have triggered
if (failsafe.pilot_input || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain) {
system_status = MAV_STATE_CRITICAL;
}
MAV_MODE GCS_MAVLINK_Sub::base_mode() const
{
uint8_t base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
// work out the base_mode. This value is not very useful
// for APM, but we calculate it as best we can so a generic
@ -44,8 +42,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
// only get useful information from the custom_mode, which maps to
// the APM flight mode and has a well defined meaning in the
// ArduPlane documentation
base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
switch (control_mode) {
switch (sub.control_mode) {
case AUTO:
case GUIDED:
case CIRCLE:
@ -63,21 +60,33 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
// override if stick mixing is enabled
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
// we are armed if we are not initialising
if (motors.armed()) {
if (sub.motors.armed()) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t mav_type;
mav_type = MAV_TYPE_SUBMARINE;
return (MAV_MODE)base_mode;
}
gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(mav_type,
base_mode,
custom_mode,
system_status);
uint32_t GCS_MAVLINK_Sub::custom_mode() const
{
return sub.control_mode;
}
MAV_STATE GCS_MAVLINK_Sub::system_status() const
{
// set system as critical if any failsafe have triggered
if (sub.any_failsafe_triggered()) {
return MAV_STATE_CRITICAL;
}
if (sub.motors.armed()) {
return MAV_STATE_ACTIVE;
}
return MAV_STATE_STANDBY;
}
NOINLINE void Sub::send_attitude(mavlink_channel_t chan)
@ -500,7 +509,7 @@ bool GCS_MAVLINK_Sub::try_send_message(enum ap_message id)
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
sub.send_heartbeat(chan);
send_heartbeat();
sub.send_info(chan);
break;

4
ArduSub/GCS_Mavlink.h
View File

@ -38,4 +38,8 @@ private:
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override;
bool try_send_message(enum ap_message id) override;
MAV_TYPE frame_type() const override;
MAV_MODE base_mode() const override;
uint32_t custom_mode() const override;
MAV_STATE system_status() const override;
};

4
ArduSub/Sub.h
View File

@ -285,6 +285,10 @@ private:
uint8_t sensor_health : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes)
} failsafe;
bool any_failsafe_triggered() const {
return (failsafe.pilot_input || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain);
}
// sensor health for logging
struct {
uint8_t baro : 1; // true if any single baro is healthy