Browse Source

ArduCopter-HIL: fixed HIL build

Logging isn't always enabled
mission-4.1.18
Andrew Tridgell 13 years ago committed by Pat Hickey
parent
commit
338445e60c
  1. 15
      ArduCopter/GCS_Mavlink.pde
  2. 2
      ArduCopter/system.pde

15
ArduCopter/GCS_Mavlink.pde

@ -1383,27 +1383,32 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
// handle variables with standard type IDs // handle variables with standard type IDs
if (var_type == AP_Var::k_typeid_float) { if (var_type == AP_Var::k_typeid_float) {
((AP_Float *)vp)->set_and_save(packet.param_value); ((AP_Float *)vp)->set_and_save(packet.param_value);
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(1, (float)((AP_Float *)vp)->get()); Log_Write_Data(1, (float)((AP_Float *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_float16) { } else if (var_type == AP_Var::k_typeid_float16) {
((AP_Float16 *)vp)->set_and_save(packet.param_value); ((AP_Float16 *)vp)->set_and_save(packet.param_value);
#if LOGGING_ENABLED == ENABLED
Log_Write_Data(2, (float)((AP_Float *)vp)->get()); Log_Write_Data(2, (float)((AP_Float *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_int32) { } else if (var_type == AP_Var::k_typeid_int32) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int32 *)vp)->set_and_save(packet.param_value+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()); Log_Write_Data(3, (int32_t)((AP_Float *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_int16) { } else if (var_type == AP_Var::k_typeid_int16) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int16 *)vp)->set_and_save(packet.param_value+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()); Log_Write_Data(4, (int32_t)((AP_Float *)vp)->get());
#endif
} else if (var_type == AP_Var::k_typeid_int8) { } else if (var_type == AP_Var::k_typeid_int8) {
if (packet.param_value < 0) rounding_addition = -rounding_addition; if (packet.param_value < 0) rounding_addition = -rounding_addition;
((AP_Int8 *)vp)->set_and_save(packet.param_value+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()); Log_Write_Data(5, (int32_t)((AP_Float *)vp)->get());
#endif
} else { } else {
// we don't support mavlink set on this parameter // we don't support mavlink set on this parameter
break; break;

2
ArduCopter/system.pde

@ -330,11 +330,13 @@ static void init_ardupilot()
startup_ground(); startup_ground();
#if LOGGING_ENABLED == ENABLED
Log_Write_Startup(); Log_Write_Startup();
Log_Write_Data(10, g.pi_stabilize_roll.kP()); Log_Write_Data(10, g.pi_stabilize_roll.kP());
Log_Write_Data(11, g.pi_stabilize_pitch.kP()); Log_Write_Data(11, g.pi_stabilize_pitch.kP());
Log_Write_Data(12, g.pi_rate_roll.kP()); Log_Write_Data(12, g.pi_rate_roll.kP());
Log_Write_Data(13, g.pi_rate_pitch.kP()); Log_Write_Data(13, g.pi_rate_pitch.kP());
#endif
SendDebug("\nReady to FLY "); SendDebug("\nReady to FLY ");
} }

Loading…
Cancel
Save