RealT BilinearHeightMap::height (const Vec2 &_at) {
// Offset _at so our sample space is not misaligned because of filtering:
Vec2 at(_at);
at -= 0.5;
//return _input->height(at);
RealT s[4];
Vec2 t = at.truncate(), j;
Vec2 jitter[] = {
Vec2{0, 0}, Vec2{0, 1}, Vec2{1, 0}, Vec2{1, 1}
};
for (std::size_t i = 0; i < 4; ++i) {
s[i] = _input->height((t + jitter[i]));
}
Vec2 f = at.fraction();
//std::cout << "Frac: " << f << std::endl;
RealT a = linear_interpolate(f[X], s[0], s[1]);
RealT b = linear_interpolate(f[X], s[2], s[3]);
RealT c = linear_interpolate(f[Y], a, b);
return c;
}
static int interpolate_single_lut(struct single_row_lut *lut, int x)
{
int i, result;
if (x < lut->x[0]) {
pr_debug("x %d less than known range return y = %d lut = %pS\n",
x, lut->y[0], lut);
return lut->y[0];
}
if (x > lut->x[lut->cols - 1]) {
pr_debug("x %d more than known range return y = %d lut = %pS\n",
x, lut->y[lut->cols - 1], lut);
return lut->y[lut->cols - 1];
}
for (i = 0; i < lut->cols; i++)
if (x <= lut->x[i])
break;
if (x == lut->x[i]) {
result = lut->y[i];
} else {
result = linear_interpolate(
lut->y[i - 1],
lut->x[i - 1],
lut->y[i],
lut->x[i],
x);
}
return result;
}
// update mainsail's desired angle based on wind speed and direction and desired speed (in m/s)
void Rover::sailboat_update_mainsail(float desired_speed)
{
if (!g2.motors.has_sail()) {
return;
}
// relax sail if desired speed is zero
if (!is_positive(desired_speed)) {
g2.motors.set_mainsail(100.0f);
return;
}
// + is wind over starboard side, - is wind over port side, but as the sails are sheeted the same on each side it makes no difference so take abs
float wind_dir_apparent = fabsf(g2.windvane.get_apparent_wind_direction_rad());
wind_dir_apparent = degrees(wind_dir_apparent);
// set the main sail to the ideal angle to the wind
float mainsail_angle = wind_dir_apparent - g2.sail_angle_ideal;
// make sure between allowable range
mainsail_angle = constrain_float(mainsail_angle, g2.sail_angle_min, g2.sail_angle_max);
// linear interpolate mainsail value (0 to 100) from wind angle mainsail_angle
float mainsail = linear_interpolate(0.0f, 100.0f, mainsail_angle, g2.sail_angle_min, g2.sail_angle_max);
// use PID controller to sheet out
const float pid_offset = g2.attitude_control.get_sail_out_from_heel(radians(g2.sail_heel_angle_max), G_Dt) * 100.0f;
mainsail = constrain_float((mainsail+pid_offset), 0.0f ,100.0f);
g2.motors.set_mainsail(mainsail);
}
void QuadraticSpline::Get(DBL p, EXPRESS& v)
{
if (SplineEntries.size() == 1)
memcpy(&v, &SplineEntries.front().vec, sizeof(EXPRESS));
else
{
/* Find which spline segment we're in. i is the control point at the end of the segment */
SplineEntryList::size_type i = findt(this, p);
for(int k=0; k<5; k++)
{
/* If outside the spline range, return the first or last point */
if(i == 0)
v[k] = SplineEntries.front().vec[k];
else if(i >= SplineEntries.size())
v[k] = SplineEntries.back().vec[k];
/* If not enough points, reduce order */
else if(SplineEntries.size() == 2)
v[k] = linear_interpolate(SplineEntries, i-1, k, p);
else
{
/* Normal case: between the second and last points */
if(i > 1)
v[k] = quadratic_interpolate(SplineEntries, i-1, k, p);
else /* Special case: between first and second points */
v[k] = quadratic_interpolate(SplineEntries, i, k, p);
}
}
}
}
void Spline::setup (const float * fun)
{
LOG1("Spline setting up forward mapping");
for (size_t dom_i = 0; dom_i < m_size_in; ++dom_i) {
float rng_t = fun[dom_i];
float rng_i = rng_t * m_size_out - 0.5;
linear_interpolate(
rng_i,
m_size_out,
m_i0[dom_i],
m_w0[dom_i],
m_w1[dom_i]);
}
LOG1("Spline setting up backward mapping");
m_scale_bwd.zero();
for (size_t dom_i = 0; dom_i < m_size_in; ++dom_i) {
size_t rng_i0 = m_i0[dom_i];
m_scale_bwd[rng_i0 ] += m_w0[dom_i];
m_scale_bwd[rng_i0+1] += m_w1[dom_i];
}
for (size_t rng_i = 0; rng_i < m_size_out; ++rng_i) {
safely_invert(m_scale_bwd[rng_i], m_tolerance);
}
LOG1("spline inverse scale bounds = ["
<< min(m_scale_bwd) << ", "
<< max(m_scale_bwd) << "]");
}
/**
* adc_to_temp - converts the ADC code to temperature in C
* @direct: true if ths channel is direct index
* @adc_val: the adc_val that needs to be converted
* @tp: temperature return value
*
* Can sleep
*/
static int adc_to_temp(int direct, uint16_t adc_val, unsigned long *tp)
{
int indx;
/* Direct conversion for msic die temperature */
if (direct) {
if (is_valid_adc(adc_val, MSIC_DIE_ADC_MIN, MSIC_DIE_ADC_MAX)) {
*tp = TO_MSIC_DIE_TEMP(adc_val);
return 0;
}
return -ERANGE;
}
indx = find_adc_code(adc_val);
if (indx < 0)
return -ERANGE;
if (adc_code[0][indx] == adc_val) {
/* Convert temperature in celsius to milli degree celsius */
*tp = adc_code[1][indx] * 1000;
return 0;
}
/*
* The ADC code is in between two values directly defined in the
* table. So, do linear interpolation to calculate the temperature.
*/
*tp = linear_interpolate(indx, adc_val);
return 0;
}
T operator()(Storage const& buffer, T index) const
{
auto y1 = buffer[std::size_t(index)];
auto y2 = buffer[std::size_t(index) + 1];
auto mu = index - std::floor(index);
return linear_interpolate(y1, y2, mu);
}
RealT PerlinNoise::sample(const Vec3 &v) const {
Vec3 t = v.fraction();
Vector<3, std::size_t> o = v.truncate();
RealT d[8];
/* Get noise at 8 lattice points */
for (std::size_t lz = 0; lz < 2; ++lz) {
d[0 + lz*4] = lattice_noise(o[X], o[Y], o[Z] + lz);
d[1 + lz*4] = lattice_noise(o[X], o[Y] + 1, o[Z] + lz);
d[2 + lz*4] = lattice_noise(o[X] + 1, o[Y] + 1, o[Z] + lz);
d[3 + lz*4] = lattice_noise(o[X] + 1, o[Y], o[Z] + lz);
}
RealT x0, x1, x2, x3, y0, y1;
x0 = linear_interpolate(t[X], d[0], d[3]);
x1 = linear_interpolate(t[X], d[1], d[2]);
x2 = linear_interpolate(t[X], d[4], d[7]);
x3 = linear_interpolate(t[X], d[5], d[6]);
y0 = linear_interpolate(t[Y], x0, x1);
y1 = linear_interpolate(t[Y], x2, x3);
RealT result = linear_interpolate(t[Z], y0, y1);
return result;
}
/* Get the interpolated value at p from the material grid g.
p.x/p.y/p.z must be in (-1,2). This involves a bit more Guile
internals than I would like, ripped out of scm_uniform_vector_ref
in libguile/unif.c, but the alternative is a lot of overhead given
that we know for certain that the material grid is a uniform 3d
array of g->size double-precision values. */
real material_grid_val(vector3 p, const material_grid *g)
{
real val;
CHECK(SCM_ARRAYP(g->matgrid), "bug: matgrid is not an array");
val = linear_interpolate(p.x, p.y, p.z, material_grid_array(g),
g->size.x, g->size.y, g->size.z, 1);
material_grid_array_release(g);
return val;
}
/**
* This routine returns the smoothed interpolated noise for an x and y, using
* the values from the surrounding positions.
*/
static double interpolated_noise(const double x, const double y, const int prime)
{
const int integer_X = (int)x;
const int integer_Y = (int)y;
const double fractional_X = x - (double)integer_X;
const double fractional_Y = y - (double)integer_Y;
const double v1 = int_noise(integer_X, integer_Y, prime);
const double v2 = int_noise(integer_X + 1, integer_Y, prime);
const double v3 = int_noise(integer_X, integer_Y + 1, prime);
const double v4 = int_noise(integer_X + 1, integer_Y + 1, prime);
const double i1 = linear_interpolate(v1, v2, fractional_X);
const double i2 = linear_interpolate(v3, v4, fractional_X);
return linear_interpolate(i1, i2, fractional_Y);
}
double compute_effective_interest_rate( int days )
{
/* for performance reasons, we hand-code the table in code, rather than use data-driven table of interest rates */
if (days <= 0) return 0; /* no interest for no time */
if (days == 1) return 10;
if (days < 30) return linear_interpolate( 10, 5, 30, days);
if (days == 30) return 5;
if (days < 60) return linear_interpolate( 5, 4, 30, days - 30);
if (days == 60) return 4;
if (days < 90) return linear_interpolate( 4, 2, 30, days - 60);
if (days == 90) return 2;
/* NOTE WELL: the table above stops at "210 days", yet maintains the same interest rate of 2. */
/* we choose to interpret the instructions as 90 days through 210 days, or longer */
return 2;
}
bool AP_Landing_Deepstall::override_servos(void)
{
if (!(stage == DEEPSTALL_STAGE_LAND)) {
return false;
}
SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
if (elevator == nullptr) {
// deepstalls are impossible without these channels, abort the process
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,
"Deepstall: Unable to find the elevator channels");
request_go_around();
return false;
}
// calculate the progress on slewing the elevator
float slew_progress = 1.0f;
if (slew_speed > 0) {
slew_progress = (AP_HAL::millis() - stall_entry_time) / (100.0f * slew_speed);
slew_progress = constrain_float (slew_progress, 0.0f, 1.0f);
}
// mix the elevator to the correct value
elevator->set_output_pwm(linear_interpolate(initial_elevator_pwm, elevator_pwm,
slew_progress, 0.0f, 1.0f));
// use the current airspeed to dictate the travel limits
float airspeed;
landing.ahrs.airspeed_estimate(&airspeed);
// only allow the deepstall steering controller to run below the handoff airspeed
if (slew_progress >= 1.0f || airspeed <= handoff_airspeed) {
// run the steering conntroller
float pid = update_steering();
float travel_limit = constrain_float((handoff_airspeed - airspeed) /
(handoff_airspeed - handoff_lower_limit_airspeed) *
0.5f + 0.5f,
0.5f, 1.0f);
float output = constrain_float(pid, -travel_limit, travel_limit);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, output*4500);
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input, output*4500);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, output*4500);
} else {
// allow the normal servo control of the channel
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron_with_input,
SRV_Channels::get_output_scaled(SRV_Channel::k_aileron));
}
// hand off rudder control to deepstall controlled
return true;
}
static OBJ_PTR
c_create_colormap(FM *p, bool rgb_flag, int length, int num_pts, double *ps,
double *c1s, double *c2s, double *c3s, int *ierr)
{
int i;
if (ps[0] != 0.0 || ps[num_pts-1] != 1.0) {
RAISE_ERROR("Sorry: first control point for create colormap must be "
"at 0.0 and last must be at 1.0", ierr);
RETURN_NIL;
}
for (i = 1; i < num_pts; i++) {
if (ps[i-1] > ps[i]) {
RAISE_ERROR("Sorry: control points for create colormap must be "
"increasing from 0 to 1", ierr);
RETURN_NIL;
}
}
int j, buff_len = length * 3, hival = length - 1;
unsigned char *buff;
buff = ALLOC_N_unsigned_char(buff_len);
for (j = 0, i = 0; j < length; j++) {
double x = j; x /= (length-1);
double c1, c2, c3, r, g, b;
c1 = linear_interpolate(num_pts, ps, c1s, x);
c2 = linear_interpolate(num_pts, ps, c2s, x);
c3 = linear_interpolate(num_pts, ps, c3s, x);
if (rgb_flag) { r = c1; g = c2; b = c3; }
else convert_hls_to_rgb(c1, c2, c3, &r, &g, &b);
buff[i++] = ROUND(hival * r);
buff[i++] = ROUND(hival * g);
buff[i++] = ROUND(hival * b);
}
OBJ_PTR lookup = String_New((char *)buff, buff_len);
free(buff);
OBJ_PTR result = Array_New(2);
Array_Store(result, 0, Integer_New(hival), ierr);
Array_Store(result, 1, lookup, ierr);
if (*ierr != 0) RETURN_NIL;
return result;
}
void load_material_gridB(material_grid g, string filename, vector3 supercell)
{
if (mpi_is_master()) {
matrixio_id file_id;
int dims[3] = {1,1,1}, rank = 3;
int nx, ny, nz, ix,iy,iz;
double *data, *grid;
double sx, sy, sz;
file_id = matrixio_open_serial(filename, 1);
data = matrixio_read_real_data(file_id, "data", &rank,dims, 0,0,0,0);
CHECK(data, "couldn't find dataset in material grid file");
matrixio_close(file_id);
nx = g.size.x;
ny = g.size.y;
nz = g.size.z;
sx = supercell.x > 0 ? supercell.x : 1;
sy = supercell.y > 0 ? supercell.y : 1;
sz = supercell.z > 0 ? supercell.z : 1;
grid = material_grid_array(&g);
for (ix = 0; ix < nx; ++ix)
for (iy = 0; iy < ny; ++iy)
for (iz = 0; iz < nz; ++iz) {
double dummy;
double x,y,z, val;
x = modf((ix + 0.5) * (sx / nx), &dummy);
y = modf((iy + 0.5) * (sy / ny), &dummy);
z = modf((iz + 0.5) * (sz / nz), &dummy);
val = linear_interpolate(x,y,z, data,
dims[0],dims[1],dims[2], 1);
if (val > 1) val = 1;
else if (val < 0) val = 0;
grid[(ix * ny + iy) * nz + iz] = val;
}
material_grid_array_release(&g);
free(data);
}
synchronize_material_grid(&g);
}
virtual void test( ObjectiveFn& testCase, long long maxTimeInMilliseconds ) {
std::vector< bool > incumbent = random_bitvector( testCase.getNumGenes() );
double lastValue = testCase.value( incumbent );
const long numEvaluations = testCase.getRemainingEvaluations();
for( int i=0; i<numEvaluations; ++i ){
const std::vector< bool > incoming = randomHamming1Neighbour( incumbent );
const double value = testCase.value( incoming );
// linear annealing schedule...
const double temperature = linear_interpolate(
i, 0, numEvaluations - 1,
saScheduleUpperBound, saScheduleLowerBound );
if( SAAccept( lastValue, value, temperature ) ) {
incumbent = incoming;
lastValue = value;
}
}
}
void LinearSpline::Get(DBL p, EXPRESS& v)
{
if (SplineEntries.size() == 1)
memcpy(&v, &SplineEntries.front().vec, sizeof(EXPRESS));
else
{
/* Find which spline segment we're in. i is the control point at the end of the segment */
SplineEntryList::size_type i = findt(this, p);
for(int k=0; k<5; k++)
{
/* If outside spline range, return first or last point */
if(i == 0)
v[k] = SplineEntries.front().vec[k];
else if(i >= SplineEntries.size())
v[k] = SplineEntries.back().vec[k];
/* Else, normal case */
else
v[k] = linear_interpolate(SplineEntries, i-1, k, p);
}
}
}
// return the remaining battery capacity according to the battery voltage
int bat_percentage(int vbat)
{
if (vbat >= vbat_alkaline[0])
{
return 100;
}
else if (vbat <= vbat_alkaline[10])
{
return 0;
}
else // linear interpolation between the array values
{
int i = 0;
while (vbat < vbat_alkaline[i])
{
i++;
}
return linear_interpolate(vbat, vbat_alkaline[i], vbat_alkaline[i - 1], 100 - i * 10, 100 - (i - 1) * 10);
}
}
boost::tuple<double, int, int>
detect_slope(boost::gil::rgb8_image_t const& source)
{
boost::gil::rgb8c_view_t::xy_locator src_loc = boost::gil::const_view(source).xy_at(0, 0);
int const w = source.width();
int const h = source.height();
int x = -1;
int y = source.height() - 1;
for (int i = y + 1; x < 0 && i > 0; --i, --y) {
for (int xx = 0; xx < source.width(); ++xx) {
if (is_border_mark(src_loc(xx, y)) &&
((xx > 0 && y > 0 && is_border_mark(src_loc(xx-1, y-1))) ||
( y > 0 && is_border_mark(src_loc(xx, y-1))) ||
(xx+1 < w && y > 0 && is_border_mark(src_loc(xx+1, y-1))))) {
std::string msg = (boost::format("(%d, %d)") % xx % y).str();
::MessageBox(NULL, multi_to_wide(msg).c_str(), L"slope-bottom was detected", MB_OK);
while (is_border_mark(src_loc(xx+1, y))) ++xx;
x = xx;
break;
}
}
}
std::vector<boost::gil::point2<int> > points;
if (x <= source.width() / 2) {
collect_slope_points_right(source, x, y, std::back_inserter(points));
}
else {
collect_slope_points_left(source, x, y, std::back_inserter(points));
}
double slope, intercept;
boost::tie(slope, intercept) = linear_interpolate(points.begin(), points.end());
return boost::make_tuple(slope, x, y);
}
int interpolate_scalingfactor(struct sf_lut *sf_lut, int row_entry, int pc)
{
int i, scalefactorrow1, scalefactorrow2, scalefactor, rows, cols;
int row1 = 0;
int row2 = 0;
/*
* sf table could be null when no battery aging data is available, in
* that case return 100%
*/
if (!sf_lut)
return 100;
rows = sf_lut->rows;
cols = sf_lut->cols;
if (pc > sf_lut->percent[0]) {
pr_debug("pc %d greater than known pc ranges for sfd\n", pc);
row1 = 0;
row2 = 0;
}
if (pc < sf_lut->percent[rows - 1]) {
pr_debug("pc %d less than known pc ranges for sf\n", pc);
row1 = rows - 1;
row2 = rows - 1;
}
for (i = 0; i < rows; i++) {
if (pc == sf_lut->percent[i]) {
row1 = i;
row2 = i;
break;
}
if (pc > sf_lut->percent[i]) {
row1 = i - 1;
row2 = i;
break;
}
}
if (row_entry < sf_lut->row_entries[0])
row_entry = sf_lut->row_entries[0];
if (row_entry > sf_lut->row_entries[cols - 1])
row_entry = sf_lut->row_entries[cols - 1];
for (i = 0; i < cols; i++)
if (row_entry <= sf_lut->row_entries[i])
break;
if (row_entry == sf_lut->row_entries[i]) {
scalefactor = linear_interpolate(
sf_lut->sf[row1][i],
sf_lut->percent[row1],
sf_lut->sf[row2][i],
sf_lut->percent[row2],
pc);
return scalefactor;
}
scalefactorrow1 = linear_interpolate(
sf_lut->sf[row1][i - 1],
sf_lut->row_entries[i - 1],
sf_lut->sf[row1][i],
sf_lut->row_entries[i],
row_entry);
scalefactorrow2 = linear_interpolate(
sf_lut->sf[row2][i - 1],
sf_lut->row_entries[i - 1],
sf_lut->sf[row2][i],
sf_lut->row_entries[i],
row_entry);
scalefactor = linear_interpolate(
scalefactorrow1,
sf_lut->percent[row1],
scalefactorrow2,
sf_lut->percent[row2],
pc);
return scalefactor;
}
//#define UPDATEDEBUG //for hard to track down bugs
void GameStarSystem::Draw(bool DrawCockpit)
{
double starttime=queryTime();
GFXEnable (DEPTHTEST);
GFXEnable (DEPTHWRITE);
saved_interpolation_blend_factor=interpolation_blend_factor = (1./PHY_NUM)*((PHY_NUM*time)/SIMULATION_ATOM+current_stage);
GFXColor4f(1,1,1,1);
if (DrawCockpit) {
AnimatedTexture::UpdateAllFrame();
}
for (unsigned int i=0;i<contterrains.size();++i) {
contterrains[i]->AdjustTerrain(this);
}
Unit * par;
bool alreadysetviewport=false;
if ((par=_Universe->AccessCockpit()->GetParent())==NULL) {
_Universe->AccessCamera()->UpdateGFX (GFXTRUE);
}
else {
if (!par->isSubUnit()) {
//now we can assume world is topps
par-> cumulative_transformation = linear_interpolate (par->prev_physical_state,par->curr_physical_state,interpolation_blend_factor);
Unit * targ = par->Target();
if (targ&&!targ->isSubUnit()) {
targ-> cumulative_transformation = linear_interpolate (targ->prev_physical_state,targ->curr_physical_state,interpolation_blend_factor);
}
_Universe->AccessCockpit()->SetupViewPort(true);
alreadysetviewport=true;
}
}
double setupdrawtime=queryTime();
{
cam_setup_phase=true;
//int numships=0;
Unit * saveparent=_Universe->AccessCockpit()->GetSaveParent();
Unit * targ=NULL;
if (saveparent) {
targ=saveparent->Target();
}
//Array containing the two interesting units, so as not to have to copy-paste code
Unit * camunits[2]={saveparent,targ};
float backup=SIMULATION_ATOM;
unsigned int cur_sim_frame = _Universe->activeStarSystem()->getCurrentSimFrame();
for(int i=0;i<2;++i) {
Unit *unit=camunits[i];
// Make sure unit is not null;
if(unit&&!unit->isSubUnit()) {
interpolation_blend_factor=calc_blend_factor(saved_interpolation_blend_factor,unit->sim_atom_multiplier,unit->cur_sim_queue_slot,cur_sim_frame);
SIMULATION_ATOM = backup*unit->sim_atom_multiplier;
((GameUnit<Unit> *)unit)->GameUnit<Unit>::Draw();
}
}
interpolation_blend_factor=saved_interpolation_blend_factor;
SIMULATION_ATOM=backup;
//printf("Number of insystem ships: %d (%d FPS)\n",numships,(int)(1.f/GetElapsedTime()));
///this is the final, smoothly calculated cam
_Universe->AccessCockpit()->SetupViewPort(true);
cam_setup_phase=false;
}
setupdrawtime=queryTime()-setupdrawtime;
GFXDisable (LIGHTING);
bg->Draw();
double drawtime=queryTime();
double maxdrawtime=0;
//Ballpark estimate of when an object of configurable size first becomes one pixel
QVector drawstartpos=_Universe->AccessCamera()->GetPosition();
Collidable key_iterator(0,1,drawstartpos);
UnitWithinRangeOfPosition<UnitDrawer> drawer(game_options.precull_dist,0,key_iterator);
//Need to draw really big stuff (i.e. planets, deathstars, and other mind-bogglingly big things that shouldn't be culled despited extreme distance
Unit* unit;
if ((drawer.action.parent=_Universe->AccessCockpit()->GetParent())!=NULL) {
drawer.action.parenttarget=drawer.action.parent->Target();
}
for(un_iter iter=this->GravitationalUnits.createIterator();(unit=*iter)!=NULL;++iter) {
float distance = (drawstartpos-unit->Position()).Magnitude()-unit->rSize();
if(distance < game_options.precull_dist) {
drawer.action.grav_acquire(unit);
}
else {
drawer.action.draw(unit);
}
}
// Need to get iterator to approx camera position
CollideMap::iterator parent=collidemap[Unit::UNIT_ONLY]->lower_bound(key_iterator);
findObjectsFromPosition(this->collidemap[Unit::UNIT_ONLY],parent,&drawer,drawstartpos,0,true);
drawer.action.drawParents();//draw units targeted by camera
//FIXME maybe we could do bolts & units instead of unit only--and avoid bolt drawing step
#if 0
for (unsigned int sim_counter=0;sim_counter<=SIM_QUEUE_SIZE;++sim_counter) {
double tmp=queryTime();
Unit *unit;
UnitCollection::UnitIterator iter = physics_buffer[sim_counter].createIterator();
float backup=SIMULATION_ATOM;
unsigned int cur_sim_frame = _Universe->activeStarSystem()->getCurrentSimFrame();
while((unit = iter.current())!=NULL) {
interpolation_blend_factor=calc_blend_factor(saved_interpolation_blend_factor,unit->sim_atom_multiplier,unit->cur_sim_queue_slot,cur_sim_frame);
//if (par&&par->Target()==unit) {
//printf ("i:%f s:%f m:%d c:%d l:%d\n",interpolation_blend_factor,saved_interpolation_blend_factor,unit->sim_atom_multiplier,sim_counter,current_sim_location);
//}
SIMULATION_ATOM = backup*unit->sim_atom_multiplier;
((GameUnit<Unit> *)unit)->Draw();
iter.advance();
}
interpolation_blend_factor=saved_interpolation_blend_factor;
SIMULATION_ATOM=backup;
tmp=queryTime()-tmp;
if (tmp>maxdrawtime)maxdrawtime=tmp;
}
#endif
drawtime=queryTime()-drawtime;
WarpTrailDraw();
GFXFogMode (FOG_OFF);
GFXColor tmpcol (0,0,0,1);
GFXGetLightContextAmbient(tmpcol);
double processmesh=queryTime();
if (!game_options.draw_near_stars_in_front_of_planets) stars->Draw();
Mesh::ProcessZFarMeshes();
if (game_options.draw_near_stars_in_front_of_planets) stars->Draw();
GFXEnable (DEPTHTEST);
GFXEnable (DEPTHWRITE);
//need to wait for lights to finish
GamePlanet::ProcessTerrains();
Terrain::RenderAll();
Mesh::ProcessUndrawnMeshes(true);
processmesh=queryTime()-processmesh;
Nebula * neb;
Matrix ident;
Identity(ident);
//Atmosphere::ProcessDrawQueue();
GFXPopGlobalEffects();
GFXLightContextAmbient(tmpcol);
if ((neb = _Universe->AccessCamera()->GetNebula())) {
neb->SetFogState();
}
Beam::ProcessDrawQueue();
Bolt::Draw();
// if (_Universe->AccessCamera()->GetNebula()!=NULL)
GFXFogMode (FOG_OFF);
Animation::ProcessDrawQueue();
Halo::ProcessDrawQueue();
particleTrail.DrawAndUpdate();
GameStarSystem::DrawJumpStars();
ConditionalCursorDraw(false);
// static bool doInputDFA = XMLSupport::parse_bool (vs_config->getVariable ("graphics","MouseCursor","false"));
if (DrawCockpit) {
_Universe->AccessCockpit()->Draw();
// if (doInputDFA) {
// GFXHudMode (true);
// systemInputDFA->Draw();
// GFXHudMode (false);
// }
}
double fintime=queryTime()-starttime;
if (debugPerformance()) {
printf("draw: %f setup %f units %f maxunit %f processmesh %f ",fintime,setupdrawtime,drawtime,maxdrawtime,processmesh);
}
}
DBL Get_Spline_Val(SPLINE *sp, DBL p, EXPRESS v, int *Terms)
{
int i, k;
int last;
SPLINE_ENTRY * se;
*Terms = sp->Terms;
if(!sp->Coeffs_Computed)
{
switch(sp->Type)
{
case NATURAL_SPLINE:
Precompute_Cubic_Coeffs(sp);
break;
default:
break;
}
}
// check if the value is in the cache
if((sp->Cache_Point == p) && (sp->Cache_Type == sp->Type))
{
if(sp->Cache_Valid == true) // doing this here is more efficient as it is rarely false [trf]
{
Assign_Express(v, sp->Cache_Data);
return sp->Cache_Data[0];
}
}
// init some cache data
sp->Cache_Valid = false;
sp->Cache_Type = sp->Type;
sp->Cache_Point = p;
last = sp->Number_Of_Entries-1;
se = sp->SplineEntries;
if(last == 0)
{/* if only one entry then return this */
for(k=0; k<5; k++)
v[k] = se[0].vec[k];
return se[0].vec[0];
}
/* Find which spline segment we're in. i is the control point at the end of the segment */
i = findt(sp, p);
switch(sp->Type)
{
case LINEAR_SPLINE:
for(k=0; k<5; k++)
{
/* If outside spline range, return first or last point */
if(i == 0)
v[k] = se[0].vec[k];
else if(i > last)
v[k] = se[last].vec[k];
/* Else, normal case */
else
v[k] = linear_interpolate(se, i-1, k, p);
}
break;
case QUADRATIC_SPLINE:
for(k=0; k<5; k++)
{
/* If outside the spline range, return the first or last point */
if(i == 0)
v[k] = se[0].vec[k];
else if(i > last)
v[k] = se[last].vec[k];
/* If not enough points, reduce order */
else if(last == 1)
v[k] = linear_interpolate(se, i-1, k, p);
/* Normal case: between the second and last points */
else if(i > 1)
{
v[k] = quadratic_interpolate(se, i-1, k, p);
}
else /* Special case: between first and second points */
{
v[k] = quadratic_interpolate(se, i, k, p);
}
}
break;
case NATURAL_SPLINE:
for(k=0; k<5; k++)
{
/* If outside the spline range, return the first or last point */
if(i == 0)
v[k] = se[0].vec[k];
else if(i > last)
v[k] = se[last].vec[k];
/* Else, normal case. cubic_interpolate can handle the case of not enough points */
else
v[k] = natural_interpolate(se, i-1, k, p);
}
break;
case CATMULL_ROM_SPLINE:
for(k=0; k<5; k++)
{
/* If only two points, return their average */
if(last == 1)
v[k] = (se[0].vec[k] + se[1].vec[k])/2.0;
/* Catmull-Rom: If only three points, return the second one */
/* Catmull-Rom: Can't interpolate before second point or after next-to-last */
else if(i < 2)
v[k] = se[1].vec[k];
else if(i >= last)
v[k] = se[last-1].vec[k];
/* Else, normal case */
else
v[k] = catmull_rom_interpolate(se, i-1, k, p);
}
break;
default:
Error("Unknown spline type %d found.\n", sp->Type);
}
// put data in cache
Assign_Express(sp->Cache_Data, v);
sp->Cache_Valid = true;
return v[0];
}
void Copter::Mode::land_run_horizontal_control()
{
LowPassFilterFloat &rc_throttle_control_in_filter = copter.rc_throttle_control_in_filter;
AP_Vehicle::MultiCopter &aparm = copter.aparm;
float target_roll = 0.0f;
float target_pitch = 0.0f;
float target_yaw_rate = 0;
// relax loiter target if we might be landed
if (ap.land_complete_maybe) {
loiter_nav->soften_for_landing();
}
// process pilot inputs
if (!copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
copter.Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) {
set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
}
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
// record if pilot has overriden roll or pitch
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
ap.land_repo_active = true;
}
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
#if PRECISION_LANDING == ENABLED
AC_PrecLand &precland = copter.precland;
bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired();
// run precision landing
if (doing_precision_landing) {
Vector2f target_pos, target_vel_rel;
if (!precland.get_target_position_cm(target_pos)) {
target_pos.x = inertial_nav.get_position().x;
target_pos.y = inertial_nav.get_position().y;
}
if (!precland.get_target_velocity_relative_cms(target_vel_rel)) {
target_vel_rel.x = -inertial_nav.get_velocity().x;
target_vel_rel.y = -inertial_nav.get_velocity().y;
}
pos_control->set_xy_target(target_pos.x, target_pos.y);
pos_control->override_vehicle_velocity_xy(-target_vel_rel);
}
#endif
// process roll, pitch inputs
loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
// run loiter controller
loiter_nav->update(ekfGndSpdLimit, ekfNavVelGainScaler);
int32_t nav_roll = loiter_nav->get_roll();
int32_t nav_pitch = loiter_nav->get_pitch();
if (g2.wp_navalt_min > 0) {
// user has requested an altitude below which navigation
// attitude is limited. This is used to prevent commanded roll
// over on landing, which particularly affects helicopters if
// there is any position estimate drift after touchdown. We
// limit attitude to 7 degrees below this limit and linearly
// interpolate for 1m above that
const int alt_above_ground = get_alt_above_ground();
float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground,
g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U);
float total_angle_cd = norm(nav_roll, nav_pitch);
if (total_angle_cd > attitude_limit_cd) {
float ratio = attitude_limit_cd / total_angle_cd;
nav_roll *= ratio;
nav_pitch *= ratio;
// tell position controller we are applying an external limit
pos_control->set_limit_accel_xy();
}
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate);
}
static gboolean
spectrogram_draw (GtkWidget *widget, cairo_t *cr, gpointer user_data) {
w_spectrogram_t *w = user_data;
GtkAllocation a;
gtk_widget_get_allocation (widget, &a);
if (!w->samples || a.height < 1) {
return FALSE;
}
int width, height;
width = a.width;
height = a.height;
int ratio = ftoi (FFT_SIZE/(a.height*2));
ratio = CLAMP (ratio,0,1023);
if (deadbeef->get_output ()->state () == OUTPUT_STATE_PLAYING) {
do_fft (w);
float log_scale = (log2f(w->samplerate/2)-log2f(25.))/(a.height);
float freq_res = w->samplerate / FFT_SIZE;
if (a.height != w->height) {
w->height = MIN (a.height, MAX_HEIGHT);
for (int i = 0; i < w->height; i++) {
w->log_index[i] = ftoi (powf(2.,((float)i) * log_scale + log2f(25.)) / freq_res);
if (i > 0 && w->log_index[i-1] == w->log_index [i]) {
w->low_res_end = i;
}
}
}
}
// start drawing
if (!w->surf || cairo_image_surface_get_width (w->surf) != a.width || cairo_image_surface_get_height (w->surf) != a.height) {
if (w->surf) {
cairo_surface_destroy (w->surf);
w->surf = NULL;
}
w->surf = cairo_image_surface_create (CAIRO_FORMAT_RGB24, a.width, a.height);
}
cairo_surface_flush (w->surf);
unsigned char *data = cairo_image_surface_get_data (w->surf);
if (!data) {
return FALSE;
}
int stride = cairo_image_surface_get_stride (w->surf);
if (deadbeef->get_output ()->state () == OUTPUT_STATE_PLAYING) {
for (int i = 0; i < a.height; i++) {
// scrolling: move line i 1px to the left
memmove (data + (i*stride), data + sizeof (uint32_t) + (i*stride), stride - sizeof (uint32_t));
}
for (int i = 0; i < a.height; i++)
{
float f = 1.0;
int index0, index1;
int bin0, bin1, bin2;
if (CONFIG_LOG_SCALE) {
bin0 = w->log_index[CLAMP (i-1,0,height-1)];
bin1 = w->log_index[i];
bin2 = w->log_index[CLAMP (i+1,0,height-1)];
}
else {
bin0 = (i-1) * ratio;
bin1 = i * ratio;
bin2 = (i+1) * ratio;
}
index0 = bin0 + ftoi ((bin1 - bin0)/2.f);
if (index0 == bin0) index0 = bin1;
index1 = bin1 + ftoi ((bin2 - bin1)/2.f);
if (index1 == bin2) index1 = bin1;
index0 = CLAMP (index0,0,FFT_SIZE/2-1);
index1 = CLAMP (index1,0,FFT_SIZE/2-1);
f = spectrogram_get_value (w, index0, index1);
float x = 10 * log10f (f);
// interpolate
if (i <= w->low_res_end && CONFIG_LOG_SCALE) {
int j = 0;
// find index of next value
while (i+j < height && w->log_index[i+j] == w->log_index[i]) {
j++;
}
float v0 = x;
float v1 = w->data[w->log_index[i+j]];
if (v1 != 0) {
v1 = 10 * log10f (v1);
}
int k = 0;
while ((k+i) >= 0 && w->log_index[k+i] == w->log_index[i]) {
j++;
k--;
}
x = linear_interpolate (v0,v1,(1.0/(j-1)) * ((-1 * k) - 1));
}
// TODO: get rid of hardcoding
x += CONFIG_DB_RANGE - 63;
x = CLAMP (x, 0, CONFIG_DB_RANGE);
int color_index = GRADIENT_TABLE_SIZE - ftoi (GRADIENT_TABLE_SIZE/(float)CONFIG_DB_RANGE * x);
color_index = CLAMP (color_index, 0, GRADIENT_TABLE_SIZE-1);
_draw_point (data, stride, width-1, height-1-i, w->colors[color_index]);
}
}
cairo_surface_mark_dirty (w->surf);
cairo_save (cr);
cairo_set_source_surface (cr, w->surf, 0, 0);
cairo_rectangle (cr, 0, 0, a.width, a.height);
cairo_fill (cr);
cairo_restore (cr);
return FALSE;
}
static double match_eps_func(int n, const double *u, double *grad, void *data)
{
match_eps_data *d = (match_eps_data *) data;
double *eps = d->eps, *work = d->work;
int eps_nx = d->eps_nx, eps_ny = d->eps_ny, eps_nz = d->eps_nz;
material_grid *grids = d->grids;
int ngrids = d->ngrids;
double scaleby = 1.0 / H.N, val = 0;
int i, j, k, n1, n2, n3, n_other, n_last, rank, last_dim;
#ifdef HAVE_MPI
int local_n2, local_y_start, local_n3;
#endif
real s1, s2, s3, c1, c2, c3;
material_grids_set(u, d->grids, d->ngrids);
reset_epsilon();
if (grad) memset(work, 0, sizeof(double) * n);
d->iter++;
n1 = mdata->nx;
n2 = mdata->ny;
n3 = mdata->nz;
n_other = mdata->other_dims;
n_last = mdata->last_dim_size / (sizeof(scalar_complex)/sizeof(scalar));
last_dim = mdata->last_dim;
rank = (n3 == 1) ? (n2 == 1 ? 1 : 2) : 3;
s1 = geometry_lattice.size.x / n1;
s2 = geometry_lattice.size.y / n2;
s3 = geometry_lattice.size.z / n3;
c1 = n1 <= 1 ? 0 : geometry_lattice.size.x * 0.5;
c2 = n2 <= 1 ? 0 : geometry_lattice.size.y * 0.5;
c3 = n3 <= 1 ? 0 : geometry_lattice.size.z * 0.5;
/* Here we have different loops over the coordinates, depending
upon whether we are using complex or real and serial or
parallel transforms. Each loop must define, in its body,
variables (i2,j2,k2) describing the coordinate of the current
point, and "index" describing the corresponding index in
the curfield array.
This was all stolen from fields.c...it would be better
if we didn't have to cut and paste, sigh. */
#ifdef SCALAR_COMPLEX
# ifndef HAVE_MPI
for (i = 0; i < n1; ++i)
for (j = 0; j < n2; ++j)
for (k = 0; k < n3; ++k)
{
int i2 = i, j2 = j, k2 = k;
int index = ((i * n2 + j) * n3 + k);
# else /* HAVE_MPI */
local_n2 = mdata->local_ny;
local_y_start = mdata->local_y_start;
/* first two dimensions are transposed in MPI output: */
for (j = 0; j < local_n2; ++j)
for (i = 0; i < n1; ++i)
for (k = 0; k < n3; ++k)
{
int i2 = i, j2 = j + local_y_start, k2 = k;
int index = ((j * n1 + i) * n3 + k);
# endif /* HAVE_MPI */
#else /* not SCALAR_COMPLEX */
# ifndef HAVE_MPI
for (i = 0; i < n_other; ++i)
for (j = 0; j < n_last; ++j)
{
int index = i * n_last + j;
int i2, j2, k2;
switch (rank) {
case 2:
i2 = i;
j2 = j;
k2 = 0;
break;
case 3:
i2 = i / n2;
j2 = i % n2;
k2 = j;
break;
default:
i2 = j;
j2 = k2 = 0;
break;
}
# else /* HAVE_MPI */
local_n2 = mdata->local_ny;
local_y_start = mdata->local_y_start;
/* For a real->complex transform, the last dimension is cut in
half. For a 2d transform, this is taken into account in local_ny
already, but for a 3d transform we must compute the new n3: */
if (n3 > 1)
local_n3 = mdata->last_dim_size / 2;
else
local_n3 = 1;
/* first two dimensions are transposed in MPI output: */
for (j = 0; j < local_n2; ++j)
for (i = 0; i < n1; ++i)
for (k = 0; k < local_n3; ++k)
{
# define i2 i
int j2 = j + local_y_start;
# define k2 k
int index = ((j * n1 + i) * local_n3 + k);
# endif /* HAVE_MPI */
#endif /* not SCALAR_COMPLEX */
{
real epsilon, eps0;
double scalegrad;
vector3 p;
epsilon = mean_medium_from_matrix(mdata->eps_inv + index);
eps0 = linear_interpolate((i2 + 0.5) / n1,
(j2 + 0.5) / n2,
(k2 + 0.5) / n3,
eps, eps_nx, eps_ny, eps_nz, 1);
val += (epsilon - eps0) * (epsilon - eps0);
scalegrad = 2.0 * scaleby * (epsilon - eps0);
if (grad) {
p.x = i2 * s1 - c1;
p.y = j2 * s2 - c2;
p.z = k2 * s3 - c3;
material_grids_addgradient_point(work, p, scalegrad,
grids, ngrids);
}
#ifndef SCALAR_COMPLEX
{
int last_index;
# ifdef HAVE_MPI
if (n3 == 1)
last_index = j + local_y_start;
else
last_index = k;
# else
last_index = j;
# endif
if (last_index != 0 && 2*last_index != last_dim) {
int i2c, j2c, k2c;
i2c = i2 ? (n1 - i2) : 0;
j2c = j2 ? (n2 - j2) : 0;
k2c = k2 ? (n3 - k2) : 0;
eps0 = linear_interpolate((i2c + 0.5) / n1,
(j2c + 0.5) / n2,
(k2c + 0.5) / n3, eps,
eps_nx, eps_ny, eps_nz, 1);
val += (epsilon - eps0) * (epsilon - eps0);
if (grad) {
p.x = i2c * s1 - c1;
p.y = j2c * s2 - c2;
p.z = k2c * s3 - c3;
material_grids_addgradient_point(work, p,
scalegrad,
grids, ngrids);
}
}
}
#endif /* !SCALAR_COMPLEX */
}
}
if (grad) /* gradient w.r.t. epsilon needs to be summed over processes */
mpi_allreduce(work, grad, n, double, MPI_DOUBLE,
MPI_SUM, mpb_comm);
{
double valtmp = val * scaleby;
mpi_allreduce(&valtmp, &val, 1, double, MPI_DOUBLE,
MPI_SUM, mpb_comm);
}
mpi_one_printf("match-epsilon-file:, %d, %g\n", d->iter, sqrt(val));
return val;
}
void material_grids_match_epsilon_fileB(string filename, number eps_tol)
{
int dims[3] = {1,1,1}, rank = 3;
matrixio_id file_id;
match_eps_data d;
int i, n, have_uprod;
double *u, *lb, *ub, *u_tol, func_min;
file_id = matrixio_open_serial(filename, 1);
d.eps = matrixio_read_real_data(file_id, NULL, &rank,dims, 0,0,0,0);
CHECK(d.eps, "couldn't find dataset in epsilon file");
matrixio_close(file_id);
d.eps_nx = dims[0];
d.eps_ny = dims[1];
d.eps_nz = dims[2];
d.grids = get_material_grids(geometry, &d.ngrids);
d.iter = 0;
n = material_grids_ntot(d.grids, d.ngrids);
u = (double *) malloc(sizeof(double) * n * 5);
lb = u + n;
ub = lb + n;
u_tol = ub + n;
d.work = u_tol + n;
material_grids_get(u, d.grids, d.ngrids);
for (i = 0; i < d.ngrids && d.grids[i].material_grid_kind != U_PROD; ++i);
have_uprod = i < d.ngrids;
for (i = 0; i < n; ++i) {
ub[i] = 1;
u_tol[i] = eps_tol;
lb[i] = have_uprod ? 1e-4 : 0;
if (u[i] < lb[i]) u[i] = lb[i];
}
#if defined(HAVE_NLOPT_H) && defined(HAVE_NLOPT)
{
nlopt_result res;
res = nlopt_minimize(NLOPT_LD_MMA, n, match_eps_func, &d,
lb, ub, u, &func_min,
-HUGE_VAL, 0,0, 0,u_tol, 0,0);
CHECK(res > 0, "failure of nlopt_minimize");
}
#else
CHECK(0, "nlopt library is required for match-epsilon-file");
#endif
material_grids_set(u, d.grids, d.ngrids);
reset_epsilon();
mpi_one_printf("match-epsilon-file converged to %g after %d iterations\n",
sqrt(func_min), d.iter);
free(u);
free(d.eps);
}
void Plane::calc_airspeed_errors()
{
float airspeed_measured = 0;
// we use the airspeed estimate function not direct sensor as TECS
// may be using synthetic airspeed
ahrs.airspeed_estimate(&airspeed_measured);
// FBW_B/cruise airspeed target
if (!failsafe.rc_failsafe && (control_mode == &mode_fbwb || control_mode == &mode_cruise)) {
if (g2.flight_options & FlightOptions::CRUISE_TRIM_AIRSPEED) {
target_airspeed_cm = aparm.airspeed_cruise_cm;
} else if (g2.flight_options & FlightOptions::CRUISE_TRIM_THROTTLE) {
float control_min = 0.0f;
float control_mid = 0.0f;
const float control_max = channel_throttle->get_range();
const float control_in = get_throttle_input();
switch (channel_throttle->get_type()) {
case RC_Channel::RC_CHANNEL_TYPE_ANGLE:
control_min = -control_max;
break;
case RC_Channel::RC_CHANNEL_TYPE_RANGE:
control_mid = channel_throttle->get_control_mid();
break;
}
if (control_in <= control_mid) {
target_airspeed_cm = linear_interpolate(aparm.airspeed_min * 100, aparm.airspeed_cruise_cm,
control_in,
control_min, control_mid);
} else {
target_airspeed_cm = linear_interpolate(aparm.airspeed_cruise_cm, aparm.airspeed_max * 100,
control_in,
control_mid, control_max);
}
} else {
target_airspeed_cm = ((int32_t)(aparm.airspeed_max - aparm.airspeed_min) *
get_throttle_input()) + ((int32_t)aparm.airspeed_min * 100);
}
} else if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) {
// Landing airspeed target
target_airspeed_cm = landing.get_target_airspeed_cm();
} else if ((control_mode == &mode_auto) &&
(quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH) &&
((vtol_approach_s.approach_stage == Landing_ApproachStage::APPROACH_LINE) ||
(vtol_approach_s.approach_stage == Landing_ApproachStage::VTOL_LANDING))) {
float land_airspeed = SpdHgt_Controller->get_land_airspeed();
if (is_positive(land_airspeed)) {
target_airspeed_cm = SpdHgt_Controller->get_land_airspeed() * 100;
} else {
// fallover to normal airspeed
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
} else {
// Normal airspeed target
target_airspeed_cm = aparm.airspeed_cruise_cm;
}
// Set target to current airspeed + ground speed undershoot,
// but only when this is faster than the target airspeed commanded
// above.
if (auto_throttle_mode &&
aparm.min_gndspeed_cm > 0 &&
control_mode != &mode_circle) {
int32_t min_gnd_target_airspeed = airspeed_measured*100 + groundspeed_undershoot;
if (min_gnd_target_airspeed > target_airspeed_cm) {
target_airspeed_cm = min_gnd_target_airspeed;
}
}
// Bump up the target airspeed based on throttle nudging
if (throttle_allows_nudging && airspeed_nudge_cm > 0) {
target_airspeed_cm += airspeed_nudge_cm;
}
// Apply airspeed limit
if (target_airspeed_cm > (aparm.airspeed_max * 100))
target_airspeed_cm = (aparm.airspeed_max * 100);
// use the TECS view of the target airspeed for reporting, to take
// account of the landing speed
airspeed_error = SpdHgt_Controller->get_target_airspeed() - airspeed_measured;
}
Spline2D::Spline2D (
size_t width_in,
size_t height_in,
size_t width_out,
size_t height_out,
const Transform & transform,
float min_mass)
: m_width_in(width_in),
m_height_in(height_in),
m_width_out(width_out),
m_height_out(height_out),
m_size_in(width_in * height_in),
m_size_out(width_out * height_out),
m_ij_out(m_size_in),
m_weights(m_size_in)
{
ASSERT_DIVIDES(block_size, m_width_in);
ASSERT_DIVIDES(block_size, m_height_in);
const size_t J_in = m_height_in;
const size_t J_out = m_height_out;
// transform via function
for (size_t i_in = 0; i_in < m_width_in; ++i_in) {
for (size_t j_in = 0; j_in < m_height_in; ++j_in) {
size_t ij_in = J_in * i_in + j_in;
int i0,j0;
float w0_, w1_, w_0, w_1;
float x_in = 2.0f * i_in / m_width_in - 1;
float y_in = 2.0f * j_in / m_height_in - 1;
Point xy(x_in, y_in);
transform(xy);
float i_out = m_width_out * (1 + xy.x) / 2.0f - 0.5f;
float j_out = m_height_out * (1 + xy.y) / 2.0f - 0.5f;
linear_interpolate(i_out, m_width_out, i0, w0_, w1_);
linear_interpolate(j_out, m_height_out, j0, w_0, w_1);
size_t ij_out = m_ij_out[ij_in] = J_out * i0 + j0;
float4 & weights = m_weights[ij_out];
weights[0] = w0_ * w_0;
weights[1] = w0_ * w_1;
weights[2] = w1_ * w_0;
weights[3] = w1_ * w_1;
}}
// renormalize to preserve constant functions
Vector<float> mass_in(m_size_in);
mass_in.set(1.0f);
Vector<float> mass_out(m_size_out);
transform_fwd(mass_in, mass_out);
float total_mass = 0;
for (size_t ij = 0; ij < m_size_out; ++ij) {
total_mass += mass_out[ij];
mass_out[ij] = 1 / (min_mass + mass_out[ij]);
}
float mean_mass = total_mass / m_size_in;
LOG("Spline2D image intersects " << mean_mass << " of range");
for (size_t ij = 0; ij < m_size_in; ++ij) {
size_t ij_out = m_ij_out[ij];
float4 & weights = m_weights[ij];
weights[0] *= mass_out[ij_out + 0 + 0];
weights[1] *= mass_out[ij_out + 0 + 1];
weights[2] *= mass_out[ij_out + J_out + 0];
weights[3] *= mass_out[ij_out + J_out + 1];
}
}
/* get ocv given a soc -- reverse lookup */
int interpolate_ocv(struct pc_temp_ocv_lut *pc_temp_ocv,
int batt_temp_degc, int pc)
{
int i, ocvrow1, ocvrow2, ocv, rows, cols;
int row1 = 0;
int row2 = 0;
rows = pc_temp_ocv->rows;
cols = pc_temp_ocv->cols;
if (pc > pc_temp_ocv->percent[0]) {
pr_debug("pc %d greater than known pc ranges for sfd\n", pc);
row1 = 0;
row2 = 0;
}
if (pc < pc_temp_ocv->percent[rows - 1]) {
pr_debug("pc %d less than known pc ranges for sf\n", pc);
row1 = rows - 1;
row2 = rows - 1;
}
for (i = 0; i < rows; i++) {
if (pc == pc_temp_ocv->percent[i]) {
row1 = i;
row2 = i;
break;
}
if (pc > pc_temp_ocv->percent[i]) {
row1 = i - 1;
row2 = i;
break;
}
}
if (batt_temp_degc < pc_temp_ocv->temp[0])
batt_temp_degc = pc_temp_ocv->temp[0];
if (batt_temp_degc > pc_temp_ocv->temp[cols - 1])
batt_temp_degc = pc_temp_ocv->temp[cols - 1];
for (i = 0; i < cols; i++)
if (batt_temp_degc <= pc_temp_ocv->temp[i])
break;
if (batt_temp_degc == pc_temp_ocv->temp[i]) {
ocv = linear_interpolate(
pc_temp_ocv->ocv[row1][i],
pc_temp_ocv->percent[row1],
pc_temp_ocv->ocv[row2][i],
pc_temp_ocv->percent[row2],
pc);
return ocv;
}
ocvrow1 = linear_interpolate(
pc_temp_ocv->ocv[row1][i - 1],
pc_temp_ocv->temp[i - 1],
pc_temp_ocv->ocv[row1][i],
pc_temp_ocv->temp[i],
batt_temp_degc);
ocvrow2 = linear_interpolate(
pc_temp_ocv->ocv[row2][i - 1],
pc_temp_ocv->temp[i - 1],
pc_temp_ocv->ocv[row2][i],
pc_temp_ocv->temp[i],
batt_temp_degc);
ocv = linear_interpolate(
ocvrow1,
pc_temp_ocv->percent[row1],
ocvrow2,
pc_temp_ocv->percent[row2],
pc);
return ocv;
}
int interpolate_pc(struct pc_temp_ocv_lut *pc_temp_ocv,
int batt_temp_degc, int ocv)
{
int i, j, pcj, pcj_minus_one, pc;
int rows = pc_temp_ocv->rows;
int cols = pc_temp_ocv->cols;
if (batt_temp_degc < pc_temp_ocv->temp[0]) {
pr_debug("batt_temp %d < known temp range\n", batt_temp_degc);
batt_temp_degc = pc_temp_ocv->temp[0];
}
if (batt_temp_degc > pc_temp_ocv->temp[cols - 1]) {
pr_debug("batt_temp %d > known temp range\n", batt_temp_degc);
batt_temp_degc = pc_temp_ocv->temp[cols - 1];
}
for (j = 0; j < cols; j++)
if (batt_temp_degc <= pc_temp_ocv->temp[j])
break;
if (batt_temp_degc == pc_temp_ocv->temp[j]) {
/* found an exact match for temp in the table */
if (ocv >= pc_temp_ocv->ocv[0][j])
return pc_temp_ocv->percent[0];
if (ocv <= pc_temp_ocv->ocv[rows - 1][j])
return pc_temp_ocv->percent[rows - 1];
for (i = 0; i < rows; i++) {
if (ocv >= pc_temp_ocv->ocv[i][j]) {
if (ocv == pc_temp_ocv->ocv[i][j])
return pc_temp_ocv->percent[i];
pc = linear_interpolate(
pc_temp_ocv->percent[i],
pc_temp_ocv->ocv[i][j],
pc_temp_ocv->percent[i - 1],
pc_temp_ocv->ocv[i - 1][j],
ocv);
return pc;
}
}
}
/*
* batt_temp_degc is within temperature for
* column j-1 and j
*/
if (ocv >= pc_temp_ocv->ocv[0][j])
return pc_temp_ocv->percent[0];
if (ocv <= pc_temp_ocv->ocv[rows - 1][j - 1])
return pc_temp_ocv->percent[rows - 1];
pcj_minus_one = 0;
pcj = 0;
for (i = 0; i < rows-1; i++) {
if (pcj == 0
&& is_between(pc_temp_ocv->ocv[i][j],
pc_temp_ocv->ocv[i+1][j], ocv)) {
pcj = linear_interpolate(
pc_temp_ocv->percent[i],
pc_temp_ocv->ocv[i][j],
pc_temp_ocv->percent[i + 1],
pc_temp_ocv->ocv[i+1][j],
ocv);
}
if (pcj_minus_one == 0
&& is_between(pc_temp_ocv->ocv[i][j-1],
pc_temp_ocv->ocv[i+1][j-1], ocv)) {
pcj_minus_one = linear_interpolate(
pc_temp_ocv->percent[i],
pc_temp_ocv->ocv[i][j-1],
pc_temp_ocv->percent[i + 1],
pc_temp_ocv->ocv[i+1][j-1],
ocv);
}
if (pcj && pcj_minus_one) {
pc = linear_interpolate(
pcj_minus_one,
pc_temp_ocv->temp[j-1],
pcj,
pc_temp_ocv->temp[j],
batt_temp_degc);
return pc;
}
}
if (pcj)
return pcj;
if (pcj_minus_one)
return pcj_minus_one;
pr_debug("%d ocv wasn't found for temp %d in the LUT returning 100%%\n",
ocv, batt_temp_degc);
return 100;
}
