#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>


#include "AP_UAVCAN.h"
#include "HVcallback.h"





//hight volt HighVoltThrusterSetRegs.hpp




void HVcycle(uint8_t _code,uint8_t _buffer[6],HVmes &Thruster){

	uint8_t code = (_code&0xF0)>>4;//bfd
	switch (code)
		{
			case 0x03:
				{
					Thruster.voltage = _buffer[0]+(_buffer[1]<<8);
					Thruster.speed = (int16_t)(_buffer[2]+(_buffer[3]<<8)) ;//+ (_buffer[4]<<16) + (_buffer[5]<<24));
					Thruster.currentlimit = (int16_t)(_buffer[4]+(_buffer[5]<<8));
					
				}
				break;
			case 0x05:
				{
					Thruster.fault =  (int16_t)_buffer[0]+( (int16_t)_buffer[1]<<8);// + (_buffer[2]<<16)+(_buffer[3]<<24);
					Thruster.Imax = (int16_t)_buffer[2]+( (int16_t)_buffer[3]<<8);
					Thruster.speedref = (int16_t)( _buffer[4] + (_buffer[5]<<8));
					
				}
				break;
			case 0x0A:
				{
					Thruster.torqueIQ = (int16_t)(_buffer[0]+(_buffer[1]<<8)) ;
					Thruster.temperature = (int16_t)( _buffer[2] + (_buffer[3]<<8));
					Thruster.motortemperature = (int16_t)( _buffer[4] + (_buffer[5]<<8));
					
					
					
					
				}
				break;
			default:
				break;
		}

}

void Haoye(uint8_t _buffer[7],HVmotor &HVmotor){

	uint16_t code = _buffer[1] +( _buffer[0]<<8);
	switch(code){
		case 0x5156://speed
			HVmotor.speed = _buffer[7] +( _buffer[6]<<8);
			break;
		case 0x5143://current
			HVmotor.current = _buffer[7] +( _buffer[6]<<8);// 扩大10倍
			break;
		case 0x5150://voltage
			HVmotor.voltage = _buffer[7] +( _buffer[6]<<8);// 扩大10倍
			break;
		case 0x5154://电机温度 motor temp
			HVmotor.motortemperature = _buffer[7] +( _buffer[6]<<8);// 扩大10倍
			break;
		case 0x4551://驱动器温度 driver temp
			HVmotor.temperature =_buffer[7] +( _buffer[6]<<8);
			break;
		case 0x4546://fault
			HVmotor.fault =_buffer[7] +( _buffer[6]<<8);
		default:
			break;
	}
	


}