diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index ee72e87646..35cc254db2 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -517,6 +517,8 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, case MSG_EXTENDED_STATUS1: CHECK_PAYLOAD_SIZE(SYS_STATUS); send_extended_status1(chan, packet_drops); + CHECK_PAYLOAD_SIZE(POWER_STATUS); + gcs[chan-MAVLINK_COMM_0].send_power_status(); break; case MSG_EXTENDED_STATUS2: diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index a8d329e912..5d9e238dc6 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -454,6 +454,9 @@ static void Log_Write_Current() current_total : battery.current_total_mah() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); + + // also write power status + DataFlash.Log_Write_Power(); } struct PACKED log_Compass {