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

Copter: move sending of sys_status message up

Browse Source
This commit is contained in:
Peter Barker 2019-01-23 13:04:46 +11:00 committed by Andrew Tridgell
parent 7ea223eac6
commit 7197cf9e5c
3 changed files with 9 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/Copter.h
View File

@ -727,7 +727,6 @@ private:
// GCS_Mavlink.cpp // GCS_Mavlink.cpp
void gcs_send_heartbeat(void); void gcs_send_heartbeat(void);
void send_sys_status(mavlink_channel_t chan);
void send_nav_controller_output(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan);
void send_rpm(mavlink_channel_t chan); void send_rpm(mavlink_channel_t chan);

39
ArduCopter/GCS_Mavlink.cpp
View File

@ -118,30 +118,15 @@ void GCS_MAVLINK_Copter::send_position_target_global_int()
0.0f); // yaw_rate 0.0f); // yaw_rate
} }
NOINLINE void Copter::send_sys_status(mavlink_channel_t chan) void GCS_MAVLINK_Copter::get_sensor_status_flags(uint32_t &present,
uint32_t &enabled,
uint32_t &health)
{ {
int16_t battery_current = -1; copter.update_sensor_status_flags();
int8_t battery_remaining = -1;
if (battery.has_current() && battery.healthy()) { present = copter.control_sensors_present;
battery_remaining = battery.capacity_remaining_pct(); enabled = copter.control_sensors_enabled;
battery_current = battery.current_amps() * 100; health = copter.control_sensors_health;
}
update_sensor_status_flags();
mavlink_msg_sys_status_send(
chan,
control_sensors_present,
control_sensors_enabled,
control_sensors_health,
(uint16_t)(scheduler.load_average() * 1000),
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
0, // comm drops in pkts,
0, 0, 0, 0);
} }
void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan) void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
@ -269,16 +254,6 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
switch(id) { switch(id) {
case MSG_SYS_STATUS:
// send extended status only once vehicle has been initialised
// to avoid unnecessary errors being reported to user
if (!vehicle_initialised()) {
return true;
}
CHECK_PAYLOAD_SIZE(SYS_STATUS);
copter.send_sys_status(chan);
break;
case MSG_NAV_CONTROLLER_OUTPUT: case MSG_NAV_CONTROLLER_OUTPUT:
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
copter.send_nav_controller_output(chan); copter.send_nav_controller_output(chan);

2
ArduCopter/GCS_Mavlink.h
View File

@ -48,6 +48,8 @@ private:
bool vehicle_initialised() const override; bool vehicle_initialised() const override;
void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
void packetReceived(const mavlink_status_t &status, void packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg) override; mavlink_message_t &msg) override;