From 338445e60cafab09c50e2cbcc8ec8f5624715352 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 20 Nov 2011 19:49:56 +1100 Subject: [PATCH] ArduCopter-HIL: fixed HIL build Logging isn't always enabled --- ArduCopter/GCS_Mavlink.pde | 15 ++++++++++----- ArduCopter/system.pde | 2 ++ 2 files changed, 12 insertions(+), 5 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 9388db597c..9e260a36e0 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1383,27 +1383,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // handle variables with standard type IDs if (var_type == AP_Var::k_typeid_float) { ((AP_Float *)vp)->set_and_save(packet.param_value); +#if LOGGING_ENABLED == ENABLED Log_Write_Data(1, (float)((AP_Float *)vp)->get()); - +#endif } else if (var_type == AP_Var::k_typeid_float16) { ((AP_Float16 *)vp)->set_and_save(packet.param_value); +#if LOGGING_ENABLED == ENABLED Log_Write_Data(2, (float)((AP_Float *)vp)->get()); - +#endif } else if (var_type == AP_Var::k_typeid_int32) { if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); +#if LOGGING_ENABLED == ENABLED Log_Write_Data(3, (int32_t)((AP_Float *)vp)->get()); - +#endif } else if (var_type == AP_Var::k_typeid_int16) { if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); +#if LOGGING_ENABLED == ENABLED Log_Write_Data(4, (int32_t)((AP_Float *)vp)->get()); - +#endif } else if (var_type == AP_Var::k_typeid_int8) { if (packet.param_value < 0) rounding_addition = -rounding_addition; ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); +#if LOGGING_ENABLED == ENABLED Log_Write_Data(5, (int32_t)((AP_Float *)vp)->get()); - +#endif } else { // we don't support mavlink set on this parameter break; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 53bbe591da..44812aaa10 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -330,11 +330,13 @@ static void init_ardupilot() startup_ground(); +#if LOGGING_ENABLED == ENABLED Log_Write_Startup(); Log_Write_Data(10, g.pi_stabilize_roll.kP()); Log_Write_Data(11, g.pi_stabilize_pitch.kP()); Log_Write_Data(12, g.pi_rate_roll.kP()); Log_Write_Data(13, g.pi_rate_pitch.kP()); +#endif SendDebug("\nReady to FLY "); }