From bf18fb896acf77fd6b09ab234a9ab38fba47f6ad Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Sep 2014 22:17:46 +0900 Subject: [PATCH] Copter: send extended status to GCS only after initialisation --- ArduCopter/ArduCopter.pde | 1 + ArduCopter/GCS_Mavlink.pde | 12 ++++++++---- ArduCopter/system.pde | 3 +++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b6dc7dbd2e..befe1c8bc2 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -388,6 +388,7 @@ static union { uint8_t rc_receiver_present : 1; // 14 // true if we have an rc receiver present (i.e. if we've ever received an update uint8_t compass_mot : 1; // 15 // true if we are currently performing compassmot calibration uint8_t motor_test : 1; // 16 // true if we are currently performing the motors test + uint8_t initialised : 1; // 17 // true once the init_ardupilot function has completed. Extended status to GCS is not sent until this completes }; uint32_t value; } ap; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8de62a6c13..ec3b5c5d5c 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -481,10 +481,14 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id) break; case MSG_EXTENDED_STATUS1: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - send_extended_status1(chan); - CHECK_PAYLOAD_SIZE(POWER_STATUS); - gcs[chan-MAVLINK_COMM_0].send_power_status(); + // send extended status only once vehicle has been initialised + // to avoid unnecessary errors being reported to user + if (ap.initialised) { + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan); + CHECK_PAYLOAD_SIZE(POWER_STATUS); + gcs[chan-MAVLINK_COMM_0].send_power_status(); + } break; case MSG_EXTENDED_STATUS2: diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 89c0f5df54..e0fa98560d 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -295,6 +295,9 @@ static void init_ardupilot() } cliSerial->print_P(PSTR("\nReady to FLY ")); + + // flag that initialisation has completed + ap.initialised = true; }