void
CirclingComputer::TurnRate(CirclingInfo &circling_info,
const NMEAInfo &basic,
const FlyingState &flight)
{
if (!basic.time_available || !flight.flying || !turn_rate_delta_time.IsDefined()) {
circling_info.turn_rate = Angle::Zero();
circling_info.turn_rate_heading = Angle::Zero();
circling_info.turn_rate_smoothed = Angle::Zero();
circling_info.turn_rate_heading_smoothed = Angle::Zero();
last_track = basic.track;
last_heading = basic.attitude.heading;
// initialize turn_rate_delta_time on first call
if (basic.time_available)
turn_rate_delta_time.Update(basic.time, 1./3., 10);
return;
}
const auto dt = turn_rate_delta_time.Update(basic.time, 1./3., 10);
if (dt < 0) {
circling_info.turn_rate = Angle::Zero();
circling_info.turn_rate_heading = Angle::Zero();
circling_info.turn_rate_smoothed = Angle::Zero();
circling_info.turn_rate_heading_smoothed = Angle::Zero();
last_track = basic.track;
last_heading = basic.attitude.heading;
return;
}
if (dt > 0) {
circling_info.turn_rate =
(basic.track - last_track).AsDelta() / dt;
circling_info.turn_rate_heading =
(basic.attitude.heading - last_heading).AsDelta() / dt;
// JMW limit rate to 50 deg per second otherwise a big spike
// will cause spurious lock on circling for a long time
Angle turn_rate = Clamp(circling_info.turn_rate,
Angle::Degrees(-50), Angle::Degrees(50));
// Make the turn rate more smooth using the LowPassFilter
auto smoothed = LowPassFilter(circling_info.turn_rate_smoothed.Native(),
turn_rate.Native(), 0.3);
circling_info.turn_rate_smoothed = Angle::Native(smoothed);
// Makes smoothing of heading turn rate
turn_rate = Clamp(circling_info.turn_rate_heading,
Angle::Degrees(-50), Angle::Degrees(50));
// Make the heading turn rate more smooth using the LowPassFilter
smoothed = LowPassFilter(circling_info.turn_rate_heading_smoothed.Native(),
turn_rate.Native(), 0.3);
circling_info.turn_rate_heading_smoothed = Angle::Native(smoothed);
last_track = basic.track;
last_heading = basic.attitude.heading;
}
}
int
main(int argc, char *argv[])
{
ACTempData acTemps[MinPerDay];
/*
* check for the correct number of command line arguments. If incorrect
* provide a simple usage message to the assist the user
*
*/
if( argc != 3 ) {
printf("\nUsage: %s inputFile outputFile \n\n", argv[0]);
return -1;
}
if( !ReadTempDataFromFile(acTemps, argv[1]) ) {
printf("Could not read from input file %s\n", argv[1]);
return -1;
}
RemoveErroneousData(acTemps);
LowPassFilter(acTemps);
TrendExtraction(acTemps);
if( !WriteTempDataToFile(acTemps, argv[2]) ) {
printf("Could not write to output file %s\n", argv[2]);
return -1;
}
return 0;
}
uint8_t ADXL345_Poll()
{
uint8_t LSB, MSB;
twi(TWI_START);
TWDR=ADXL345_ADDR|I2C_WRITE;
twi(TWI_TRANSMIT);
TWDR=0x32;
twi(TWI_TRANSMIT);
twi(TWI_RESTART);
TWDR=ADXL345_ADDR|I2C_READ;
twi(TWI_TRANSMIT);
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_ACK);
MSB=TWDR;
p_k_y.value = k_y.value;
k_y.value = LowPassFilter((-0.0032) * (float)(MSB << 8 | LSB), 1);
k_y.value += coeffs[ADXL345_OFFY].value;
k_y.value *= coeffs[ADXL345_SCY].value;
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_ACK);
MSB=TWDR;
p_k_x.value = k_x.value;
k_x.value = LowPassFilter((-0.0032) * (float)(MSB << 8 | LSB), 2);
k_x.value += coeffs[ADXL345_OFFX].value;
k_x.value *= coeffs[ADXL345_SCX].value;
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_NACK);
MSB=TWDR;
twi(TWI_STOP);
p_k_z.value = k_z.value;
k_z.value = LowPassFilter((0.0032) * (float)(MSB << 8 | LSB), 3);
k_z.value += coeffs[ADXL345_OFFZ].value;
k_z.value *= coeffs[ADXL345_SCZ].value;
return 0;
}
//
// Sollfarh / Dolphin Speed calculator
//
void SpeedToFly(NMEA_INFO *Basic, DERIVED_INFO *Calculated) {
if (((AutoMcMode == amcFinalGlide) || (AutoMcMode == amcFinalAndClimb))
&& DoOptimizeRoute() && Calculated->NextAltitude > 0.) {
// Special case for Conical end of Speed section
int Type = -1;
double ConeSlope = 0.0;
LockTaskData();
if (ValidTaskPoint(ActiveWayPoint)) {
GetTaskSectorParameter(ActiveWayPoint, &Type, NULL);
ConeSlope = Task[ActiveWayPoint].PGConeSlope;
}
UnlockTaskData();
if (Type == CONE && ConeSlope > 0.0) {
double VOpt = GlidePolar::FindSpeedForSlope(ConeSlope);
double eqMC = GlidePolar::EquMC(VOpt);
if(eqMC <= MACCREADY ) {
Calculated->VOpt = VOpt;
return;
}
}
}
double HeadWind = 0;
if (Calculated->FinalGlide && ValidTaskPoint(ActiveWayPoint)) {
// according to MC theory STF take account of wind only if on final Glide
// TODO : for the future add config parameter for always use wind.
if (Calculated->HeadWind != -999) {
HeadWind = Calculated->HeadWind;
}
}
// this is IAS for best Ground Glide ratio acounting current air mass ( wind / Netto vario )
double VOptnew = GlidePolar::STF(MACCREADY, Calculated->NettoVario, HeadWind);
// apply cruises efficiency factor.
VOptnew *= CRUISE_EFFICIENCY;
if (Calculated->NettoVario > MACCREADY) {
// this air mass is better than maccready, so don't fly at speed less than minimum sink speed adjusted for load factor
double n = fabs((Basic->AccelerationAvailable) ? Basic->AccelZ : Calculated->Gload);
VOptnew = max(VOptnew, GlidePolar::Vminsink() * sqrt(n));
} else {
// never fly at speed less than min sink speed
VOptnew = max(VOptnew, GlidePolar::Vminsink());
}
// use low pass filter for avoid big jump of value.
Calculated->VOpt = LowPassFilter(Calculated->VOpt, VOptnew, 0.6);
}
fixed
UpdateGR(fixed gr, fixed leg_distance, fixed height_above_leg,
fixed filter_factor)
{
if (!positive(leg_distance))
return gr;
fixed glideangle = height_above_leg / leg_distance;
if (gr != INVALID_GR)
glideangle = LowPassFilter(fixed_one / gr, glideangle, filter_factor);
if (fabs(glideangle) > fixed_one / INVALID_GR)
gr = LimitGR(fixed_one / glideangle);
else
gr = INVALID_GR;
return gr;
}
fixed
UpdateLD(fixed ld, fixed leg_distance, fixed height_above_leg,
fixed filter_factor)
{
if (!positive(leg_distance))
return ld;
fixed glideangle = height_above_leg / leg_distance;
if (ld != fixed(INVALID_GR))
glideangle = LowPassFilter(fixed_one / ld, glideangle, filter_factor);
if (fabs(glideangle) > fixed_one / INVALID_GR)
ld = LimitLD(fixed_one / glideangle);
else
ld = fixed(INVALID_GR);
return ld;
}
double
UpdateLD(double LD, double d, double h, double filter_factor)
{
double glideangle;
if (LD != 0) {
glideangle = 1.0 / LD;
} else {
glideangle = 1.0;
}
if (d != 0) {
glideangle = LowPassFilter(1.0 / LD, h / d, filter_factor);
if (fabs(glideangle) > 1.0 / INVALID_GR) {
LD = LimitLD(1.0 / glideangle);
} else {
LD = INVALID_GR;
}
}
return LD;
}
fixed
UpdateLD(fixed LD, fixed leg_distance, fixed height_above_leg,
fixed filter_factor)
{
if (!positive(leg_distance))
return LD;
fixed glideangle;
if (LD != fixed_zero)
glideangle = fixed_one / LD;
else
glideangle = fixed_one;
glideangle = LowPassFilter(fixed_one / LD, height_above_leg / leg_distance,
filter_factor);
if (fabs(glideangle) > fixed_one / INVALID_GR)
LD = LimitLD(fixed_one / glideangle);
else
LD = fixed(INVALID_GR);
return LD;
}
inline void
GlideComputerAirData::LastThermalStats(const MoreData &basic,
DerivedInfo &calculated,
bool last_circling)
{
if (calculated.circling || !last_circling ||
!positive(calculated.climb_start_time))
return;
fixed duration = calculated.cruise_start_time - calculated.climb_start_time;
if (duration < THERMAL_TIME_MIN)
return;
fixed gain =
calculated.cruise_start_altitude_te - calculated.climb_start_altitude_te;
if (!positive(gain))
return;
bool was_defined = calculated.last_thermal.IsDefined();
calculated.last_thermal.start_time = calculated.climb_start_time;
calculated.last_thermal.end_time = calculated.cruise_start_time;
calculated.last_thermal.gain = gain;
calculated.last_thermal.duration = duration;
calculated.last_thermal.CalculateLiftRate();
if (!was_defined)
calculated.last_thermal_average_smooth =
calculated.last_thermal.lift_rate;
else
calculated.last_thermal_average_smooth =
LowPassFilter(calculated.last_thermal_average_smooth,
calculated.last_thermal.lift_rate, fixed(0.3));
ThermalSources(basic, calculated, calculated.thermal_locator);
}
static BOOL PDGFTL1(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
/*
$PDGFTL1 field example
QNE 1013.25 altitude 0 2025 meters
QNH altitude 1 2000 meters
vario cm/s 2 250 2.5m/s
netto vario 3 -14 dm/s
IAS 4 45 kmh
ground efficiency 5 134 13:4 GR
Wind speed 6 28 kmh
Wind direction 7 65 degrees
Main lithium battery 3,82 v 8 382 0.01v
Backup AA battery 1,53 v 9 153 0.01v
100209 Paolo Ventafridda
netto and ias optionals
*/
TCHAR ctemp[80];
double vtas, vias;
double altqne, altqnh;
static bool initqnh=true;
NMEAParser::ExtractParameter(String,ctemp,0);
altqne = StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,1);
altqnh = StrToDouble(ctemp,NULL);
// AutoQNH will take care of setting an average QNH if nobody does it for a while
if (initqnh) {
// if digifly has qnh set by user qne and qnh are of course different
if (altqne != altqnh) {
UpdateQNH(FindQNH(altqne,altqnh));
StartupStore(_T(". Using Digifly QNH %f%s"),QNH,NEWLINE);
initqnh=false;
} else {
// if locally QNH was set, either by user of by AutoQNH, stop processing QNH from digifly
if ( (QNH <= 1012) || (QNH>=1014)) initqnh=false;
// else continue entering initqnh until somebody changes qnh in either digifly or lk8000
}
}
UpdateBaroSource( pGPS,0, d, AltitudeToQNHAltitude(altqne));
NMEAParser::ExtractParameter(String,ctemp,2);
#if 1
pGPS->Vario = StrToDouble(ctemp,NULL)/100;
#else
double newVario = StrToDouble(ctemp,NULL)/100;
pGPS->Vario = LowPassFilter(pGPS->Vario,newVario,0.1);
#endif
pGPS->VarioAvailable = TRUE;
NMEAParser::ExtractParameter(String,ctemp,3);
if (ctemp[0] != '\0') {
pGPS->NettoVario = StrToDouble(ctemp,NULL)/10; // dm/s
pGPS->NettoVarioAvailable = TRUE;
} else
pGPS->NettoVarioAvailable = FALSE;
NMEAParser::ExtractParameter(String,ctemp,4);
if (ctemp[0] != '\0') {
// we store m/s , so we convert it from kmh
vias = StrToDouble(ctemp,NULL)/3.6;
if (vias >1) {
vtas = vias*AirDensityRatio(altqne);
pGPS->TrueAirspeed = vtas;
pGPS->IndicatedAirspeed = vias;
pGPS->AirspeedAvailable = TRUE;
}
} else {
pGPS->AirspeedAvailable = FALSE;
}
NMEAParser::ExtractParameter(String,ctemp,8);
pGPS->ExtBatt1_Voltage = StrToDouble(ctemp,NULL)/100;
NMEAParser::ExtractParameter(String,ctemp,9);
pGPS->ExtBatt2_Voltage = StrToDouble(ctemp,NULL)/100;
TriggerVarioUpdate();
return TRUE;
}
void DoAutoMacCready(NMEA_INFO *Basic, DERIVED_INFO *Calculated) {
if (!Calculated->AutoMacCready) return;
bool is_final_glide = false;
bool is_conical_ess = false;
double ConeSlope = 0.0;
// LockFlightData();
LockTaskData();
double mc_new = MACCREADY;
static bool first_mc = true;
if (AutoMcMode == amcEquivalent) {
if ((!Calculated->Circling) && (!Calculated->OnGround)) {
if (Calculated->EqMc >= 0) {
// MACCREADY = LowPassFilter(MACCREADY,Calculated->EqMc,0.8);
CheckSetMACCREADY(Calculated->EqMc);
} else {
// -1.0 is used as an invalid flag. Normally flying at -1 MC means almost flying
// at stall speed, which is pretty unusual. Maybe in wave conditions?
if (Calculated->EqMc >-1) {
CheckSetMACCREADY(Calculated->EqMc*-1);
}
}
}
UnlockTaskData();
return;
}
// otherwise, if AutoMc for finalglide or "both", return if no goto
if (ValidTaskPoint(ActiveWayPoint)) {
if (Calculated->FinalGlide && ActiveIsFinalWaypoint()) {
is_final_glide = true;
} else {
first_mc = true;
}
if (DoOptimizeRoute() && Calculated->NextAltitude > 0.) {
// Special case for Conical end of Speed section
int Type = -1;
GetTaskSectorParameter(ActiveWayPoint, &Type, NULL);
ConeSlope = Task[ActiveWayPoint].PGConeSlope;
if (Type == CONE && ConeSlope > 0.0) {
is_final_glide = true;
is_conical_ess = true;
}
}
}
double av_thermal = -1;
if (flightstats.ThermalAverage.y_ave > 0) {
if (Calculated->Circling && (Calculated->AverageThermal > 0)) {
#if BUGSTOP
LKASSERT((flightstats.ThermalAverage.sum_n + 1) != 0);
#endif
if (flightstats.ThermalAverage.sum_n == -1) {
flightstats.ThermalAverage.sum_n = -0.99;
}
av_thermal = (flightstats.ThermalAverage.y_ave
* flightstats.ThermalAverage.sum_n
+ Calculated->AverageThermal) /
(flightstats.ThermalAverage.sum_n + 1);
} else {
av_thermal = flightstats.ThermalAverage.y_ave;
}
} else if (Calculated->Circling && (Calculated->AverageThermal > 0)) {
// insufficient stats, so use this/last thermal's average
av_thermal = Calculated->AverageThermal;
}
if (!ValidTaskPoint(ActiveWayPoint)) {
if (av_thermal > 0) {
mc_new = av_thermal;
} else {
mc_new = 0;
}
} else if (((AutoMcMode == amcFinalGlide) || (AutoMcMode == amcFinalAndClimb)) && is_final_glide) {
if (Calculated->TaskAltitudeDifference0 > 0) {
// only change if above final glide with zero Mc
// otherwise when we are well below, it will wind Mc back to
// zero
#if BUGSTOP
LKASSERT((Calculated->WaypointDistance + 1) != 0);
#endif
if (Calculated->WaypointDistance < 0) Calculated->WaypointDistance = 0; // temporary but ok
double slope =
(Calculated->NavAltitude + Calculated->EnergyHeight
- FAIFinishHeight(Basic, Calculated, ActiveWayPoint)) /
(Calculated->WaypointDistance + 1);
double mc_pirker = PirkerAnalysis(Basic, Calculated,
Calculated->WaypointBearing,
slope);
mc_pirker = max(0.0, mc_pirker);
if (first_mc) {
// don't allow Mc to wind down to zero when first achieving
// final glide; but do allow it to wind down after that
if (mc_pirker >= mc_new) {
mc_new = mc_pirker;
first_mc = false;
} else if (AutoMcMode == amcFinalAndClimb) {
// revert to averager based auto Mc
if (av_thermal > 0) {
mc_new = av_thermal;
}
}
} else {
mc_new = mc_pirker;
}
if (is_conical_ess) {
const double VOpt = GlidePolar::FindSpeedForSlope(ConeSlope);
const double eqMC = GlidePolar::EquMC(VOpt);
if(mc_new > eqMC) {
mc_new = eqMC;
}
}
} else { // below final glide at zero Mc, never achieved final glide
if (first_mc && (AutoMcMode == amcFinalAndClimb)) {
// revert to averager based auto Mc
if (av_thermal > 0) {
mc_new = av_thermal;
}
}
}
} else if ((AutoMcMode == amcAverageClimb) || ((AutoMcMode == amcFinalAndClimb)&& !is_final_glide)) {
if (av_thermal > 0) {
mc_new = av_thermal;
}
}
CheckSetMACCREADY(LowPassFilter(MACCREADY, mc_new, 0.6));
UnlockTaskData();
// UnlockFlightData();
}
//
// Support for new 2011 Digifly TOURTELL protocol
// (Subset of TL1, pending upgrade in june 2011 for all devices)
//
static BOOL PDGFTTL(PDeviceDescriptor_t d, TCHAR *String, NMEA_INFO *pGPS)
{
/*
$PDGFTTL field example
QNE 1013.25 altitude 0 2025 meters
QNH altitude 1 2000 meters
vario cm/s 2 250 2.5m/s
IAS 4 45 kmh
netto vario 3 -14 dm/s
*/
TCHAR ctemp[80];
double vtas, vias;
double altqne, altqnh;
static bool initqnh=true;
NMEAParser::ExtractParameter(String,ctemp,0);
altqne = StrToDouble(ctemp,NULL);
NMEAParser::ExtractParameter(String,ctemp,1);
altqnh = StrToDouble(ctemp,NULL);
// AutoQNH will take care of setting an average QNH if nobody does it for a while
if (initqnh) {
// if digifly has qnh set by user qne and qnh are of course different
if (altqne != altqnh) {
QNH=FindQNH(altqne,altqnh);
CAirspaceManager::Instance().QnhChangeNotify(QNH);
StartupStore(_T(". Using Digifly QNH %f%s"),QNH,NEWLINE);
initqnh=false;
} else {
// if locally QNH was set, either by user of by AutoQNH, stop processing QNH from digifly
if ( (QNH <= 1012) || (QNH>=1014)) initqnh=false;
// else continue entering initqnh until somebody changes qnh in either digifly or lk8000
}
}
UpdateBaroSource( pGPS, 0,d, AltitudeToQNHAltitude(altqne));
NMEAParser::ExtractParameter(String,ctemp,2);
#if 1
pGPS->Vario = StrToDouble(ctemp,NULL)/100;
#else
double newVario = StrToDouble(ctemp,NULL)/100;
pGPS->Vario = LowPassFilter(pGPS->Vario,newVario,0.1);
#endif
pGPS->VarioAvailable = TRUE;
NMEAParser::ExtractParameter(String,ctemp,4);
if (ctemp[0] != '\0') {
pGPS->NettoVario = StrToDouble(ctemp,NULL)/10; // dm/s
pGPS->NettoVarioAvailable = TRUE;
} else
pGPS->NettoVarioAvailable = FALSE;
NMEAParser::ExtractParameter(String,ctemp,4);
if (ctemp[0] != '\0') {
// we store m/s , so we convert it from kmh
vias = StrToDouble(ctemp,NULL)/3.6;
if (vias >1) {
vtas = vias*AirDensityRatio(pGPS->BaroAltitude);
pGPS->TrueAirspeed = vtas;
pGPS->IndicatedAirspeed = vias;
pGPS->AirspeedAvailable = TRUE;
}
} else {
pGPS->AirspeedAvailable = FALSE;
}
TriggerVarioUpdate();
return TRUE;
}
void Wiimote::GetIRData(u8* const data, bool use_accel)
{
const bool has_focus = HAS_FOCUS;
u16 x[4], y[4];
memset(x, 0xFF, sizeof(x));
if (has_focus)
{
float xx = 10000, yy = 0, zz = 0;
double nsin,ncos;
if (use_accel)
{
double ax,az,len;
ax=m_accel.x;
az=m_accel.z;
len=sqrt(ax*ax+az*az);
if (len)
{
ax/=len;
az/=len; //normalizing the vector
nsin=ax;
ncos=az;
}
else
{
nsin=0;
ncos=1;
}
// PanicAlert("%d %d %d\nx:%f\nz:%f\nsin:%f\ncos:%f",accel->x,accel->y,accel->z,ax,az,sin,cos);
//PanicAlert("%d %d %d\n%d %d %d\n%d %d %d",accel->x,accel->y,accel->z,calib->zero_g.x,calib->zero_g.y,calib->zero_g.z,
// calib->one_g.x,calib->one_g.y,calib->one_g.z);
}
else
{
nsin=0; //m_tilt stuff here (can't figure it out yet....)
ncos=1;
}
LowPassFilter(ir_sin,nsin,1.0f/60);
LowPassFilter(ir_cos,ncos,1.0f/60);
m_ir->GetState(&xx, &yy, &zz, true);
UDPTLayer::GetIR(m_udp, &xx, &yy, &zz);
Vertex v[4];
static const int camWidth=1024;
static const int camHeight=768;
static const double bndup=-0.315447;
static const double bnddown=0.85;
static const double bndleft=0.443364;
static const double bndright=-0.443364;
static const double dist1=100.f/camWidth; //this seems the optimal distance for zelda
static const double dist2=1.2f*dist1;
for (auto& vtx : v)
{
vtx.x=xx*(bndright-bndleft)/2+(bndleft+bndright)/2;
if (m_sensor_bar_on_top) vtx.y=yy*(bndup-bnddown)/2+(bndup+bnddown)/2;
else vtx.y=yy*(bndup-bnddown)/2-(bndup+bnddown)/2;
vtx.z=0;
}
v[0].x-=(zz*0.5+1)*dist1;
v[1].x+=(zz*0.5+1)*dist1;
v[2].x-=(zz*0.5+1)*dist2;
v[3].x+=(zz*0.5+1)*dist2;
#define printmatrix(m) PanicAlert("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n",m[0][0],m[0][1],m[0][2],m[0][3],m[1][0],m[1][1],m[1][2],m[1][3],m[2][0],m[2][1],m[2][2],m[2][3],m[3][0],m[3][1],m[3][2],m[3][3])
Matrix rot,tot;
static Matrix scale;
static bool isscale=false;
if (!isscale)
{
MatrixScale(scale,1,camWidth/camHeight,1);
//MatrixIdentity(scale);
}
MatrixRotationByZ(rot,ir_sin,ir_cos);
//MatrixIdentity(rot);
MatrixMultiply(tot,scale,rot);
for (int i=0; i<4; i++)
{
MatrixTransformVertex(tot,v[i]);
if ((v[i].x<-1)||(v[i].x>1)||(v[i].y<-1)||(v[i].y>1))
continue;
x[i]=(u16)round((v[i].x+1)/2*(camWidth-1));
y[i]=(u16)round((v[i].y+1)/2*(camHeight-1));
}
// PanicAlert("%f %f\n%f %f\n%f %f\n%f %f\n%d %d\n%d %d\n%d %d\n%d %d",
// v[0].x,v[0].y,v[1].x,v[1].y,v[2].x,v[2].y,v[3].x,v[3].y,
// x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[38]);
}
// Fill report with valid data when full handshake was done
if (m_reg_ir.data[0x30])
// ir mode
switch (m_reg_ir.mode)
{
// basic
case 1 :
{
memset(data, 0xFF, 10);
wm_ir_basic* const irdata = (wm_ir_basic*)data;
for (unsigned int i=0; i<2; ++i)
{
if (x[i*2] < 1024 && y[i*2] < 768)
{
irdata[i].x1 = u8(x[i*2]);
irdata[i].x1hi = x[i*2] >> 8;
irdata[i].y1 = u8(y[i*2]);
irdata[i].y1hi = y[i*2] >> 8;
}
if (x[i*2+1] < 1024 && y[i*2+1] < 768)
{
irdata[i].x2 = u8(x[i*2+1]);
irdata[i].x2hi = x[i*2+1] >> 8;
irdata[i].y2 = u8(y[i*2+1]);
irdata[i].y2hi = y[i*2+1] >> 8;
}
}
}
void Wiimote::GetIRData(u8* const data, bool use_accel)
{
u16 x[4], y[4];
memset(x, 0xFF, sizeof(x));
ControlState xx = 10000, yy = 0, zz = 0;
double nsin, ncos;
if (use_accel)
{
double ax, az, len;
ax = m_accel.x;
az = m_accel.z;
len = sqrt(ax * ax + az * az);
if (len)
{
ax /= len;
az /= len; // normalizing the vector
nsin = ax;
ncos = az;
}
else
{
nsin = 0;
ncos = 1;
}
}
else
{
// TODO m_tilt stuff
nsin = 0;
ncos = 1;
}
LowPassFilter(ir_sin, nsin, 1.0 / 60);
LowPassFilter(ir_cos, ncos, 1.0 / 60);
m_ir->GetState(&xx, &yy, &zz, true);
Vertex v[4];
static const int camWidth = 1024;
static const int camHeight = 768;
static const double bndup = -0.315447;
static const double bnddown = 0.85;
static const double bndleft = 0.443364;
static const double bndright = -0.443364;
static const double dist1 = 100.0 / camWidth; // this seems the optimal distance for zelda
static const double dist2 = 1.2 * dist1;
for (auto& vtx : v)
{
vtx.x = xx * (bndright - bndleft) / 2 + (bndleft + bndright) / 2;
if (m_sensor_bar_on_top)
vtx.y = yy * (bndup - bnddown) / 2 + (bndup + bnddown) / 2;
else
vtx.y = yy * (bndup - bnddown) / 2 - (bndup + bnddown) / 2;
vtx.z = 0;
}
v[0].x -= (zz * 0.5 + 1) * dist1;
v[1].x += (zz * 0.5 + 1) * dist1;
v[2].x -= (zz * 0.5 + 1) * dist2;
v[3].x += (zz * 0.5 + 1) * dist2;
#define printmatrix(m) \
PanicAlert("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n", m[0][0], m[0][1], m[0][2], \
m[0][3], m[1][0], m[1][1], m[1][2], m[1][3], m[2][0], m[2][1], m[2][2], m[2][3], \
m[3][0], m[3][1], m[3][2], m[3][3])
Matrix rot, tot;
static Matrix scale;
MatrixScale(scale, 1, camWidth / camHeight, 1);
MatrixRotationByZ(rot, ir_sin, ir_cos);
MatrixMultiply(tot, scale, rot);
for (int i = 0; i < 4; i++)
{
MatrixTransformVertex(tot, v[i]);
if ((v[i].x < -1) || (v[i].x > 1) || (v[i].y < -1) || (v[i].y > 1))
continue;
x[i] = (u16)lround((v[i].x + 1) / 2 * (camWidth - 1));
y[i] = (u16)lround((v[i].y + 1) / 2 * (camHeight - 1));
}
// Fill report with valid data when full handshake was done
if (m_reg_ir.data[0x30])
// ir mode
switch (m_reg_ir.mode)
{
// basic
case 1:
{
memset(data, 0xFF, 10);
wm_ir_basic* const irdata = reinterpret_cast<wm_ir_basic*>(data);
for (unsigned int i = 0; i < 2; ++i)
{
if (x[i * 2] < 1024 && y[i * 2] < 768)
{
irdata[i].x1 = static_cast<u8>(x[i * 2]);
irdata[i].x1hi = x[i * 2] >> 8;
irdata[i].y1 = static_cast<u8>(y[i * 2]);
irdata[i].y1hi = y[i * 2] >> 8;
}
if (x[i * 2 + 1] < 1024 && y[i * 2 + 1] < 768)
{
irdata[i].x2 = static_cast<u8>(x[i * 2 + 1]);
irdata[i].x2hi = x[i * 2 + 1] >> 8;
irdata[i].y2 = static_cast<u8>(y[i * 2 + 1]);
irdata[i].y2hi = y[i * 2 + 1] >> 8;
}
}
}
void Turning(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
static double LastTrack = 0;
static double StartTime = 0;
static double StartLong = 0;
static double StartLat = 0;
static double StartAlt = 0;
static double StartEnergyHeight = 0;
static double LastTime = 0;
static int MODE = CRUISE;
static bool LEFT = FALSE;
double Rate;
static double LastRate=0;
double dRate;
double dT;
if (!Calculated->Flying) {
if (MODE!=CRUISE) {
#if TESTBENCH
StartupStore(_T(".... Not flying, still circling -> Cruise forced!\n"));
#endif
goto _forcereset;
}
return;
}
// Back in time in IGC replay mode?
if(Basic->Time <= LastTime) {
#if TESTBENCH
StartupStore(_T("...... Turning check is back in time. Now=%f Last=%f: reset %s\n"),Basic->Time, LastTime,WhatTimeIsIt());
#endif
_forcereset:
LastTime = Basic->Time; // 101216 PV not sure of this..
LastTrack = 0;
StartTime = 0;
StartLong = 0;
StartLat = 0;
StartAlt = 0;
StartEnergyHeight = 0;
LastTime = 0;
LEFT = FALSE;
if (MODE!=CRUISE) {
MODE = CRUISE;
// Finally do the transition to cruise
Calculated->Circling = false;
SwitchZoomClimb(Basic, Calculated, false, LEFT);
InputEvents::processGlideComputer(GCE_FLIGHTMODE_CRUISE);
}
return;
}
dT = Basic->Time - LastTime;
LastTime = Basic->Time;
#if BUGSTOP
LKASSERT(dT!=0);
#else
if (dT==0) dT=1;
#endif
Rate = AngleLimit180(Basic->TrackBearing-LastTrack)/dT;
#if DEBUGTURN
StartupStore(_T("... Rate=%f in time=%f\n"),Rate,dT);
#endif
if (dT<2.0 && dT!=0) {
// time step ok
// calculate acceleration
dRate = (Rate-LastRate)/dT;
double dtlead=0.3;
// integrate assuming constant acceleration, for one second
Calculated->NextTrackBearing = Basic->TrackBearing
+ dtlead*(Rate+0.5*dtlead*dRate);
// s = u.t+ 0.5*a*t*t
Calculated->NextTrackBearing =
AngleLimit360(Calculated->NextTrackBearing);
} else {
// time step too big, so just take it at last measurement
Calculated->NextTrackBearing = Basic->TrackBearing;
}
Calculated->TurnRate = Rate;
// JMW limit rate to 50 deg per second otherwise a big spike
// will cause spurious lock on circling for a long time
if (Rate>50) {
Rate = 50;
}
if (Rate<-50) {
Rate = -50;
}
// average rate, to detect essing
static double rate_history[60];
double rate_ave=0;
for (int i=59; i>0; i--) {
rate_history[i] = rate_history[i-1];
rate_ave += rate_history[i];
}
rate_history[0] = Rate;
rate_ave /= 60;
// THIS IS UNUSED in 4.0
Calculated->Essing = fabs(rate_ave)*100/MinTurnRate;
if (MODE==CLIMB||MODE==WAITCRUISE)
Rate = LowPassFilter(LastRate,Rate,0.9);
else
Rate = LowPassFilter(LastRate,Rate,0.3);
LastRate = Rate;
if(Rate <0)
{
if (LEFT) {
// OK, already going left
} else {
LEFT = true;
}
Rate *= -1;
} else {
if (!LEFT) {
// OK, already going right
} else {
LEFT = false;
}
}
PercentCircling(Basic, Calculated, Rate);
LastTrack = Basic->TrackBearing;
bool forcecruise = false;
bool forcecircling = false;
#if 1 // UNUSED, EnableExternalTriggerCruise not configurable, set to false since ever
if (EnableExternalTriggerCruise ) {
if (ExternalTriggerCruise && ExternalTriggerCircling) {
// this should never happen
ExternalTriggerCircling = false;
}
forcecruise = ExternalTriggerCruise;
forcecircling = ExternalTriggerCircling;
}
#endif
switch(MODE) {
case CRUISE:
double cruise_turnthreshold;
if (ISPARAGLIDER)
cruise_turnthreshold=5;
else
cruise_turnthreshold=4;
if((Rate >= cruise_turnthreshold)||(forcecircling)) {
// This is initialising the potential thermalling start
// We still dont know if we are really circling for thermal
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
#if DEBUGTURN
StartupStore(_T("... CRUISE -> WAITCLIMB\n"));
#endif
MODE = WAITCLIMB;
}
if (forcecircling) {
MODE = WAITCLIMB;
} else {
break;
}
case WAITCLIMB:
if (forcecruise) {
MODE = CRUISE;
break;
}
double waitclimb_turnthreshold;
double cruiseclimbswitch;
if (ISPARAGLIDER) {
waitclimb_turnthreshold=5;
cruiseclimbswitch=15; // this should be finetuned for PGs
} else {
waitclimb_turnthreshold=4;
cruiseclimbswitch=15; // this is ok for gliders
}
if((Rate >= waitclimb_turnthreshold)||(forcecircling)) {
// WE CANNOT do this, because we also may need Circling mode to detect FF!!
// if( (Calculated->FreeFlying && ((Basic->Time - StartTime) > cruiseclimbswitch))|| forcecircling) {
if( (!ISCAR && !ISGAAIRCRAFT && ((Basic->Time - StartTime) > cruiseclimbswitch))|| forcecircling) {
#ifdef TOW_CRUISE
// If free flight (FF) hasn�t yet been detected, then we may
// still be on tow. The following prevents climb mode from
// engaging due to normal on-aerotow turns.
if (!Calculated->FreeFlying && (fabs(Calculated->TurnRate) < 12))
break;
#endif
#if DEBUGTURN
StartupStore(_T("... WAITCLIMB -> CLIMB\n"));
#endif
Calculated->Circling = true;
// JMW Transition to climb
MODE = CLIMB;
// Probably a replay flight, with fast forward with no cruise init
if (StartTime==0) {
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
}
Calculated->ClimbStartLat = StartLat;
Calculated->ClimbStartLong = StartLong;
Calculated->ClimbStartAlt = StartAlt+StartEnergyHeight;
Calculated->ClimbStartTime = StartTime;
if (flightstats.Altitude_Ceiling.sum_n>0) {
// only update base if have already climbed, otherwise
// we will catch the takeoff height as the base.
flightstats.Altitude_Base.
least_squares_update(max(0.0, Calculated->ClimbStartTime
- Calculated->TakeOffTime)/3600.0,
StartAlt);
}
SwitchZoomClimb(Basic, Calculated, true, LEFT);
InputEvents::processGlideComputer(GCE_FLIGHTMODE_CLIMB);
}
} else {
// nope, not turning, so go back to cruise
#if DEBUGTURN
StartupStore(_T("... WAITCLIMB -> CRUISE\n"));
#endif
MODE = CRUISE;
}
break;
case CLIMB:
if ( (AutoWindMode == D_AUTOWIND_CIRCLING) || (AutoWindMode==D_AUTOWIND_BOTHCIRCZAG) ) {
LockFlightData();
windanalyser->slot_newSample(Basic, Calculated);
UnlockFlightData();
}
double climb_turnthreshold;
if (ISPARAGLIDER)
climb_turnthreshold=10;
else
climb_turnthreshold=4;
if((Rate < climb_turnthreshold)||(forcecruise)) {
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
// JMW Transition to cruise, due to not properly turning
MODE = WAITCRUISE;
#if DEBUGTURN
StartupStore(_T("... CLIMB -> WAITCRUISE\n"));
#endif
}
if (forcecruise) {
MODE = WAITCRUISE;
} else {
break;
}
case WAITCRUISE:
if (forcecircling) {
MODE = CLIMB;
break;
}
double waitcruise_turnthreshold;
double climbcruiseswitch;
if (ISPARAGLIDER) {
waitcruise_turnthreshold=10;
climbcruiseswitch=15;
} else {
waitcruise_turnthreshold=4;
climbcruiseswitch=9; // ok for gliders
}
//
// Exiting climb mode?
//
if((Rate < waitcruise_turnthreshold) || forcecruise) {
if( ((Basic->Time - StartTime) > climbcruiseswitch) || forcecruise) {
// We are no more in climb mode
if (StartTime==0) {
StartTime = Basic->Time;
StartLong = Basic->Longitude;
StartLat = Basic->Latitude;
StartAlt = Calculated->NavAltitude;
StartEnergyHeight = Calculated->EnergyHeight;
}
Calculated->CruiseStartLat = StartLat;
Calculated->CruiseStartLong = StartLong;
Calculated->CruiseStartAlt = StartAlt;
Calculated->CruiseStartTime = StartTime;
// Here we assign automatically this last thermal to the L> multitarget
if (Calculated->ThermalGain >100) {
// Force immediate calculation of average thermal, it would be made
// during next cycle, but we need it here immediately
AverageThermal(Basic,Calculated);
if (EnableThermalLocator) {
InsertThermalHistory(Calculated->ClimbStartTime,
Calculated->ThermalEstimate_Latitude, Calculated->ThermalEstimate_Longitude,
Calculated->ClimbStartAlt, Calculated->NavAltitude, Calculated->AverageThermal);
} else {
InsertThermalHistory(Calculated->ClimbStartTime,
Calculated->ClimbStartLat, Calculated->ClimbStartLong,
Calculated->ClimbStartAlt, Calculated->NavAltitude, Calculated->AverageThermal);
}
}
InitLDRotary(&rotaryLD);
InitWindRotary(&rotaryWind);
flightstats.Altitude_Ceiling.
least_squares_update(max(0.0, Calculated->CruiseStartTime
- Calculated->TakeOffTime)/3600.0,
Calculated->CruiseStartAlt);
// Finally do the transition to cruise
Calculated->Circling = false;
MODE = CRUISE;
#if DEBUGTURN
StartupStore(_T("... WAITCRUISE -> CRUISE\n"));
#endif
SwitchZoomClimb(Basic, Calculated, false, LEFT);
InputEvents::processGlideComputer(GCE_FLIGHTMODE_CRUISE);
} // climbcruiseswitch time in range
} else { // Rate>Minturnrate, back to climb, turning again
#if DEBUGTURN
StartupStore(_T("... WAITCRUISE -> CLIMB\n"));
#endif
MODE = CLIMB;
}
break;
default:
// error, go to cruise
MODE = CRUISE;
}
// generate new wind vector if altitude changes or a new
// estimate is available
if (AutoWindMode>D_AUTOWIND_MANUAL && AutoWindMode <D_AUTOWIND_EXTERNAL) {
LockFlightData();
windanalyser->slot_Altitude(Basic, Calculated);
UnlockFlightData();
}
if (EnableThermalLocator) {
if (Calculated->Circling) {
thermallocator.AddPoint(Basic->Time, Basic->Longitude, Basic->Latitude,
Calculated->NettoVario);
thermallocator.Update(Basic->Time, Basic->Longitude, Basic->Latitude,
Calculated->WindSpeed, Calculated->WindBearing,
Basic->TrackBearing,
&Calculated->ThermalEstimate_Longitude,
&Calculated->ThermalEstimate_Latitude,
&Calculated->ThermalEstimate_W,
&Calculated->ThermalEstimate_R);
} else {
Calculated->ThermalEstimate_W = 0;
Calculated->ThermalEstimate_R = -1;
thermallocator.Reset();
}
}
// update atmospheric model
CuSonde::updateMeasurements(Basic, Calculated);
}
void ITG3200::UpdateBias(float dt)
{
m_Bias = LowPassFilter(GetBiasedAngVel(), m_Bias, dt, 30.0f);
}
void DoAutoMacCready(NMEA_INFO *Basic, DERIVED_INFO *Calculated)
{
if (!Calculated->AutoMacCready) return;
bool is_final_glide = false;
// LockFlightData();
LockTaskData();
double mc_new = MACCREADY;
static bool first_mc = true;
if ( AutoMcMode==amcEquivalent ) {
if ( (!Calculated->Circling) && (!Calculated->OnGround)) {
if (Calculated->EqMc>=0) {
// MACCREADY = LowPassFilter(MACCREADY,Calculated->EqMc,0.8);
MACCREADY = Calculated->EqMc;
} else {
// -1.0 is used as an invalid flag. Normally flying at -1 MC means almost flying
// at stall speed, which is pretty unusual. Maybe in wave conditions?
if (Calculated->EqMc >-1) {
MACCREADY=Calculated->EqMc*-1;
}
}
}
UnlockTaskData();
return;
}
// otherwise, if AutoMc for finalglide or "both", return if no goto
if (!ValidTaskPoint(ActiveWayPoint)) {
UnlockTaskData();
return;
}
if (Calculated->FinalGlide && ActiveIsFinalWaypoint()) {
is_final_glide = true;
} else {
first_mc = true;
}
double av_thermal = -1;
if (flightstats.ThermalAverage.y_ave>0) {
if (Calculated->Circling && (Calculated->AverageThermal>0)) {
LKASSERT((flightstats.ThermalAverage.sum_n+1)!=0);
av_thermal = (flightstats.ThermalAverage.y_ave
*flightstats.ThermalAverage.sum_n
+ Calculated->AverageThermal)/
(flightstats.ThermalAverage.sum_n+1);
} else {
av_thermal = flightstats.ThermalAverage.y_ave;
}
} else if (Calculated->Circling && (Calculated->AverageThermal>0)) {
// insufficient stats, so use this/last thermal's average
av_thermal = Calculated->AverageThermal;
}
if (!ValidTaskPoint(ActiveWayPoint)) {
if (av_thermal>0) {
mc_new = av_thermal;
}
} else if ( ((AutoMcMode==amcFinalGlide)||(AutoMcMode==amcFinalAndClimb)) && is_final_glide) {
if (Calculated->TaskAltitudeDifference0>0) {
// only change if above final glide with zero Mc
// otherwise when we are well below, it will wind Mc back to
// zero
LKASSERT((Calculated->WaypointDistance+1)!=0);;
double slope =
(Calculated->NavAltitude + Calculated->EnergyHeight
- FAIFinishHeight(Basic, Calculated, ActiveWayPoint))/
(Calculated->WaypointDistance+1);
double mc_pirker = PirkerAnalysis(Basic, Calculated,
Calculated->WaypointBearing,
slope);
mc_pirker = max(0.0, mc_pirker);
if (first_mc) {
// don't allow Mc to wind down to zero when first achieving
// final glide; but do allow it to wind down after that
if (mc_pirker >= mc_new) {
mc_new = mc_pirker;
first_mc = false;
} else if (AutoMcMode==amcFinalAndClimb) {
// revert to averager based auto Mc
if (av_thermal>0) {
mc_new = av_thermal;
}
}
} else {
mc_new = mc_pirker;
}
} else { // below final glide at zero Mc, never achieved final glide
if (first_mc && (AutoMcMode==amcFinalAndClimb)) {
// revert to averager based auto Mc
if (av_thermal>0) {
mc_new = av_thermal;
}
}
}
} else if ( (AutoMcMode==amcAverageClimb) || ((AutoMcMode==amcFinalAndClimb)&& !is_final_glide) ) {
if (av_thermal>0) {
mc_new = av_thermal;
}
}
MACCREADY = LowPassFilter(MACCREADY,mc_new,0.6);
UnlockTaskData();
// UnlockFlightData();
}
void TaskSpeed(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const double this_maccready)
{
int ifinal;
static double LastTime = 0;
static double LastTimeStats = 0;
double TotalTime=0, TotalDistance=0, Vfinal=0;
if (!ValidTaskPoint(ActiveWayPoint)) return;
if (Calculated->ValidFinish) return;
if (!Calculated->Flying) return;
// in case we leave early due to error
Calculated->TaskSpeedAchieved = 0;
Calculated->TaskSpeed = 0;
if (ActiveWayPoint<=0) { // no task speed before start
Calculated->TaskSpeedInstantaneous = 0;
return;
}
// LockFlightData();
LockTaskData();
if (TaskAltitudeRequired(Basic, Calculated, this_maccready, &Vfinal,
&TotalTime, &TotalDistance, &ifinal)) {
double t0 = TotalTime;
// total time expected for task
double t1 = Basic->Time-Calculated->TaskStartTime;
// time elapsed since start
double d0 = TotalDistance;
// total task distance
double d1 = Calculated->TaskDistanceCovered;
// actual distance covered
double dr = Calculated->TaskDistanceToGo;
// distance remaining
double hf = FAIFinishHeight(Basic, Calculated, -1);
double h0 = Calculated->TaskAltitudeRequiredFromStart-hf;
// total height required from start (takes safety arrival alt
// and finish waypoint altitude into account)
double h1 = max(0.0, Calculated->NavAltitude-hf);
// height above target
double dFinal;
// final glide distance
// equivalent speed
double v2, v1;
if ((t1<=0) || (d1<=0) || (d0<=0) || (t0<=0) || (h0<=0)) {
// haven't started yet or not a real task
Calculated->TaskSpeedInstantaneous = 0;
//? Calculated->TaskSpeed = 0;
goto OnExit;
}
// JB's task speed...
double hx = max(0.0, SpeedHeight(Basic, Calculated));
double t1mod = t1-hx/MacCreadyOrAvClimbRate(Basic, Calculated, this_maccready);
// only valid if flown for 5 minutes or more
if (t1mod>300.0) {
Calculated->TaskSpeedAchieved = d1/t1mod;
} else {
Calculated->TaskSpeedAchieved = d1/t1;
}
Calculated->TaskSpeed = Calculated->TaskSpeedAchieved;
if (Vfinal<=0) {
// can't reach target at current mc
goto OnExit;
}
// distance that can be usefully final glided from here
// (assumes average task glide angle of d0/h0)
// JMW TODO accuracy: make this more accurate by working out final glide
// through remaining turnpoints. This will more correctly account
// for wind.
#if BUGSTOP
LKASSERT(h0!=0);
#endif
if (h0==0) h0=1;
dFinal = min(dr, d0*min(1.0,max(0.0,h1/h0)));
if (Calculated->ValidFinish) {
dFinal = 0;
}
double dc = max(0.0, dr-dFinal);
// amount of extra distance to travel in cruise/climb before final glide
// actual task speed achieved so far
v1 = d1/t1;
#ifdef OLDTASKSPEED
// time at end of final glide
// equivalent time elapsed after final glide
double t2 = t1+dFinal/Vfinal;
// equivalent distance travelled after final glide
// equivalent distance to end of final glide
double d2 = d1+dFinal;
// average speed to end of final glide from here
v2 = d2/t2;
Calculated->TaskSpeed = max(v1,v2);
#else
// average speed to end of final glide from here, weighted
// according to how much extra time would be spent in cruise/climb
// the closer dc (the difference between remaining distance and
// final glidable distance) gets to zero, the closer v2 approaches
// the average speed to end of final glide from here
// in other words, the more we consider the final glide part to have
// been earned.
// this will be bogus at fast starts though...
LKASSERT((t1+dc/v1+dFinal/Vfinal)!=0);
LKASSERT((t1+dFinal/Vfinal)!=0);
if (v1>0) {
v2 = (d1+dc+dFinal)/(t1+dc/v1+dFinal/Vfinal);
} else {
v2 = (d1+dFinal)/(t1+dFinal/Vfinal);
}
Calculated->TaskSpeed = v2;
#endif
if(Basic->Time < LastTime) {
LastTime = Basic->Time;
} else if (Basic->Time-LastTime >=1.0) {
double dt = Basic->Time-LastTime;
LastTime = Basic->Time;
// Calculate contribution to average task speed.
// This is equal to the change in virtual distance
// divided by the time step
// This is a novel concept.
// When climbing at the MC setting, this number should
// be similar to the estimated task speed.
// When climbing slowly or when flying off-course,
// this number will drop.
// In cruise at the optimum speed in zero lift, this
// number will be similar to the estimated task speed.
// A low pass filter is applied so it doesn't jump around
// too much when circling.
// If this number is higher than the overall task average speed,
// it means that the task average speed is increasing.
// When cruising in sink, this number will decrease.
// When cruising in lift, this number will increase.
// Therefore, it shows well whether at any time the glider
// is wasting time.
// VNT 090723 NOTICE: all of this is totally crazy. Did anyone ever cared to check
// what happens with MC=0 ? Did anyone care to tell people how a simple "ETE" or TaskSpeed
// has been complicated over any limit?
// TODO: start back from scratch, not possible to trust any number here.
static double dr_last = 0;
double mc_safe = max(0.1,this_maccready);
double Vstar = max(1.0,Calculated->VMacCready);
#if BUGSTOP
LKASSERT(dt!=0);
#endif
if (dt==0) dt=1;
double vthis = (Calculated->LegDistanceCovered-dr_last)/dt;
vthis /= AirDensityRatio(Calculated->NavAltitude);
dr_last = Calculated->LegDistanceCovered;
double ttg = max(1.0, Calculated->LegTimeToGo);
// double Vav = d0/max(1.0,t0);
double Vrem = Calculated->LegDistanceToGo/ttg;
double Vref = // Vav;
Vrem;
double sr = -GlidePolar::SinkRate(Vstar);
double height_diff = max(0.0, -Calculated->TaskAltitudeDifference);
if (Calculated->timeCircling>30) {
mc_safe = max(mc_safe,
Calculated->TotalHeightClimb/Calculated->timeCircling);
}
// circling percentage during cruise/climb
double rho_cruise = max(0.0,min(1.0,mc_safe/(sr+mc_safe)));
double rho_climb = 1.0-rho_cruise;
#if BUGSTOP
LKASSERT(mc_safe!=0);
#endif
if (mc_safe==0) mc_safe=0.1;
double time_climb = height_diff/mc_safe;
// calculate amount of time in cruise/climb glide
double rho_c = max(0.0, min(1.0, time_climb/ttg));
if (Calculated->FinalGlide) {
if (rho_climb>0) {
rho_c = max(0.0, min(1.0, rho_c/rho_climb));
}
if (!Calculated->Circling) {
if (Calculated->TaskAltitudeDifference>0) {
rho_climb *= rho_c;
rho_cruise *= rho_c;
// Vref = Vrem;
}
}
}
LKASSERT(mc_safe!=0);
double w_comp = min(10.0,max(-10.0,Calculated->Vario/mc_safe));
double vdiff = vthis/Vstar + w_comp*rho_cruise + rho_climb;
if (vthis > SAFTEYSPEED*2) {
vdiff = 1.0;
// prevent funny numbers when starting mid-track
}
// Calculated->Experimental = vdiff*100.0;
vdiff *= Vref;
if (t1<5) {
Calculated->TaskSpeedInstantaneous = vdiff;
// initialise
} else {
static int lastActiveWayPoint = 0;
static double tsi_av = 0;
static int n_av = 0;
if ((ActiveWayPoint==lastActiveWayPoint)
&& (Calculated->LegDistanceToGo>1000.0)
&& (Calculated->LegDistanceCovered>1000.0)) {
Calculated->TaskSpeedInstantaneous =
LowPassFilter(Calculated->TaskSpeedInstantaneous, vdiff, 0.1);
// update stats
if(Basic->Time < LastTimeStats) {
LastTimeStats = Basic->Time;
tsi_av = 0;
n_av = 0;
} else if (n_av>=60) {
tsi_av/= n_av;
flightstats.Task_Speed.
least_squares_update(
max(0.0,
Basic->Time-Calculated->TaskStartTime)/3600.0,
max(0.0, min(100.0,tsi_av)));
LastTimeStats = Basic->Time;
tsi_av = 0;
n_av = 0;
}
tsi_av += Calculated->TaskSpeedInstantaneous;
n_av ++;
} else {
Calculated->TaskSpeedInstantaneous =
LowPassFilter(Calculated->TaskSpeedInstantaneous, vdiff, 0.5);
// Calculated->TaskSpeedInstantaneous = vdiff;
tsi_av = 0;
n_av = 0;
}
lastActiveWayPoint = ActiveWayPoint;
}
}
}
OnExit:
UnlockTaskData();
}
void
CirclingComputer::Turning(CirclingInfo &circling_info,
const MoreData &basic, const MoreData &last_basic,
const DerivedInfo &calculated,
const DerivedInfo &last_calculated,
const ComputerSettings &settings_computer)
{
// You can't be circling unless you're flying
if (!calculated.flight.flying || !basic.HasTimeAdvancedSince(last_basic))
return;
// JMW limit rate to 50 deg per second otherwise a big spike
// will cause spurious lock on circling for a long time
fixed turn_rate = max(fixed(-50), min(fixed(50), calculated.turn_rate));
// Make the turn rate more smooth using the LowPassFilter
turn_rate = LowPassFilter(last_calculated.turn_rate_smoothed,
turn_rate, fixed(0.3));
circling_info.turn_rate_smoothed = turn_rate;
circling_info.turning = fabs(turn_rate) >= min_turn_rate;
// Force cruise or climb mode if external device says so
bool force_cruise = false;
bool force_circling = false;
if (settings_computer.external_trigger_cruise_enabled && !basic.gps.replay) {
switch (basic.switch_state.flight_mode) {
case SwitchInfo::FlightMode::UNKNOWN:
force_circling = false;
force_cruise = false;
break;
case SwitchInfo::FlightMode::CIRCLING:
force_circling = true;
force_cruise = false;
break;
case SwitchInfo::FlightMode::CRUISE:
force_circling = false;
force_cruise = true;
break;
}
}
switch (calculated.turn_mode) {
case CirclingMode::CRUISE:
// If (in cruise mode and beginning of circling detected)
if (circling_info.turning || force_circling) {
// Remember the start values of the turn
circling_info.turn_start_time = basic.time;
circling_info.turn_start_location = basic.location;
circling_info.turn_start_altitude = basic.nav_altitude;
circling_info.turn_start_energy_height = basic.energy_height;
circling_info.turn_mode = CirclingMode::POSSIBLE_CLIMB;
}
if (!force_circling)
break;
case CirclingMode::POSSIBLE_CLIMB:
if (force_cruise) {
circling_info.turn_mode = CirclingMode::CRUISE;
break;
}
if (circling_info.turning || force_circling) {
if (((basic.time - calculated.turn_start_time) > cruise_climb_switch)
|| force_circling) {
// yes, we are certain now that we are circling
circling_info.circling = true;
// JMW Transition to climb
circling_info.turn_mode = CirclingMode::CLIMB;
// Remember the start values of the climbing period
circling_info.climb_start_location = circling_info.turn_start_location;
circling_info.climb_start_altitude = circling_info.turn_start_altitude
+ circling_info.turn_start_energy_height;
circling_info.climb_start_time = circling_info.turn_start_time;
}
} else {
// nope, not turning, so go back to cruise
circling_info.turn_mode = CirclingMode::CRUISE;
}
break;
case CirclingMode::CLIMB:
if (!circling_info.turning || force_cruise) {
// Remember the end values of the turn
circling_info.turn_start_time = basic.time;
circling_info.turn_start_location = basic.location;
circling_info.turn_start_altitude = basic.nav_altitude;
circling_info.turn_start_energy_height = basic.energy_height;
// JMW Transition to cruise, due to not properly turning
circling_info.turn_mode = CirclingMode::POSSIBLE_CRUISE;
}
if (!force_cruise)
break;
case CirclingMode::POSSIBLE_CRUISE:
if (force_circling) {
circling_info.turn_mode = CirclingMode::CLIMB;
break;
}
if (!circling_info.turning || force_cruise) {
if (((basic.time - circling_info.turn_start_time) > climb_cruise_switch)
|| force_cruise) {
// yes, we are certain now that we are cruising again
circling_info.circling = false;
// Transition to cruise
circling_info.turn_mode = CirclingMode::CRUISE;
circling_info.cruise_start_location = circling_info.turn_start_location;
circling_info.cruise_start_altitude = circling_info.turn_start_altitude;
circling_info.cruise_start_time = circling_info.turn_start_time;
}
} else {
// nope, we are circling again
// JMW Transition back to climb, because we are turning again
circling_info.turn_mode = CirclingMode::CLIMB;
}
break;
}
}
