/*

Force Feedback Joystick

Joystick model specific code for handling force feedback data.

Copyright 2012 Tero Loimuneva (tloimu [at] gmail [dot] com)

Copyright 2013 Saku Kekkonen

Copyright 2016 Jaka Simonic (telesimke [at] gmail [dot] com)

Permission to use, copy, modify, distribute, and sell this

software and its documentation for any purpose is hereby granted

without fee, provided that the above copyright notice appear in

all copies and that both that the copyright notice and this

permission notice and warranty disclaimer appear in supporting

documentation, and that the name of the author not be used in

advertising or publicity pertaining to distribution of the

software without specific, written prior permission.

The author disclaim all warranties with regard to this

software, including all implied warranties of merchantability

and fitness. In no event shall the author be liable for any

special, indirect or consequential damages or any damages

whatsoever resulting from loss of use, data or profits, whether

in an action of contract, negligence or other tortious action,

arising out of or in connection with the use or performance of

this software.

*/

#include "ffb.h"

#include "ffb_math.h"

// Effect management

volatile uint8_t nextEID = 1; // FFP effect indexes starts from 1

volatile TEffectState gEffectStates[MAX_EFFECTS + 1];

uint8_t GetNextFreeEffect(void);

void StartEffect(uint8_t id);

void StopEffect(uint8_t id);

void StopAllEffects(void);

void FreeEffect(uint8_t id);

void FreeAllEffects(void);

volatile uint8_t devicePaused=0;

//variables for storing previous values

volatile int32_t inertiaT[2] = {0,0};

volatile int16_t oldSpeed[2] = {0,0};

volatile int16_t oldAxisPosition[2] = {0,0};

//ffb state structures

volatile USB_FFBReport_PIDStatus_Input_Data_t pidState = {2,30,0};

volatile USB_FFBReport_PIDBlockLoad_Feature_Data_t pidBlockLoad;

volatile USB_FFBReport_PIDPool_Feature_Data_t pidPoolReport;

volatile USB_FFBReport_DeviceGain_Output_Data_t deviceGain;

uint8_t GetNextFreeEffect(void)

{

if (nextEID == MAX_EFFECTS)

return 0;

uint8_t id = nextEID++;

// Find the next free effect ID for next time

//nextEID=0;

while (gEffectStates[nextEID].state != 0)

{

if (nextEID >= MAX_EFFECTS)

break; // the last spot was taken

nextEID++;

}

gEffectStates[id].state = MEFFECTSTATE_ALLOCATED;

return id;

}

void StopAllEffects(void)

{

for (uint8_t id = 0; id <= MAX_EFFECTS; id++)

StopEffect(id);

}

void StartEffect(uint8_t id)

{

if (id > MAX_EFFECTS)

return;

gEffectStates[id].state |= MEFFECTSTATE_PLAYING;

gEffectStates[id].elapsedTime = 0;

}

void StopEffect(uint8_t id)

{

if (id > MAX_EFFECTS)

return;

gEffectStates[id].state &= ~MEFFECTSTATE_PLAYING;

pidBlockLoad.ramPoolAvailable += SIZE_EFFECT;

}

void FreeEffect(uint8_t id)

{

if (id > MAX_EFFECTS)

return;

gEffectStates[id].state = 0;

if (id < nextEID)

nextEID = id;

}

void FreeAllEffects(void)

{

nextEID = 1;

memset((void*)&gEffectStates, 0, sizeof(gEffectStates));

pidBlockLoad.ramPoolAvailable = MEMORY_SIZE;

}

void FfbHandle_EffectOperation(USB_FFBReport_EffectOperation_Output_Data_t* data);

void FfbHandle_BlockFree(USB_FFBReport_BlockFree_Output_Data_t* data);

void FfbHandle_DeviceControl(USB_FFBReport_DeviceControl_Output_Data_t* data);

void FfbHandle_DeviceGain(USB_FFBReport_DeviceGain_Output_Data_t* data);

void FfbHandle_SetCustomForceData(USB_FFBReport_SetCustomForceData_Output_Data_t* data);

void FfbHandle_SetDownloadForceSample(USB_FFBReport_SetDownloadForceSample_Output_Data_t* data);

void FfbHandle_SetCustomForce(USB_FFBReport_SetCustomForce_Output_Data_t* data);

void FfbHandle_SetEffect(USB_FFBReport_SetEffect_Output_Data_t* data);

void SetEnvelope(USB_FFBReport_SetEnvelope_Output_Data_t* data, volatile TEffectState* effect);

void SetCondition(USB_FFBReport_SetCondition_Output_Data_t* data, volatile TEffectState* effect);

void SetPeriodic(USB_FFBReport_SetPeriodic_Output_Data_t* data, volatile TEffectState* effect);

void SetConstantForce(USB_FFBReport_SetConstantForce_Output_Data_t* data, volatile TEffectState* effect);

void SetRampForce(USB_FFBReport_SetRampForce_Output_Data_t* data, volatile TEffectState* effect);

// Handle incoming data from USB

void FfbOnUsbData(uint8_t* data, uint16_t len)

{

uint8_t effectId = data[1]; // effectBlockIndex is always the second byte.

switch (data[0]) // reportID

{

case 1:

FfbHandle_SetEffect((USB_FFBReport_SetEffect_Output_Data_t*)data);

break;

case 2:

SetEnvelope((USB_FFBReport_SetEnvelope_Output_Data_t*)data, &gEffectStates[effectId]);

break;

case 3:

SetCondition((USB_FFBReport_SetCondition_Output_Data_t*)data, &gEffectStates[effectId]);

break;

case 4:

SetPeriodic((USB_FFBReport_SetPeriodic_Output_Data_t*)data, &gEffectStates[effectId]);

break;

case 5:

SetConstantForce((USB_FFBReport_SetConstantForce_Output_Data_t*)data, &gEffectStates[effectId]);

break;

case 6:

SetRampForce((USB_FFBReport_SetRampForce_Output_Data_t*)data, &gEffectStates[effectId]);

break;

case 7:

FfbHandle_SetCustomForceData((USB_FFBReport_SetCustomForceData_Output_Data_t*)data);

break;

case 8:

FfbHandle_SetDownloadForceSample((USB_FFBReport_SetDownloadForceSample_Output_Data_t*)data);

break;

case 9:

break;

case 10:

FfbHandle_EffectOperation((USB_FFBReport_EffectOperation_Output_Data_t*)data);

break;

case 11:

FfbHandle_BlockFree((USB_FFBReport_BlockFree_Output_Data_t*)data);

break;

case 12:

FfbHandle_DeviceControl((USB_FFBReport_DeviceControl_Output_Data_t*)data);

break;

case 13:

FfbHandle_DeviceGain((USB_FFBReport_DeviceGain_Output_Data_t*)data);

break;

case 14:

FfbHandle_SetCustomForce((USB_FFBReport_SetCustomForce_Output_Data_t*)data);

break;

default:

break;

};

}

void SetRampForce(USB_FFBReport_SetRampForce_Output_Data_t* data, volatile TEffectState* effect)

{

effect->start = data->start;

effect->end = data->end;

}

void SetConstantForce(USB_FFBReport_SetConstantForce_Output_Data_t* data, volatile TEffectState* effect)

{

effect->magnitude = data->magnitude;

}

void SetPeriodic(USB_FFBReport_SetPeriodic_Output_Data_t* data, volatile TEffectState* effect)

{

effect->magnitude = data->magnitude;

effect->offset = data->offset;

effect->phase = data->phase;

effect->period = data->period;

}

void SetCondition(USB_FFBReport_SetCondition_Output_Data_t* data, volatile TEffectState* effect)

{

effect->cpOffset[data->parameterBlockOffset] = data->cpOffset;

effect->positiveCoefficient[data->parameterBlockOffset] = data->positiveCoefficient;

effect->negativeCoefficient[data->parameterBlockOffset] = data->negativeCoefficient;

effect->positiveSaturation[data->parameterBlockOffset] = data->positiveSaturation;

effect->negativeSaturation[data->parameterBlockOffset] = data->negativeSaturation;

effect->deadBand[data->parameterBlockOffset] = data->deadBand;

effect->axesIdx = (data->parameterBlockOffset + 1 > effect->axesIdx) ? (data->parameterBlockOffset + 1) : (effect->axesIdx);

}

void SetEnvelope(USB_FFBReport_SetEnvelope_Output_Data_t* data, volatile TEffectState* effect)

{

effect->attackLevel = data->attackLevel;

effect->fadeLevel = data->fadeLevel;

effect->attackTime = data->attackTime;

effect->fadeTime = data->fadeTime;

}

void FfbOnCreateNewEffect(USB_FFBReport_CreateNewEffect_Feature_Data_t* inData)

{

pidBlockLoad.reportId = 6;

pidBlockLoad.effectBlockIndex = GetNextFreeEffect();

if (pidBlockLoad.effectBlockIndex == 0)

{

pidBlockLoad.loadStatus = 2; // 1=Success,2=Full,3=Error

}

else

{

pidBlockLoad.loadStatus = 1; // 1=Success,2=Full,3=Error

volatile TEffectState* effect = &gEffectStates[pidBlockLoad.effectBlockIndex];

memset((void*)effect, 0, sizeof(TEffectState));

effect->state = MEFFECTSTATE_ALLOCATED;

pidBlockLoad.ramPoolAvailable -= SIZE_EFFECT;

}

}

void FfbHandle_SetEffect(USB_FFBReport_SetEffect_Output_Data_t* data)

{

volatile TEffectState* effect = &gEffectStates[data->effectBlockIndex];

effect->duration = data->duration;

effect->directionX = data->directionX;

effect->directionY = data->directionY;

effect->effectType = data->effectType;

effect->gain = data->gain;

effect->enableAxis = data->enableAxis;

// effect->triggerRepeatInterval;

// effect->samplePeriod; // 0..32767 ms

// effect->triggerButton;

}

uint8_t* FfbOnPIDPool()

{

FreeAllEffects();

pidPoolReport.reportId = 7;

pidPoolReport.ramPoolSize = MEMORY_SIZE;

pidPoolReport.maxSimultaneousEffects = MAX_EFFECTS;

pidPoolReport.memoryManagement = 3;

return (uint8_t*)&pidPoolReport;

}

uint8_t* FfbOnPIDBlockLoad()

{

return (uint8_t*)&pidBlockLoad;

}

uint8_t* FfbOnPIDStatus()

{

return (uint8_t*)&pidState;

}

void FfbHandle_SetCustomForceData(USB_FFBReport_SetCustomForceData_Output_Data_t* data)

{

}

void FfbHandle_SetDownloadForceSample(USB_FFBReport_SetDownloadForceSample_Output_Data_t* data)

{

}

void FfbHandle_EffectOperation(USB_FFBReport_EffectOperation_Output_Data_t* data)

{

if (data->operation == 1)

{ // Start

if(data->loopCount > 0) gEffectStates[data->effectBlockIndex].duration *= data->loopCount;

if(data->loopCount==0xFF) gEffectStates[data->effectBlockIndex].duration = USB_DURATION_INFINITE;

StartEffect(data->effectBlockIndex);

}

else if (data->operation == 2)

{ // StartSolo

// Stop all first

StopAllEffects();

// Then start the given effect

StartEffect(data->effectBlockIndex);

}

else if (data->operation == 3)

{ // Stop

StopEffect(data->effectBlockIndex);

}

else

{

}

}

void FfbHandle_BlockFree(USB_FFBReport_BlockFree_Output_Data_t* data)

{

uint8_t eid = data->effectBlockIndex;

if (eid == 0xFF)

{ // all effects

FreeAllEffects();

}

else

{

FreeEffect(eid);

}

}

void FfbHandle_DeviceControl(USB_FFBReport_DeviceControl_Output_Data_t* data)

{

uint8_t control = data->control;

if (control == 0x01)

{ // 1=Enable Actuators

pidState.status |= 2;

}

else if (control == 0x02)

{// 2=Disable Actuators

pidState.status &= ~(0x02);

}

else if (control == 0x03)

{ // 3=Stop All Effects

StopAllEffects();

}

else if (control == 0x04)

{ // 4=Reset

FreeAllEffects();

}

else if (control == 0x05)

{ // 5=Pause

devicePaused = 1;

}

else if (control == 0x06)

{ // 6=Continue

devicePaused = 0;

}

else if (control & (0xFF - 0x3F))

{

}

}

void FfbHandle_DeviceGain(USB_FFBReport_DeviceGain_Output_Data_t* data)

{

deviceGain.gain = data->gain;

}

void FfbHandle_SetCustomForce(USB_FFBReport_SetCustomForce_Output_Data_t* data)

{

}

void FfbGetFeedbackValue(int16_t* axisPosition, int16_t* out)

{

int32_t x_y_force[2] = {0,0};

for (uint8_t id = 0; id <= MAX_EFFECTS; id++)

{

TEffectState effect = gEffectStates[id];

if ((effect.state & MEFFECTSTATE_PLAYING) && ((effect.elapsedTime <= effect.duration)||(effect.duration == USB_DURATION_INFINITE)) && !devicePaused)

{

switch (effect.effectType)

{

case USB_EFFECT_CONSTANT:

{

int32_t tforce = ApplyEnvelope(effect, (int32_t)effect.magnitude);

ApplyDirection(effect, tforce, x_y_force);

}

break;

case USB_EFFECT_RAMP:

{

int32_t end = effect.end * 2;

int32_t start = effect.start * 2;

uint32_t elapsedTime = effect.elapsedTime;

int32_t duration = effect.duration;

int32_t temp = (end - start) * elapsedTime;

temp /= duration;

temp += start;

ApplyDirection(effect, temp, x_y_force);

}

break;

case USB_EFFECT_SQUARE:

{

int32_t offset = effect.offset * 2;

uint32_t magnitude = effect.magnitude;

uint32_t elapsedTime = effect.elapsedTime;

uint32_t phase = effect.phase;

uint32_t period = effect.period;

int32_t max = offset + magnitude;

int32_t min = offset - magnitude;

uint32_t phasetime = (phase * period) / 255;

uint32_t time = elapsedTime + phasetime;

uint32_t reminder = time % period;

int32_t tempforce;

if (reminder > (period / 2)) tempforce = min;

else tempforce = max;

ApplyDirection(effect, ApplyEnvelope(effect, tempforce), x_y_force);

}

break;

case USB_EFFECT_SINE:

{

float offset = effect.offset * 2;

float magnitude = effect.magnitude;

float phase = effect.phase;

float time = effect.elapsedTime;

float period = effect.period;

float angle = ((time / period) + (phase / 255) * period) * 2 * PI;

float sine = FfbSin(angle);

float tempforce = sine * magnitude;

tempForce += offset;

ApplyDirection(effect, ApplyEnvelope(effect, (int32_t)tempforce), x_y_force);

}

break;

case USB_EFFECT_TRIANGLE:

{

float offset = effect.offset*2;

float magnitude = effect.magnitude;

float elapsedTime=effect.elapsedTime;

uint32_t phase=effect.phase;

uint32_t period = effect.period;

float periodF = effect.period;

float max = offset + magnitude;

float min = offset - magnitude;

uint32_t phasetime = (phase * period) / 255;

uint32_t time = elapsedTime + phasetime;

float reminder = time % period;

float slope = ((max - min)*2) / periodF;

float tempforce = 0;

if (reminder > (periodF / 2)) tempforce = slope * (periodF - reminder);

else tempforce = slope * reminder;

tempforce += min;

ApplyDirection(effect, ApplyEnvelope(effect, tempforce), x_y_force);

}

break;

case USB_EFFECT_SAWTOOTHDOWN:

{

float offset = effect.offset*2;

float magnitude = effect.magnitude;

float elapsedTime=effect.elapsedTime;

float phase=effect.phase;

uint32_t period = effect.period;

float periodF = effect.period;

float max = offset + magnitude;

float min = offset - magnitude;

int32_t phasetime = (phase * period) / 255;

uint32_t time = elapsedTime + phasetime;

float reminder = time % period;

float slope = (max - min) / periodF;

float tempforce = 0;

tempforce = slope * (period - reminder);

tempforce += min;

ApplyDirection(effect, ApplyEnvelope(effect, tempforce), x_y_force);

}

break;

case USB_EFFECT_SAWTOOTHUP:

{

float offset = effect.offset*2;

float magnitude = effect.magnitude;

float elapsedTime=effect.elapsedTime;

uint32_t phase=effect.phase;

uint32_t period = effect.period;

float periodF = effect.period;

float max = offset + magnitude;

float min = offset - magnitude;

int32_t phasetime = (phase * period) / 255;

uint32_t time = elapsedTime + phasetime;

float reminder = time % period;

float slope = (max - min) / periodF;

float tempforce = 0;

tempforce = slope * reminder;

tempforce += min;

ApplyDirection(effect, ApplyEnvelope(effect, tempforce), x_y_force);

}

break;

case USB_EFFECT_SPRING:

{

int32_t axis[2] = {NORMALIZE_RANGE(axisPosition[0]),NORMALIZE_RANGE(axisPosition[1])};

int32_t temp[2]={0,0};

CalcCondition(effect,temp,axis);

x_y_force[0] += -temp[0];

x_y_force[1] += -temp[1];

}

break;

case USB_EFFECT_DAMPER:

{

int32_t speed[2] = {NORMALIZE_RANGE(axisPosition[0]) - NORMALIZE_RANGE(oldAxisPosition[0]), NORMALIZE_RANGE(axisPosition[1]) - NORMALIZE_RANGE(oldAxisPosition[1])};

int32_t temp[2]={0,0};

CalcCondition(effect,temp,speed);

x_y_force[0] += -temp[0];

x_y_force[1] += -temp[1];

}

break;

case USB_EFFECT_INERTIA:

{

int32_t speed[2] = {NORMALIZE_RANGE(axisPosition[0]) - NORMALIZE_RANGE(oldAxisPosition[0]), NORMALIZE_RANGE(axisPosition[1]) - NORMALIZE_RANGE(oldAxisPosition[1])};

int32_t acceleration[2] = {speed[0] - oldSpeed[0], speed[1] - oldSpeed[1]};

int32_t temp[2] = {0, 0};

memset((void*)&effect.positiveSaturation, INERTIA_FORCE, 4);

memset((void*)&effect.deadBand, INERTIA_DEADBAND, 2);

CalcCondition(effect,temp,speed);

inertiaT[0] += temp[0];

inertiaT[1] += temp[1];

x_y_force[0] += inertiaT[0];

x_y_force[1] += inertiaT[1];

if (speed[0] == 0) inertiaT[0] = 0;

if (speed[1] == 0) inertiaT[1] = 0;

}

break;

case USB_EFFECT_FRICTION:

{

int32_t speed[2] = {NORMALIZE_RANGE(axisPosition[0]) - NORMALIZE_RANGE(oldAxisPosition[0]), NORMALIZE_RANGE(axisPosition[1]) - NORMALIZE_RANGE(oldAxisPosition[1])};

int32_t temp[2] = {0, 0};

memset((void*)&effect.positiveSaturation, FRICTION_FORCE, 4);

memset((void*)&effect.deadBand, FRICTION_DEADBAND, 2);

CalcCondition(effect,temp,speed);

x_y_force[0] += -temp[0];

x_y_force[1] += -temp[1];

}

break;

case USB_EFFECT_CUSTOM:

break;

}

gEffectStates[id].elapsedTime += WHEEL_SAMPLE_RATE_MS;

}

}

uint32_t gain = deviceGain.gain;

for(uint32_t i = 0;i < 2; i++)

{

x_y_force[i] *= gain;

x_y_force[i] /= 255;

out[i] = x_y_force[i] > 255 ? 255 : x_y_force[i];

out[i] = x_y_force[i] < -255 ? -255 : x_y_force[i];

}

oldSpeed[0] = axisPosition[0] - oldAxisPosition[0];

oldSpeed[1] = axisPosition[1] - oldAxisPosition[1];

}