#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));
					
				}
				break;
			case 0x05:
				{
					Thruster.fault = _buffer[0]+(_buffer[1]<<8) + (_buffer[2]<<16)+(_buffer[3]<<24);
					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;
		}

}