diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4866a7d047..b8aa27879f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1356,6 +1356,7 @@ static void slow_loop() if(motors.armed()) { if (g.log_bitmask & MASK_LOG_ITERM) Log_Write_Iterm(); + Log_Write_Data(30, initial_simple_bearing); }else{ // check the user hasn't updated the frame orientation motors.set_frame_orientation(g.frame_orientation); diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 8baea04f75..805235418d 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -320,6 +320,7 @@ static void init_ardupilot() #endif + /////////////////////////////////////////////////////////////////////////////// // Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters //////////////////////////////////////////////////////////////////////////////// @@ -394,6 +395,8 @@ static void startup_ground(void) static void set_mode(byte mode) { + Log_Write_Data(31, initial_simple_bearing); + // if we don't have GPS lock if(home_is_set == false) { // THOR @@ -575,6 +578,7 @@ static void set_mode(byte mode) } Log_Write_Mode(control_mode); + Log_Write_Data(32, initial_simple_bearing); } static void set_failsafe(boolean mode)