// Get superellipsoid value in the given point.
float PrimitiveSuperellipsoid::evaluateSuperellipsoid( const TPoint3 &P ) const {
float x = powerf( fabsf(P[__X]), power[__X] );
float y = powerf( fabsf(P[__Y]), power[__X] );
float z = powerf( fabsf(P[__Z]), power[__Z] );
return powerf( x + y, power[__Y]) + z - 1.0f;
}
void powerf(long long F[2][2],long long n) // RECURSIVE FUNCTION FOR MATRIX
{
if(n==0 || n==1)
return;
long long M[2][2]={0,1,1,1};
powerf(F,n/2);
mul(F,F);
if(n%2!=0)
mul(F,M);
}
long long fib(long long n) //FUNCTION CALLED FROM main
{
long long F[2][2]={0,1,1,1};
long long result;
if(n==0)
return 0;
else if(n==1 || n==2)
return 1;
powerf(F,(n-1));
return (F[0][0]+F[0][1]);
}
TVector3 PrimitiveSuperellipsoid::normalAt( const TIntersection &inter ) const {
float x = fabsf( inter.pointWorldSpace[__X] );
float y = fabsf( inter.pointWorldSpace[__Y] );
float z = fabsf( inter.pointWorldSpace[__Z] );
float k = powerf( powerf( x, power[__X] ) + powerf( y, power[__X] ), power[__Y] - 1.0f );
TVector3 N;
N[__X] = k * SGNX(inter.pointWorldSpace[__X]) * powerf( x, power[__X] - 1.0f );
N[__Y] = k * SGNX(inter.pointWorldSpace[__Y]) * powerf( y, power[__X] - 1.0f ); // FIXME ?
N[__Z] = SGNX(inter.pointWorldSpace[__Z]) * powerf( z, power[__Z] - 1.0f );
N.normalize();
return N;
}
void calculateSetpointRate(int axis, int16_t rc) {
float angleRate, rcRate, rcSuperfactor, rcCommandf;
uint8_t rcExpo;
if (axis != YAW) {
rcExpo = currentControlRateProfile->rcExpo8;
rcRate = currentControlRateProfile->rcRate8 / 100.0f;
} else {
rcExpo = currentControlRateProfile->rcYawExpo8;
rcRate = currentControlRateProfile->rcYawRate8 / 100.0f;
}
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
rcCommandf = rc / 500.0f;
rcInput[axis] = ABS(rcCommandf);
if (rcExpo) {
float expof = rcExpo / 100.0f;
rcCommandf = rcCommandf * powerf(rcInput[axis], RC_EXPO_POWER) * expof + rcCommandf * (1-expof);
}
angleRate = 200.0f * rcRate * rcCommandf;
if (currentControlRateProfile->rates[axis]) {
rcSuperfactor = 1.0f / (constrainf(1.0f - (ABS(rcCommandf) * (currentControlRateProfile->rates[axis] / 100.0f)), 0.01f, 1.00f));
angleRate *= rcSuperfactor;
}
if (debugMode == DEBUG_ANGLERATE) {
debug[axis] = angleRate;
}
if (currentProfile->pidProfile.pidController == PID_CONTROLLER_LEGACY)
setpointRate[axis] = constrainf(angleRate * 4.1f, -8190.0f, 8190.0f); // Rate limit protection
else
setpointRate[axis] = constrainf(angleRate, -1998.0f, 1998.0f); // Rate limit protection (deg/sec)
}
static void qam_report(void *user_data, const complexf_t *constel, const complexf_t *target, int symbol)
#endif
{
int i;
int len;
#if defined(SPANDSP_USE_FIXED_POINT)
complexi16_t *coeffs;
complexf_t constel_point;
#else
complexf_t *coeffs;
#endif
float fpower;
v17_rx_state_t *rx;
static float smooth_power = 0.0f;
static int update_interval = 100;
rx = (v17_rx_state_t *) user_data;
if (constel)
{
fpower = (constel->re - target->re)*(constel->re - target->re)
+ (constel->im - target->im)*(constel->im - target->im);
#if defined(SPANDSP_USE_FIXED_POINT)
fpower /= 4096.0*4096.0;
#endif
smooth_power = 0.95f*smooth_power + 0.05f*fpower;
#if defined(ENABLE_GUI)
if (use_gui)
{
#if defined(SPANDSP_USE_FIXED_POINT)
constel_point.re = constel->re/4096.0;
constel_point.im = constel->im/4096.0;
qam_monitor_update_constel(qam_monitor, &constel_point);
#else
qam_monitor_update_constel(qam_monitor, constel);
#endif
qam_monitor_update_carrier_tracking(qam_monitor, v17_rx_carrier_frequency(rx));
qam_monitor_update_symbol_tracking(qam_monitor, v17_rx_symbol_timing_correction(rx));
}
#endif
printf("%8d [%8.4f, %8.4f] [%8.4f, %8.4f] %2x %8.4f %8.4f %9.4f %7.3f %7.4f\n",
symbol_no,
#if defined(SPANDSP_USE_FIXED_POINT)
constel->re/4096.0,
constel->im/4096.0,
target->re/4096.0,
target->im/4096.0,
#else
constel->re,
constel->im,
target->re,
target->im,
#endif
symbol,
fpower,
smooth_power,
v17_rx_carrier_frequency(rx),
v17_rx_signal_power(rx),
v17_rx_symbol_timing_correction(rx));
//printf("Carrier %d %f %f\n", symbol_no, v17_rx_carrier_frequency(rx), v17_rx_symbol_timing_correction(rx));
symbol_no++;
if (--update_interval <= 0)
{
len = v17_rx_equalizer_state(rx, &coeffs);
printf("Equalizer A:\n");
for (i = 0; i < len; i++)
#if defined(SPANDSP_USE_FIXED_POINT)
printf("%3d (%15.5f, %15.5f)\n", i, coeffs[i].re/4096.0f, coeffs[i].im/4096.0f);
#else
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#endif
#if defined(ENABLE_GUI)
if (use_gui)
{
#if defined(SPANDSP_USE_FIXED_POINT)
qam_monitor_update_int_equalizer(qam_monitor, coeffs, len);
#else
qam_monitor_update_equalizer(qam_monitor, coeffs, len);
#endif
}
#endif
update_interval = 100;
}
}
}
static void v17_rx_status(void *user_data, int status)
{
v17_rx_state_t *s;
int i;
int len;
#if defined(SPANDSP_USE_FIXED_POINT)
complexi16_t *coeffs;
#else
complexf_t *coeffs;
#endif
printf("V.17 rx status is %s (%d)\n", signal_status_to_str(status), status);
s = (v17_rx_state_t *) user_data;
switch (status)
{
case SIG_STATUS_TRAINING_SUCCEEDED:
len = v17_rx_equalizer_state(s, &coeffs);
printf("Equalizer:\n");
for (i = 0; i < len; i++)
#if defined(SPANDSP_USE_FIXED_POINT)
printf("%3d (%15.5f, %15.5f)\n", i, coeffs[i].re/4096.0f, coeffs[i].im/4096.0f);
#else
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#endif
break;
}
}
static void qam_report(void *user_data, const complexf_t *constel, const complexf_t *target, int symbol)
#endif
{
int i;
int len;
#if defined(SPANDSP_USE_FIXED_POINT)
complexi16_t *coeffs;
#else
complexf_t *coeffs;
#endif
complexf_t constel_point;
complexf_t target_point;
float fpower;
v29_rx_state_t *rx;
static float smooth_power = 0.0f;
static int update_interval = 100;
rx = (v29_rx_state_t *) user_data;
if (constel)
{
constel_point.re = constel->re/V29_CONSTELLATION_SCALING_FACTOR;
constel_point.im = constel->im/V29_CONSTELLATION_SCALING_FACTOR;
target_point.re = target->re/V29_CONSTELLATION_SCALING_FACTOR,
target_point.im = target->im/V29_CONSTELLATION_SCALING_FACTOR,
fpower = (constel_point.re - target_point.re)*(constel_point.re - target_point.re)
+ (constel_point.im - target_point.im)*(constel_point.im - target_point.im);
smooth_power = 0.95f*smooth_power + 0.05f*fpower;
#if defined(ENABLE_GUI)
if (use_gui)
{
qam_monitor_update_constel(qam_monitor, &constel_point);
qam_monitor_update_carrier_tracking(qam_monitor, v29_rx_carrier_frequency(rx));
//qam_monitor_update_carrier_tracking(qam_monitor, (fpower) ? fpower : 0.001f);
qam_monitor_update_symbol_tracking(qam_monitor, v29_rx_symbol_timing_correction(rx));
}
#endif
printf("%8d [%8.4f, %8.4f] [%8.4f, %8.4f] %2x %8.4f %8.4f %9.4f %7.3f %7.4f\n",
symbol_no,
constel_point.re,
constel_point.im,
target_point.re,
target_point.im,
symbol,
fpower,
smooth_power,
v29_rx_carrier_frequency(rx),
v29_rx_signal_power(rx),
v29_rx_symbol_timing_correction(rx));
symbol_no++;
if (--update_interval <= 0)
{
if ((len = v29_rx_equalizer_state(rx, &coeffs)))
{
printf("Equalizer A:\n");
for (i = 0; i < len; i++)
#if defined(SPANDSP_USE_FIXED_POINT)
printf("%3d (%15.5f, %15.5f)\n", i, coeffs[i].re/V29_CONSTELLATION_SCALING_FACTOR, coeffs[i].im/V29_CONSTELLATION_SCALING_FACTOR);
#else
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#endif
#if defined(ENABLE_GUI)
if (use_gui)
{
#if defined(SPANDSP_USE_FIXED_POINT)
qam_monitor_update_int_equalizer(qam_monitor, coeffs, len);
#else
qam_monitor_update_equalizer(qam_monitor, coeffs, len);
#endif
}
#endif
}
update_interval = 100;
}
}
}
static void v29_rx_status(void *user_data, int status)
{
v29_rx_state_t *s;
int i;
int len;
#if defined(SPANDSP_USE_FIXED_POINT)
complexi16_t *coeffs;
#else
complexf_t *coeffs;
#endif
printf("V.29 rx status is %s (%d)\n", signal_status_to_str(status), status);
s = (v29_rx_state_t *) user_data;
switch (status)
{
case SIG_STATUS_TRAINING_SUCCEEDED:
printf("Training succeeded\n");
if ((len = v29_rx_equalizer_state(s, &coeffs)))
{
printf("Equalizer:\n");
for (i = 0; i < len; i++)
#if defined(SPANDSP_USE_FIXED_POINT)
printf("%3d (%15.5f, %15.5f)\n", i, coeffs[i].re/V29_CONSTELLATION_SCALING_FACTOR, coeffs[i].im/V29_CONSTELLATION_SCALING_FACTOR);
#else
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#endif
}
break;
}
}
static void qam_report(void *user_data, const complexf_t *constel, const complexf_t *target, int symbol)
{
int i;
int len;
complexf_t *coeffs;
float fpower;
float error;
v27ter_rx_state_t *rx;
static float smooth_power = 0.0f;
#if defined(ENABLE_GUI)
static int reports = 0;
#endif
rx = (v27ter_rx_state_t *) user_data;
if (constel)
{
fpower = (constel->re - target->re)*(constel->re - target->re)
+ (constel->im - target->im)*(constel->im - target->im);
smooth_power = 0.95f*smooth_power + 0.05f*fpower;
#if defined(ENABLE_GUI)
if (use_gui)
{
qam_monitor_update_constel(qam_monitor, constel);
qam_monitor_update_carrier_tracking(qam_monitor, v27ter_rx_carrier_frequency(rx));
qam_monitor_update_symbol_tracking(qam_monitor, v27ter_rx_symbol_timing_correction(rx));
}
#endif
error = constel->im*target->re - constel->re*target->im;
printf("Tracking error %f %f %f %f %f %f\n", error, v27ter_rx_carrier_frequency(rx), constel->re, constel->im, target->re, target->im);
printf("%8d [%8.4f, %8.4f] [%8.4f, %8.4f] %2x %8.4f %8.4f %9.4f %7.3f %7.2f\n",
symbol_no,
constel->re,
constel->im,
target->re,
target->im,
symbol,
fpower,
smooth_power,
v27ter_rx_carrier_frequency(rx),
v27ter_rx_signal_power(rx),
v27ter_rx_symbol_timing_correction(rx));
len = v27ter_rx_equalizer_state(rx, &coeffs);
printf("Equalizer B:\n");
for (i = 0; i < len; i++)
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
printf("Gardtest %d %f %d\n", symbol_no, v27ter_rx_symbol_timing_correction(rx), rx->gardner_integrate);
printf("Carcar %d %f\n", symbol_no, v27ter_rx_carrier_frequency(rx));
#if defined(ENABLE_GUI)
if (use_gui)
{
if (++reports >= 1000)
{
qam_monitor_update_equalizer(qam_monitor, coeffs, len);
reports = 0;
}
}
#endif
symbol_no++;
}
else
{
printf("Gardner step %d\n", symbol);
len = v27ter_rx_equalizer_state(rx, &coeffs);
printf("Equalizer A:\n");
for (i = 0; i < len; i++)
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#if defined(ENABLE_GUI)
if (use_gui)
qam_monitor_update_equalizer(qam_monitor, coeffs, len);
#endif
}
}
static void qam_report(void *user_data, const complexf_t *constel, const complexf_t *target, int symbol)
#endif
{
int i;
int len;
#if defined(SPANDSP_USE_FIXED_POINT)
complexi16_t *coeffs;
#else
complexf_t *coeffs;
#endif
complexf_t constel_point;
complexf_t target_point;
float fpower;
endpoint_t *s;
s = (endpoint_t *) user_data;
if (constel)
{
constel_point.re = constel->re/V22BIS_CONSTELLATION_SCALING_FACTOR;
constel_point.im = constel->im/V22BIS_CONSTELLATION_SCALING_FACTOR;
target_point.re = target->re/V22BIS_CONSTELLATION_SCALING_FACTOR;
target_point.im = target->im/V22BIS_CONSTELLATION_SCALING_FACTOR;
#if defined(ENABLE_GUI)
if (use_gui)
{
qam_monitor_update_constel(s->qam_monitor, &constel_point);
qam_monitor_update_carrier_tracking(s->qam_monitor, v22bis_rx_carrier_frequency(s->v22bis));
qam_monitor_update_symbol_tracking(s->qam_monitor, v22bis_rx_symbol_timing_correction(s->v22bis));
}
#endif
fpower = (constel->re - target->re)*(constel->re - target->re)
+ (constel->im - target->im)*(constel->im - target->im);
s->smooth_power = 0.95f*s->smooth_power + 0.05f*fpower;
printf("%8d [%8.4f, %8.4f] [%8.4f, %8.4f] %2x %8.4f %8.4f %8.4f\n",
s->symbol_no,
constel_point.re,
constel_point.im,
target_point.re,
target_point.im,
symbol,
fpower,
s->smooth_power,
v22bis_rx_signal_power(s->v22bis));
s->symbol_no++;
}
else
{
printf("Gardner step %d\n", symbol);
if ((len = v22bis_rx_equalizer_state(s->v22bis, &coeffs)))
{
printf("Equalizer A:\n");
for (i = 0; i < len; i++)
#if defined(SPANDSP_USE_FIXED_POINT)
printf("%3d (%15.5f, %15.5f)\n", i, coeffs[i].re/V22BIS_CONSTELLATION_SCALING_FACTOR, coeffs[i].im/V22BIS_CONSTELLATION_SCALING_FACTOR);
#else
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#endif
#if defined(ENABLE_GUI)
if (use_gui)
{
#if defined(SPANDSP_USE_FIXED_POINT)
qam_monitor_update_int_equalizer(s->qam_monitor, coeffs, len);
#else
qam_monitor_update_equalizer(s->qam_monitor, coeffs, len);
#endif
}
#endif
}
}
}
static void v22bis_rx_status(void *user_data, int status)
{
endpoint_t *s;
int bit_rate;
int i;
int len;
#if defined(SPANDSP_USE_FIXED_POINT)
complexi16_t *coeffs;
#else
complexf_t *coeffs;
#endif
/* Special conditions */
s = (endpoint_t *) user_data;
printf("V.22bis rx %p status is %s (%d)\n", user_data, signal_status_to_str(status), status);
switch (status)
{
case SIG_STATUS_TRAINING_SUCCEEDED:
bit_rate = v22bis_get_current_bit_rate(s->v22bis);
printf("Negotiated bit rate: %d\n", bit_rate);
if ((len = v22bis_rx_equalizer_state(s->v22bis, &coeffs)))
{
printf("Equalizer:\n");
for (i = 0; i < len; i++)
#if defined(SPANDSP_USE_FIXED_POINT)
printf("%3d (%15.5f, %15.5f)\n", i, coeffs[i].re/V22BIS_CONSTELLATION_SCALING_FACTOR, coeffs[i].im/V22BIS_CONSTELLATION_SCALING_FACTOR);
#else
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#endif
}
break;
}
}
static void v22bis_putbit(void *user_data, int bit)
{
v22bis_state_t *s;
int i;
int len;
complexf_t *coeffs;
s = (v22bis_state_t *) user_data;
if (bit < 0)
{
/* Special conditions */
switch (bit)
{
case PUTBIT_TRAINING_FAILED:
printf("Training failed\n");
break;
case PUTBIT_TRAINING_IN_PROGRESS:
printf("Training in progress\n");
break;
case PUTBIT_TRAINING_SUCCEEDED:
printf("Training succeeded\n");
len = v22bis_equalizer_state(s, &coeffs);
printf("Equalizer:\n");
for (i = 0; i < len; i++)
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
break;
case PUTBIT_CARRIER_UP:
printf("Carrier up\n");
break;
case PUTBIT_CARRIER_DOWN:
printf("Carrier down\n");
break;
default:
printf("Eh! - %d\n", bit);
break;
}
return;
}
if (bit != tx_buf[rx_ptr])
{
printf("Rx bit %d - %d\n", rx_bits, bit);
rx_bad_bits++;
}
rx_ptr++;
if (rx_ptr > 1000)
rx_ptr = 0;
rx_bits++;
if ((rx_bits % 100000) == 0)
{
printf("%d bits received, %d bad bits\r", rx_bits, rx_bad_bits);
fflush(stdout);
}
}
static void qam_report(void *user_data, const complexf_t *constel, const complexf_t *target, int symbol)
{
int i;
int len;
complexf_t *coeffs;
float fpower;
struct qam_report_control_s *s;
s = (struct qam_report_control_s *) user_data;
if (constel)
{
#if defined(ENABLE_GUI)
if (use_gui)
{
qam_monitor_update_constel(s->qam_monitor, constel);
qam_monitor_update_carrier_tracking(s->qam_monitor, v22bis_rx_carrier_frequency(s->s));
qam_monitor_update_symbol_tracking(s->qam_monitor, v22bis_symbol_timing_correction(s->s));
}
#endif
fpower = (constel->re - target->re)*(constel->re - target->re)
+ (constel->im - target->im)*(constel->im - target->im);
s->smooth_power = 0.95f*s->smooth_power + 0.05f*fpower;
printf("%8d [%8.4f, %8.4f] [%8.4f, %8.4f] %2x %8.4f %8.4f\n",
s->symbol_no,
constel->re,
constel->im,
target->re,
target->im,
symbol,
fpower,
s->smooth_power);
s->symbol_no++;
}
else
{
printf("Gardner step %d\n", symbol);
len = v22bis_equalizer_state(s->s, &coeffs);
printf("Equalizer A:\n");
for (i = 0; i < len; i++)
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#if defined(ENABLE_GUI)
if (use_gui)
qam_monitor_update_equalizer(s->qam_monitor, coeffs, len);
#endif
}
}
static void qam_report(void *user_data, const complexf_t *constel, const complexf_t *target, int symbol)
{
int i;
int len;
complexf_t *coeffs;
float fpower;
v17_rx_state_t *rx;
static float smooth_power = 0.0f;
static int update_interval = 100;
rx = (v17_rx_state_t *) user_data;
if (constel)
{
#if defined(ENABLE_GUI)
if (use_gui)
{
qam_monitor_update_constel(qam_monitor, constel);
qam_monitor_update_carrier_tracking(qam_monitor, v17_rx_carrier_frequency(rx));
qam_monitor_update_symbol_tracking(qam_monitor, v17_rx_symbol_timing_correction(rx));
}
#endif
fpower = (constel->re - target->re)*(constel->re - target->re)
+ (constel->im - target->im)*(constel->im - target->im);
smooth_power = 0.95f*smooth_power + 0.05f*fpower;
printf("%8d [%8.4f, %8.4f] [%8.4f, %8.4f] %2x %8.4f %8.4f %9.4f %7.3f %7.4f\n",
symbol_no,
constel->re,
constel->im,
target->re,
target->im,
symbol,
fpower,
smooth_power,
v17_rx_carrier_frequency(rx),
v17_rx_signal_power(rx),
v17_rx_symbol_timing_correction(rx));
printf("Carrier %d %f %f\n", symbol_no, v17_rx_carrier_frequency(rx), v17_rx_symbol_timing_correction(rx));
symbol_no++;
if (--update_interval <= 0)
{
len = v17_rx_equalizer_state(rx, &coeffs);
printf("Equalizer A:\n");
for (i = 0; i < len; i++)
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
#if defined(ENABLE_GUI)
if (use_gui)
qam_monitor_update_equalizer(qam_monitor, coeffs, len);
#endif
update_interval = 100;
}
}
}
static void v17_rx_status(void *user_data, int status)
{
v17_rx_state_t *rx;
int i;
int len;
complexf_t *coeffs;
printf("V.17 rx status is %s (%d)\n", signal_status_to_str(status), status);
rx = (v17_rx_state_t *) user_data;
switch (status)
{
case SIG_STATUS_TRAINING_SUCCEEDED:
len = v17_rx_equalizer_state(rx, &coeffs);
printf("Equalizer:\n");
for (i = 0; i < len; i++)
printf("%3d (%15.5f, %15.5f) -> %15.5f\n", i, coeffs[i].re, coeffs[i].im, powerf(&coeffs[i]));
break;
}
}