byte Beak::chirp(const char *chirpStr) {
int16_t nFrames = numberOfFrames();
size_t frameStoreSize = sizeof(SynthFrame[nFrames]);
if(enoughSpaceFor(frameStoreSize)) {
SynthFrame *frames = (SynthFrame *) alloca(frameStoreSize);
return chirp(chirpStr, nFrames, frames);
}
else {
return TOO_LITTLE_RAM_WARNING;
}
}
byte Beak::chirp(const char *chirpStr, char *enoughSpaceForFrames) {
return chirp(chirpStr, numberOfFrames(), (SynthFrame *) enoughSpaceForFrames);
}
/*----------------------------------------------------------------------------*/
void rls(void)
{
double yhat = 0.0, er = 0.0, K = 0.0;
double lambda = 0.99; /* forgetting factor, in range of [0.0, 1.0] */
double den = lambda;
int i, j;
int n = (int)NA + (int)NB;
double pf[n]; /* pf = p * phi */
double f1, f2, T, amp;
f1 = 0.1; /* Hz */
f2 = 100.0; /* Hz */
T = 2.0; /* seconds */
amp = 1.0; /* volt, rpm, etc */
printf("RLS: time =%5.3f, est_theta=[", t);
for(i = 0; i < n; i++)
printf("%8.4f", est_theta[i]);
printf("]\n");
/* Excited input signal */
u[0] = chirp(f1, f2, t, T, amp);
/* Process output */
y[0] = -a[0] * y[1] - a[1] * y[2] + b[0] * u[1] + b[1] * u[2];
for(i = 0; i < n; i++)
{
yhat += phi[i] * est_theta[i]; /* compute the estimated value */
pf[i] = 0.0;
for(j = 0; j < n; j++)
pf[i] += p[i][j] * phi[j];
den += phi[i] * pf[i]; /* den = lambda + phi'*p*phi */
}
if ((den * 1000.0) < 0.001)
{
printf("Error: divided by zero, exiting...\n");
exit(1);
}
er = y[0] - yhat; /* error between the process output and the estimated value */
for (i = 0; i < n; i++)
{
/* the represence of the Kalman gain K here is just for easily understanding */
K = pf[i] / den;
/* update estimate */
est_theta[i] += K * er;
/* update covariance matrix */
for(j = 0; j <= i; j++) /* reason: update one part first then transpose */
{
/**
* p is a symmetric matrix so p = p'
* k is a vector, k[n]
* p = p - k*phi'*p = p - k*phi'*p' = p - k*(p*phi)' = p - k* pf'
*/
/* update the lower (left) triangular */
p[i][j] -= K * pf[j];
p[i][j] /= lambda;
/* update the upper (right) triangular */
p[j][i] = p[i][j];
}
}
/* update process */
u[2] = u[1];
u[1] = u[0];
y[2] = y[1];
y[1] = y[0];
/* update regression vector */
phi[1] = phi[0];
phi[0] = -y[0];
phi[3] = phi[2];
phi[2] = u[1];
}
extern void
display_vario( u8 line, u8 update )
{
static u8 _idone; // initialisation helper
static u8 _vbeat; // heartbeat
u32 pressure;
switch( update )
{
case DISPLAY_LINE_CLEAR:
stop_buzzer();
display_symbol( LCD_ICON_BEEPER1, SEG_OFF );
display_symbol( LCD_ICON_BEEPER2, SEG_OFF );
display_symbol( LCD_ICON_RECORD, SEG_OFF );
_display_l2_clean();
return;
case DISPLAY_LINE_UPDATE_FULL:
display_symbol( LCD_ICON_BEEPER1,
( G_vario.beep_mode ) ? SEG_ON : SEG_OFF );
display_symbol( LCD_ICON_BEEPER2,
( ( G_vario.beep_mode == VARIO_BEEPMODE_ASCENT_0 )
|| ( G_vario.beep_mode == VARIO_BEEPMODE_BOTH ))
? SEG_ON : SEG_OFF );
//
// fall through to partial update
//
case DISPLAY_LINE_UPDATE_PARTIAL:
break;
}
#if VARIO_F_TIME
// Update flight time regardless of whether we do have an altitude.
//
// Be careful to only update the time when a time update is active,
// ie, not because this refresh is due to a DOWN button press.
//
if ( display.flag.update_time )
{
G_vario.stats.f_time.ss++;
if ( G_vario.stats.f_time.ss > 59 )
{
G_vario.stats.f_time.ss = 0;
G_vario.stats.f_time.mm++;
if ( G_vario.stats.f_time.mm > 59 )
{
G_vario.stats.f_time.mm = 0;
G_vario.stats.f_time.hh++;
}
// Reset flight time after 19 hours, more will not fit on lcd !
if ( G_vario.stats.f_time.hh > 19 )
G_vario.stats.f_time.hh = 0;
}
}
#endif
//
// Partial or full update. Make sure pressure sensor is being sampled, ie,
// that line 1 is in altimeter mode.
//
if ( is_altitude_measurement() )
{
s16 diff;
if ( vario_p_read( &pressure ) )
{
// Happens during key presses, never mind.
return; // no data, wait for update
}
//
// If this is the very first time we are here, we have no previous
// pressure, handle that situation.
//
if ( !_idone )
{
diff = 0;
++_idone;
}
else
{
//
// Calculate difference in Pascal - we will need it anyway for the
// buzzer. Pressure decreases with altitude, ensure going lower is
// negative.
//
diff = G_vario.prev_pa - pressure;
#if VARIO_VZ
// update stats as we may want to see these after the flight.
if ( diff > G_vario.stats.vzmax ) G_vario.stats.vzmax = diff;
if ( diff < G_vario.stats.vzmin ) G_vario.stats.vzmin = diff;
#endif
#if VARIO_ALTMAX
// Peek at current altitude in altimeter data.
if ( G_vario.stats.altmax < sAlt.altitude )
G_vario.stats.altmax = sAlt.altitude;
#endif
}
_display_l2_clean();
// Pulse the vario heartbeat indicator.
++_vbeat;
display_symbol( LCD_ICON_RECORD, ( _vbeat & 1 ) ? SEG_ON : SEG_OFF );
// Now see what value to display.
switch( G_vario.view_mode )
{
case VARIO_VIEWMODE_ALT_M:
//
// convert the difference in Pa to a vertical velocity.
//
_display_signed( _pascal_to_vz( diff ), 1 );
break;
#if VARIO_ALT_PA
case VARIO_VIEWMODE_ALT_PA:
//
// display raw difference in Pascal.
//
_display_signed( diff, 0 );
break;
#endif
#if VARIO_PA
case VARIO_VIEWMODE_PA:
//
// display pressure as hhhh.pp (hPa and Pa)
//
_display_signed( pressure, 1 );
break;
#endif
#if VARIO_VZ
case VARIO_VIEWMODE_VZMAX:
display_symbol( LCD_SYMB_MAX, SEG_ON);
_display_signed( _pascal_to_vz( G_vario.stats.vzmax ), 1 );
break;
case VARIO_VIEWMODE_VZMIN:
display_symbol( LCD_SYMB_MAX, SEG_ON);
_display_signed( _pascal_to_vz( G_vario.stats.vzmin ), 1 );
break;
#endif
#if VARIO_ALTMAX
case VARIO_VIEWMODE_ALT_MAX:
display_symbol( LCD_SYMB_MAX, SEG_ON);
_display_signed( G_vario.stats.altmax, 0 );
break;
#endif
#if VARIO_F_TIME
case VARIO_VIEWMODE_F_TIME:
display_chars(LCD_SEG_L2_5_0, int_to_array(G_vario.stats.f_time.hh,2,0), SEG_ON);
display_chars(LCD_SEG_L2_3_0, int_to_array(G_vario.stats.f_time.mm,2,0), SEG_ON);
display_chars(LCD_SEG_L2_1_0, int_to_array(G_vario.stats.f_time.ss,2,0), SEG_ON);
display_symbol(LCD_SEG_L2_COL1, SEG_ON);
display_symbol(LCD_SEG_L2_COL0, SEG_ON);
break;
#endif
case VARIO_VIEWMODE_MAX:
break;
} // switch view mode
// If beeper is enabled, beep.
switch ( G_vario.beep_mode )
{
case VARIO_BEEPMODE_ASCENT_0:
if ( diff >= 0 ) chirp( diff );
break;
case VARIO_BEEPMODE_ASCENT_1:
if ( diff > 0 ) chirp( diff );
break;
case VARIO_BEEPMODE_BOTH:
if ( diff ) chirp( diff );
break;
case VARIO_BEEPMODE_OFF:
case VARIO_BEEPMODE_MAX:
break;
}
// update previous pressure measurement.
G_vario.prev_pa = pressure;
} // L1 is in altimeter mode
else
{
_display_l2_clean();
display_chars(LCD_SEG_L2_5_0, (u8*) " NOALT", SEG_ON);
_idone = 0; // avoid false peaks when re-enabling the altimeter
}
}
byte Beak::chirp(const char *chirpStr) {
int16_t nFrames = numberOfFrames();
SynthFrame *frames = (SynthFrame *) alloca(sizeof(SynthFrame[nFrames]));
return chirp(chirpStr, nFrames, frames);
}
