Copter: factor vehicle's mavlink send_heartbeat

This commit is contained in:
Peter Barker 2018-03-22 19:50:24 +11:00 committed by Francisco Ferreira
parent 4fd2ec1cef
commit 0c8110ecb2
4 changed files with 42 additions and 20 deletions

View File

@ -391,6 +391,10 @@ private:
uint8_t adsb : 1; // true if an adsb related failsafe has occurred
} failsafe;
bool any_failsafe_triggered() const {
return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb;
}
// sensor health for logging
struct {
uint8_t baro : 1; // true if baro is healthy
@ -776,7 +780,6 @@ private:
// GCS_Mavlink.cpp
void gcs_send_heartbeat(void);
void gcs_send_deferred(void);
void send_heartbeat(mavlink_channel_t chan);
void send_attitude(mavlink_channel_t chan);
void send_fence_status(mavlink_channel_t chan);
void send_extended_status1(mavlink_channel_t chan);
@ -949,7 +952,7 @@ private:
void check_usb_mux(void);
bool should_log(uint32_t mask);
void set_default_frame_class();
uint8_t get_frame_mav_type();
MAV_TYPE get_frame_mav_type();
const char* get_frame_string();
void allocate_motors(void);

View File

@ -22,17 +22,14 @@ void Copter::gcs_send_deferred(void)
* pattern below when adding any new messages
*/
NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
MAV_TYPE GCS_MAVLINK_Copter::frame_type() const
{
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
uint8_t system_status = ap.land_complete ? MAV_STATE_STANDBY : MAV_STATE_ACTIVE;
uint32_t custom_mode = control_mode;
// set system as critical if any failsafe have triggered
if (failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb) {
system_status = MAV_STATE_CRITICAL;
}
return copter.get_frame_mav_type();
}
MAV_MODE GCS_MAVLINK_Copter::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
// MAVLink enabled ground station can work out something about
@ -41,8 +38,7 @@ NOINLINE void Copter::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 (copter.control_mode) {
case AUTO:
case RTL:
case LOITER:
@ -71,19 +67,37 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
#endif
// we are armed if we are not initialising
if (motors != nullptr && motors->armed()) {
if (copter.motors != nullptr && copter.motors->armed()) {
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
}
// indicate we have set a custom mode
base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
gcs().chan(chan-MAVLINK_COMM_0).send_heartbeat(get_frame_mav_type(),
base_mode,
custom_mode,
system_status);
return (MAV_MODE)base_mode;
}
uint32_t GCS_MAVLINK_Copter::custom_mode() const
{
return copter.control_mode;
}
MAV_STATE GCS_MAVLINK_Copter::system_status() const
{
// set system as critical if any failsafe have triggered
if (copter.any_failsafe_triggered()) {
return MAV_STATE_CRITICAL;
}
if (copter.ap.land_complete) {
return MAV_STATE_STANDBY;
}
return MAV_STATE_ACTIVE;
}
NOINLINE void Copter::send_attitude(mavlink_channel_t chan)
{
const Vector3f &gyro = ins.get_gyro();
@ -296,7 +310,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
case MSG_HEARTBEAT:
CHECK_PAYLOAD_SIZE(HEARTBEAT);
last_heartbeat_time = AP_HAL::millis();
copter.send_heartbeat(chan);
send_heartbeat();
break;
case MSG_EXTENDED_STATUS1:

View File

@ -48,4 +48,9 @@ private:
void packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg) 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;
};

View File

@ -446,7 +446,7 @@ void Copter::set_default_frame_class()
}
// return MAV_TYPE corresponding to frame class
uint8_t Copter::get_frame_mav_type()
MAV_TYPE Copter::get_frame_mav_type()
{
switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
case AP_Motors::MOTOR_FRAME_QUAD: