From c762abdd004a03c1916e54e4c8950926bcf5fb63 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 19 Nov 2011 16:58:20 -0800 Subject: [PATCH] Logging APVar saves --- ArduCopter/GCS_Mavlink.pde | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index efda1da0ac..def4cba2be 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -1367,6 +1367,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) // find the requested parameter vp = AP_Var::find(key); + if ((NULL != vp) && // exists !isnan(packet.param_value) && // not nan !isinf(packet.param_value)) { // not inf @@ -1382,19 +1383,27 @@ 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); + Log_Write_Data(1, (float)((AP_Float *)vp)->get()); } else if (var_type == AP_Var::k_typeid_float16) { ((AP_Float16 *)vp)->set_and_save(packet.param_value); + Log_Write_Data(2, (float)((AP_Float *)vp)->get()); } 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); + Log_Write_Data(3, (int32_t)((AP_Float *)vp)->get()); + } 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); + Log_Write_Data(4, (int32_t)((AP_Float *)vp)->get()); + } 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); + Log_Write_Data(5, (int32_t)((AP_Float *)vp)->get()); + } else { // we don't support mavlink set on this parameter break; @@ -1410,6 +1419,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) vp->cast_to_float(), _count_parameters(), -1); // XXX we don't actually know what its index is... + } break;