text: /* ================================================================================================
     JTQuad
     
     Author: Jerry Tostao   
     ================================================================================================ */
  
  /* ================================================================================================
     Includes
     ================================================================================================ */
     
  	#include "heading_fusion_processor_marg.h"
  
  /* ================================================================================================
     Externs
     ================================================================================================ */
     
  	extern double G_Dt;
  	
  /* ================================================================================================
     Externs
     ================================================================================================ */
     
  	#define LKPACC			0.2			// IMPORTANT: LKPACC and LKPMAG both share register temporary_1 in function MagCalculateHeading
  	#define LKPMAG			0.2			// IMPORTANT: LKPACC and LKPMAG both share register temporary_1 in function MagCalculateHeading
  	#define LKIACC			0.0005		// Register temporary_2 in function MagCalculateHeading
  	//#define LKIMAG			0.0005		// Not used
  	
  /* ================================================================================================
     Functions
     ================================================================================================ */
  
  /* ----------------------------------------------------------------------------
     Initialize heading fusion parameters
     ---------------------------------------------------------------------------- */
  void initialize_heading_fusion(void)
  {
  	// Assume flat craft: (ax, ay, az) of (0.0, 0.0, -9.8)
  	fpu_wait();
  	fpu_eeprom_function_call(FPU_EEPROM_FUNC_MAG_INITIALIZE_HEADING_FUSION);
  	fpu_wait();
  }
  
  /* ----------------------------------------------------------------------------
     Calculate heading
       - true north heading
  	 - magnetic declination not applied at this time so heading is magnetic north
  	   and not true north
  	 - true north heading is needed if integrating GPS
     ---------------------------------------------------------------------------- */
  void calculate_heading(void)
  {
  	fpu_wait();
  	fpu_write_double_to_reg_nn(G_Dt, FPU_REG_TEMPORARY_3);		// lhalfT is temporary_3 in function MagCalculateHeading
  	fpu_write_double_to_reg_nn(LKPACC, FPU_REG_TEMPORARY_1);	// lkpacc and lkpmag have the same value so they share register temporary_1 in function MagCalculateHeading
  	fpu_write_double_to_reg_nn(LKIACC, FPU_REG_TEMPORARY_2);	// lkiacc is register temporary_2 in function MagCalculateHeading
  	fpu_function_call(FPU_FUNC_MAG_CALCULATE_HEADING);
  	fpu_wait();
  }
  
  /* ----------------------------------------------------------------------------
     FPU function heading fusion
       - generated by FPU IDE
  	 - readable function implemented in .fpu file
     ---------------------------------------------------------------------------- */
  void write_mag_init_heading_fusion_function_to_fpu_eeprom(void)
  {
  	fpu_wait();
      fpu_write4(EEWRITE, FPU_EEPROM_FUNC_MAG_INITIALIZE_HEADING_FUSION, 0xB5, 0xB4);
      fpu_write4(0x01, 0x01, 0x04, 0x01);
      fpu_write4(0x02, 0x04, 0x01, 0x03);
      fpu_write4(0x20, 0x76, 0x4D, 0x75);
      fpu_write4(0x07, 0x03, 0x7E, 0x01);
      fpu_write4(0x04, 0x20, 0x03, 0x37);
      fpu_write4(0x02, 0x01, 0x77, 0x14);
      fpu_write4(0x20, 0x01, 0x48, 0x15);
      fpu_write4(0x29, 0x14, 0x20, 0x02);
      fpu_write4(0x48, 0x15, 0x2D, 0x14);
      fpu_write4(0x20, 0x04, 0x48, 0x15);
      fpu_write4(0x2D, 0x14, 0x20, 0x01);
      fpu_write4(0x47, 0x14, 0x20, 0x02);
      fpu_write4(0x47, 0x14, 0x20, 0x04);
      fpu_write4(0x47, 0x15, 0x2D, 0x15);
      fpu_write4(0x2D, 0x15, 0x2A, 0x01);
      fpu_write4(0x78, 0x14, 0x20, 0x01);
      fpu_write4(0x47, 0x15, 0x29, 0x14);
      fpu_write4(0x20, 0x02, 0x48, 0x15);
      fpu_write4(0x2D, 0x14, 0x20, 0x04);
      fpu_write4(0x48, 0x15, 0x2D, 0x14);
      fpu_write4(0x20, 0x01, 0x48, 0x14);
      fpu_write4(0x20, 0x02, 0x47, 0x14);
      fpu_write4(0x20, 0x04, 0x47, 0x15);
      fpu_write4(0x2D, 0x15, 0x2D, 0x15);
      fpu_write4(0x2B, 0x01, 0x79, 0x14);
      fpu_write4(0x20, 0x01, 0x48, 0x15);
      fpu_write4(0x29, 0x14, 0x20, 0x02);
      fpu_write4(0x47, 0x15, 0x2D, 0x14);
      fpu_write4(0x20, 0x04, 0x48, 0x15);
      fpu_write4(0x2D, 0x14, 0x20, 0x01);
      fpu_write4(0x47, 0x14, 0x20, 0x02);
      fpu_write4(0x48, 0x14, 0x20, 0x04);
      fpu_write4(0x47, 0x15, 0x2D, 0x15);
      fpu_write4(0x2D, 0x15, 0x2A, 0x01);
      fpu_write4(0x7A, 0x14, 0x20, 0x01);
      fpu_write4(0x48, 0x15, 0x29, 0x14);
      fpu_write4(0x20, 0x02, 0x48, 0x15);
      fpu_write4(0x2D, 0x14, 0x20, 0x04);
      fpu_write4(0x47, 0x15, 0x2D, 0x14);
      fpu_write4(0x20, 0x01, 0x47, 0x14);
      fpu_write4(0x20, 0x02, 0x47, 0x14);
      fpu_write4(0x20, 0x04, 0x48, 0x15);
      fpu_write4(0x2D, 0x15, 0x2D, 0x15);
      fpu_write4(0x2B, 0x03, 0x7B, 0x03);
      fpu_write4(0x7C, 0x03, 0x7D, 0x80);
      fpu_wait();
  }