//=============================================================================================
// MahonyAHRS.c
//=============================================================================================
//
// Madgwick's implementation of Mayhony's AHRS algorithm.
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
//
// From the x-io website "Open-source resources available on this website are
// provided under the GNU General Public Licence unless an alternative licence
// is provided in source."
//
// Date                        Author                        Notes
// 29/09/2011        SOH Madgwick    Initial release
// 02/10/2011        SOH Madgwick        Optimised for reduced CPU load
//
// Algorithm paper:
// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
//
//=============================================================================================

//-------------------------------------------------------------------------------------------
// Header files

#include "hal_ano.h"
#include <math.h>
#include "system.h"
#include "hal_mahonyAHRS.h"

extern unsigned int send_bytes_client(unsigned char *bytes, uint16_t len);

void ANO_Send(uint8_t fun,uint8_t* p,int len)
{
	uint8_t buf[256];
	int L=0;
	uint8_t ver = 0;
   
	buf[L] = 0xAA; ver += buf[L++];
	buf[L] = 0x05; ver += buf[L++];
	buf[L] = 0xAF; ver += buf[L++];
	buf[L] = fun;  ver += buf[L++];
	buf[L] = len;  ver += buf[L++];
	for(int i=0;i<len;i++){
			buf[L] = p[i]; ver += buf[L++];
	}
	buf[L++] = ver;
	send_bytes_client(buf,L);
}

void ANO_Send_Status(float rol,float pit,float yaw,int32_t hight,int8_t mode,int8_t lock,int8_t link)
{
	uint8_t buf[32];
	uint8_t L=0;
   
	int16_t rol_100  = (int16_t)(rol*100);
	int16_t pit_100  = (int16_t)(pit*100);
	int16_t yaw_100  = (int16_t)(yaw*100);
	
	

	buf[L++] = (uint8_t)(rol_100>>8);
	buf[L++] = (uint8_t)(rol_100>>0);
	
	buf[L++] = (uint8_t)(pit_100>>8);
	buf[L++] = (uint8_t)(pit_100>>0);
	
	buf[L++] = (uint8_t)(yaw_100>>8);
	buf[L++] = (uint8_t)(yaw_100>>0);
	
	buf[L++] = (uint8_t)(hight>>24);
	buf[L++] = (uint8_t)(hight>>16);
	buf[L++] = (uint8_t)(hight>>8);
	buf[L++] = (uint8_t)(hight>>0);
	
	buf[L++] = mode; 
	buf[L++] = lock;
	buf[L++] = link;
	
//	SEGGER_RTT_printf(0,"rol=%d,pitch=%d,yaw=%d,hight=%d,mode=%d,lock=%d,link=%d\n",rol_100,pit_100,yaw_100,hight,mode,lock,link);
	
	ANO_Send(0x01,buf,L);
}

void ANO_Send_Senser(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz, int16_t mx, int16_t my, int16_t mz)
{
	uint8_t buf[32];
	uint8_t L=0;
       
	buf[L++] = (uint8_t)(ax>>8);
	buf[L++] = (uint8_t)(ax>>0);
	buf[L++] = (uint8_t)(ay>>8);
	buf[L++] = (uint8_t)(ay>>0);
	buf[L++] = (uint8_t)(az>>8);
	buf[L++] = (uint8_t)(az>>0);
   
	buf[L++] = (uint8_t)(gx>>8);
	buf[L++] = (uint8_t)(gx>>0);
	buf[L++] = (uint8_t)(gy>>8);
	buf[L++] = (uint8_t)(gy>>0);
	buf[L++] = (uint8_t)(gz>>8);
	buf[L++] = (uint8_t)(gz>>0);
   
	buf[L++] = (uint8_t)(mx>>8);
	buf[L++] = (uint8_t)(mx>>0);
	buf[L++] = (uint8_t)(my>>8);
	buf[L++] = (uint8_t)(my>>0);
	buf[L++] = (uint8_t)(mz>>8);
	buf[L++] = (uint8_t)(mz>>0);

	ANO_Send(0x02,buf,L);
}

void ANO_Send_RCData(int16_t thr, int16_t yaw, int16_t rol, int16_t pit, int16_t aux1, int16_t aux2, int16_t aux3, int16_t aux4, int16_t aux5, int16_t aux6)
{
	uint8_t buf[32];
	uint8_t L=0;
       
	buf[L++] = (uint8_t)(thr>>8);
	buf[L++] = (uint8_t)(thr>>0);
	
	buf[L++] = (uint8_t)(yaw>>8);
	buf[L++] = (uint8_t)(yaw>>0);
	
	buf[L++] = (uint8_t)(rol>>8);
	buf[L++] = (uint8_t)(rol>>0);
	
	buf[L++] = (uint8_t)(pit>>8);
	buf[L++] = (uint8_t)(pit>>0);
	
	buf[L++] = (uint8_t)(aux1>>8);
	buf[L++] = (uint8_t)(aux1>>0);
	
	buf[L++] = (uint8_t)(aux2>>8);
	buf[L++] = (uint8_t)(aux2>>0);
	
	buf[L++] = (uint8_t)(aux3>>8);
	buf[L++] = (uint8_t)(aux3>>0);
	
	buf[L++] = (uint8_t)(aux4>>8);
	buf[L++] = (uint8_t)(aux4>>0);
	
	buf[L++] = (uint8_t)(aux5>>8);
	buf[L++] = (uint8_t)(aux5>>0);
	
	buf[L++] = (uint8_t)(aux6>>8);
	buf[L++] = (uint8_t)(aux6>>0);

	ANO_Send(0x03,buf,L);
}

void ANO_Send_Distance(uint8_t num,int16_t dis1_100,int16_t dis2_100,int16_t dis3_100,int16_t dis4_100,int16_t dis5_100,int16_t dis6_100)
{
	uint8_t buf[32];
	uint8_t L=0;
	buf[L++] = (uint8_t)(num>>0);
	
	buf[L++] = (uint8_t)(dis1_100>>8);
	buf[L++] = (uint8_t)(dis1_100>>0);
	
	buf[L++] = (uint8_t)(dis2_100>>8);
	buf[L++] = (uint8_t)(dis2_100>>0);
	
	buf[L++] = (uint8_t)(dis3_100>>8);
	buf[L++] = (uint8_t)(dis3_100>>0);
	
	buf[L++] = (uint8_t)(dis4_100>>8);
	buf[L++] = (uint8_t)(dis4_100>>0);
	
	buf[L++] = (uint8_t)(dis5_100>>8);
	buf[L++] = (uint8_t)(dis5_100>>0);
	
	buf[L++] = (uint8_t)(dis6_100>>8);
	buf[L++] = (uint8_t)(dis6_100>>0);
	
	ANO_Send(0x31,buf,L);
}

void ANO_Send_Location(uint8_t num,int16_t locX_100,int16_t locY_100,int16_t locZ_100,int16_t locDX_100,int16_t locDY_100,int16_t locDZ_100)
{
	uint8_t buf[32];
	uint8_t L=0;
	buf[L++] = (uint8_t)(num>>0);
	
	buf[L++] = (uint8_t)(locX_100>>8);
	buf[L++] = (uint8_t)(locX_100>>0);
	
	buf[L++] = (uint8_t)(locY_100>>8);
	buf[L++] = (uint8_t)(locY_100>>0);
	
	buf[L++] = (uint8_t)(locZ_100>>8);
	buf[L++] = (uint8_t)(locZ_100>>0);
	
	buf[L++] = (uint8_t)(locDX_100>>8);
	buf[L++] = (uint8_t)(locDX_100>>0);
	
	buf[L++] = (uint8_t)(locDY_100>>8);
	buf[L++] = (uint8_t)(locDY_100>>0);
	
	buf[L++] = (uint8_t)(locDZ_100>>8);
	buf[L++] = (uint8_t)(locDZ_100>>0);
	
	ANO_Send(0x32,buf,L);
}


//============================================================================================
// END OF CODE
//============================================================================================