/*
 * user.c
 *
 *  Created on: 2 июн. 2023 г.
 *    Author: Andrey Koryagin https://blog.avislab.com/
 */

#include "u_foc.h"
#include "position.h"

extern volatile SettingsStruct Settings;

void USER_Init(void) {
	MC_ProtocolTimeoutDisable(); // Disable motor stopping on U-FOC Protocol timeout
	Position_Init();
}

void USER_While(void) {
	HAL_Delay(10);

	if (MC_GetState() == MC_STATE_IDLE) {
		MC_Start();
	}

	if (MC_GetState() == MC_STATE_FAULT) {
		MC_ResetFault();
	}
}

void USER_Tasks(void) {
	Position_Process();
}






#ifdef USER_PROTOCOL_ENABLE

#define USER_PACKET_SET_POSITION	60
#define USER_PACKET_GET_POSITION	61

extern uint8_t PACKET_REQUEST[PACKET_LENGTH];
extern uint8_t PACKET_RESPONSE[PACKET_LENGTH];

uint8_t USER_Packet_Handling(void) {

	if (PACKET_REQUEST[1] == USER_PACKET_SET_POSITION) { // Set Position
		int16_t position;
		position = (PACKET_REQUEST[2]<<8) | PACKET_REQUEST[3];
		Position_Set(position);
		return 0;
	}

	if (PACKET_REQUEST[1] == USER_PACKET_GET_POSITION) { // Get Position
		PACKET_RESPONSE[0]= Settings.DevID;
		PACKET_RESPONSE[1]= PACKET_REQUEST[1] + 127;

		uint16_t position;
		position = Position_Get_Real();
		PACKET_RESPONSE[2]= ((position & 0x0000FF00) >> 8);
		PACKET_RESPONSE[3]= (position & 0x000000FF);
		position = Position_Get();
		PACKET_RESPONSE[4]= ((position & 0x0000FF00) >> 8);
		PACKET_RESPONSE[5]= (position & 0x000000FF);
		PACKET_RESPONSE[6]= 0;
		PACKET_RESPONSE[7]= 0;
		return 1;
	}

	return 0;
}

#endif