s32 MIXER_GetTrimValue(int i)
{
s32 value = *(MIXER_GetTrim(i));
//0 to 100 step is 0.1
if (Model.trims[i].step < 10)
return PCT_TO_RANGE(value * Model.trims[i].step) / 10;
else
return PCT_TO_RANGE(value);
}
s16 eval_mixer_cb(s16 xval, void * data)
{
struct Mixer *mix = data ? (struct Mixer *)data : mp->cur_mixer;
if (MIXER_SRC_IS_INV(mix->src))
xval = -xval;
s16 yval = CURVE_Evaluate(xval, &mix->curve);
yval = yval * mix->scalar / 100 + PCT_TO_RANGE(mix->offset);
/* Min/Max is a servo limit, shouldn't be shown here
if(mix->dest < NUM_OUT_CHANNELS) {
if (yval > PCT_TO_RANGE(mp->limit.max))
yval = PCT_TO_RANGE(mp->limit.max);
else if (yval < PCT_TO_RANGE(mp->limit.min))
yval = PCT_TO_RANGE(mp->limit.min);
}
*/
if (yval > CHAN_MAX_VALUE * 5 / 4)
yval = CHAN_MAX_VALUE * 5 / 4;
else if (yval <CHAN_MIN_VALUE * 5 / 4)
yval = CHAN_MIN_VALUE * 5 / 4;
//Don't showchannel-reverse on the graph (but do show input reverse)
//if (mp->limit.flags & CH_REVERSE)
// yval = -yval;
return yval;
}
s16 STDMIX_EvalMixerCb(s16 xval, struct Mixer *mix, s16 max_value, s16 min_value)
{
if (MIXER_SRC_IS_INV(mix->src))
xval = -xval;
s16 yval = CURVE_Evaluate(xval, &mix->curve);
yval = yval * mix->scalar / 100 + PCT_TO_RANGE(mix->offset);
if (yval > max_value)
yval = max_value;
else if (yval <min_value)
yval = min_value;
return yval;
}
s32 MIXER_ApplyLimits(unsigned channel, struct Limit *limit, volatile s32 *_raw,
volatile s32 *_Channels, enum LimitMask flags)
{
int applied_safety = 0;
s32 value = _raw[NUM_INPUTS + 1 + channel] + get_trim(NUM_INPUTS + 1 + channel);
if (channel >= NUM_OUT_CHANNELS)
return value;
if ((flags & APPLY_SAFETY) && MIXER_SRC(limit->safetysw) && switch_is_on(limit->safetysw, _raw)) {
applied_safety = 1;
value = PCT_TO_RANGE(Model.limits[channel].safetyval);
}
if (flags & APPLY_SCALAR) {
if (value >= 0 || limit->servoscale_neg == 0)
value = (s32)value * limit->servoscale / 100;
else
value = (s32)value * limit->servoscale_neg / 100;
}
if ((flags & APPLY_REVERSE) && (limit->flags & CH_REVERSE)) {
value = -value;
}
if (flags & APPLY_SUBTRIM)
value += PCT_TO_RANGE(limit->subtrim) / 10;
if (! applied_safety) {
//degrees / 100msec
if (_Channels && (flags & APPLY_SPEED) && limit->speed) {
s32 rate = CHAN_MAX_VALUE * limit->speed / 60 * MEDIUM_PRIORITY_MSEC / 100;
if (value - _Channels[channel] > rate)
value = _Channels[channel] + rate;
else if(value - _Channels[channel] < -rate)
value = _Channels[channel] - rate;
}
}
if (flags & APPLY_LIMITS) {
if (value > PCT_TO_RANGE(limit->max))
value = PCT_TO_RANGE(limit->max);
else if( value < PCT_TO_RANGE(-(int)limit->min))
value = PCT_TO_RANGE(-(int)limit->min);
} else {
if (value > INT16_MAX)
value = INT16_MAX;
else if (value < INT16_MIN)
value = INT16_MIN;
}
return value;
}
void MIXER_ApplyMixer(struct Mixer *mixer, volatile s32 *raw, s32 *orig_value)
{
s32 value;
if (! MIXER_SRC(mixer->src))
return;
if (! switch_is_on(mixer->sw, raw)) {
// Switch is off, so this mixer is not active
return;
}
//1st: Get source value with trim
value = raw[MIXER_SRC(mixer->src)];
//Invert if necessary
if (MIXER_SRC_IS_INV(mixer->src))
value = - value;
//2nd: apply curve
value = CURVE_Evaluate(value, &mixer->curve);
//3rd: apply scalar and offset
value = value * mixer->scalar / 100 + PCT_TO_RANGE(mixer->offset);
//4th: multiplex result
switch(MIXER_MUX(mixer)) {
case MUX_REPLACE:
break;
case MUX_MULTIPLY:
value = raw[mixer->dest + NUM_INPUTS + 1] * value / CHAN_MAX_VALUE;
break;
case MUX_ADD:
value = raw[mixer->dest + NUM_INPUTS + 1] + value;
break;
case MUX_MAX:
value = raw[mixer->dest + NUM_INPUTS + 1] > value
? raw[mixer->dest + NUM_INPUTS + 1]
: value;
break;
case MUX_MIN:
value = raw[mixer->dest + NUM_INPUTS + 1] < value
? raw[mixer->dest + NUM_INPUTS + 1]
: value;
break;
case MUX_DELAY:
{
//value initially represents 20ths of seconds to cover 60-degrees
//convert value to represent #msecs to cover 60-degrees (zero->full)
if (value == 0 || orig_value == NULL) {
value = raw[mixer->dest + NUM_INPUTS + 1];
break;
}
value = abs(RANGE_TO_PCT(value)) * 50;
//rate represents the maximum travel per iteration (once per MEDIUM_PRIORITY_MSEC)
s32 rate = CHAN_MAX_VALUE * MEDIUM_PRIORITY_MSEC / value;
value = raw[mixer->dest + NUM_INPUTS + 1];
if (value - *orig_value > rate)
value = *orig_value + rate;
else if(value - *orig_value < -rate)
value = *orig_value - rate;
}
case MUX_LAST: break;
}
//5th: apply trim
if (MIXER_APPLY_TRIM(mixer))
value = value + (MIXER_SRC_IS_INV(mixer->src) ? -1 : 1) * get_trim(MIXER_SRC(mixer->src));
//Ensure we don't overflow
if (value > INT16_MAX)
value = INT16_MAX;
else if (value < INT16_MIN)
value = INT16_MIN;
raw[mixer->dest + NUM_INPUTS + 1] = value;
}
s32 trim_cb(void * data)
{
long i = (long)data;
int value = *MIXER_GetTrim(i);
return PCT_TO_RANGE(value);
}
