// *****************************************************************************
// Main
int main(int argc, char* const argv[])
{
try {
if (argc != 2) {
std::cout << "Usage: " << argv[0] << " file\n";
return 1;
}
std::string file(argv[1]);
std::cout <<"----- Some IFD0 tags\n";
Exiv2::ExifData ed1;
ed1["Exif.Image.Model"] = "Test 1";
Exiv2::Value::AutoPtr v1 = Exiv2::Value::create(Exiv2::unsignedShort);
v1->read("160 161 162 163");
ed1.add(Exiv2::ExifKey("Exif.Image.SamplesPerPixel"), v1.get());
Exiv2::Value::AutoPtr v2 = Exiv2::Value::create(Exiv2::signedLong);
v2->read("-2 -1 0 1");
ed1.add(Exiv2::ExifKey("Exif.Image.XResolution"), v2.get());
Exiv2::Value::AutoPtr v3 = Exiv2::Value::create(Exiv2::signedRational);
v3->read("-2/3 -1/3 0/3 1/3");
ed1.add(Exiv2::ExifKey("Exif.Image.YResolution"), v3.get());
Exiv2::Value::AutoPtr v4 = Exiv2::Value::create(Exiv2::undefined);
v4->read("255 254 253 252");
ed1.add(Exiv2::ExifKey("Exif.Image.WhitePoint"), v4.get());
write(file, ed1);
print(file);
std::cout <<"\n----- One Exif tag\n";
Exiv2::ExifData ed2;
ed2["Exif.Photo.DateTimeOriginal"] = "Test 2";
write(file, ed2);
print(file);
std::cout <<"\n----- Canon MakerNote tags\n";
Exiv2::ExifData edMn1;
edMn1["Exif.Image.Make"] = "Canon";
edMn1["Exif.Image.Model"] = "Canon PowerShot S40";
edMn1["Exif.Canon.0xabcd"] = "A Canon makernote tag";
edMn1["Exif.CanonCs.0x0002"] = uint16_t(41);
edMn1["Exif.CanonSi.0x0005"] = uint16_t(42);
edMn1["Exif.CanonCf.0x0001"] = uint16_t(43);
edMn1["Exif.CanonPi.0x0001"] = uint16_t(44);
edMn1["Exif.CanonPa.0x0001"] = uint16_t(45);
write(file, edMn1);
print(file);
std::cout <<"\n----- Non-intrusive writing of special Canon MakerNote tags\n";
Exiv2::Image::AutoPtr image = Exiv2::ImageFactory::open(file);
assert(image.get() != 0);
image->readMetadata();
Exiv2::ExifData& rEd = image->exifData();
rEd["Exif.CanonCs.0x0001"] = uint16_t(88);
rEd["Exif.CanonSi.0x0004"] = uint16_t(99);
image->writeMetadata();
print(file);
std::cout <<"\n----- One Fujifilm MakerNote tag\n";
Exiv2::ExifData edMn2;
edMn2["Exif.Image.Make"] = "FUJIFILM";
edMn2["Exif.Image.Model"] = "FinePixS2Pro";
edMn2["Exif.Fujifilm.0x1000"] = "A Fujifilm QUALITY tag";
write(file, edMn2);
print(file);
std::cout <<"\n----- One Sigma/Foveon MakerNote tag\n";
Exiv2::ExifData edMn3;
edMn3["Exif.Image.Make"] = "SIGMA";
edMn3["Exif.Image.Model"] = "SIGMA SD10";
edMn3["Exif.Sigma.0x0018"] = "Software? Exiv2!";
write(file, edMn3);
print(file);
std::cout <<"\n----- One Nikon1 MakerNote tag\n";
Exiv2::ExifData edMn4;
edMn4["Exif.Image.Make"] = "NIKON";
edMn4["Exif.Image.Model"] = "E990";
edMn4["Exif.Nikon1.0x0080"] = "ImageAdjustment by Exiv2";
write(file, edMn4);
print(file);
std::cout <<"\n----- One Nikon2 MakerNote tag\n";
Exiv2::ExifData edMn5;
edMn5["Exif.Image.Make"] = "NIKON";
edMn5["Exif.Image.Model"] = "E950";
edMn5["Exif.Nikon2.0xffff"] = "An obscure Nikon2 tag";
write(file, edMn5);
print(file);
std::cout <<"\n----- One Nikon3 MakerNote tag\n";
Exiv2::ExifData edMn6;
edMn6["Exif.Image.Make"] = "NIKON CORPORATION";
edMn6["Exif.Image.Model"] = "NIKON D70";
edMn6["Exif.Nikon3.0x0004"] = "A boring Nikon3 Quality tag";
write(file, edMn6);
print(file);
std::cout <<"\n----- One Olympus MakerNote tag\n";
Exiv2::ExifData edMn7;
edMn7["Exif.Image.Make"] = "OLYMPUS CORPORATION";
edMn7["Exif.Image.Model"] = "C8080WZ";
edMn7["Exif.Olympus.0x0201"] = uint16_t(1);
write(file, edMn7);
print(file);
std::cout <<"\n----- One Panasonic MakerNote tag\n";
Exiv2::ExifData edMn8;
edMn8["Exif.Image.Make"] = "Panasonic";
edMn8["Exif.Image.Model"] = "DMC-FZ5";
edMn8["Exif.Panasonic.0x0001"] = uint16_t(1);
write(file, edMn8);
print(file);
std::cout <<"\n----- One Sony1 MakerNote tag\n";
Exiv2::ExifData edMn9;
edMn9["Exif.Image.Make"] = "SONY";
edMn9["Exif.Image.Model"] = "DSC-W7";
edMn9["Exif.Sony1.0x2000"] = "0 1 2 3 4 5";
write(file, edMn9);
print(file);
std::cout <<"\n----- Minolta MakerNote tags\n";
Exiv2::ExifData edMn10;
edMn10["Exif.Image.Make"] = "Minolta";
edMn10["Exif.Image.Model"] = "A fancy Minolta camera";
edMn10["Exif.Minolta.ColorMode"] = uint32_t(1);
edMn10["Exif.MinoltaCsNew.WhiteBalance"] = uint32_t(2);
edMn10["Exif.MinoltaCs5D.WhiteBalance"] = uint16_t(3);
edMn10["Exif.MinoltaCs5D.ColorTemperature"] = int16_t(-1);
edMn10["Exif.MinoltaCs7D.WhiteBalance"] = uint16_t(4);
edMn10["Exif.MinoltaCs7D.ExposureCompensation"] = int16_t(-2);
edMn10["Exif.MinoltaCs7D.ColorTemperature"] = int16_t(-3);
write(file, edMn10);
print(file);
std::cout <<"\n----- One IOP tag\n";
Exiv2::ExifData ed3;
ed3["Exif.Iop.InteroperabilityIndex"] = "Test 3";
write(file, ed3);
print(file);
std::cout <<"\n----- One GPS tag\n";
Exiv2::ExifData ed4;
ed4["Exif.GPSInfo.GPSVersionID"] = "19 20";
write(file, ed4);
print(file);
std::cout <<"\n----- One IFD1 tag\n";
Exiv2::ExifData ed5;
ed5["Exif.Thumbnail.Artist"] = "Test 5";
write(file, ed5);
print(file);
std::cout <<"\n----- One IOP and one IFD1 tag\n";
Exiv2::ExifData ed6;
ed6["Exif.Iop.InteroperabilityIndex"] = "Test 6 Iop tag";
ed6["Exif.Thumbnail.Artist"] = "Test 6 Ifd1 tag";
write(file, ed6);
print(file);
std::cout <<"\n----- One IFD0 and one IFD1 tag\n";
Exiv2::ExifData ed7;
ed7["Exif.Thumbnail.Artist"] = "Test 7";
Exiv2::Value::AutoPtr v5 = Exiv2::Value::create(Exiv2::unsignedShort);
v5->read("160 161 162 163");
ed7.add(Exiv2::ExifKey("Exif.Image.SamplesPerPixel"), v5.get());
write(file, ed7);
print(file);
return 0;
}
catch (Exiv2::AnyError& e) {
std::cout << "Caught Exiv2 exception '" << e << "'\n";
return -1;
}
}
void put(int i, int j, int k, double d)
{
*int16_p(data(i, j, k)) = swap(int16_t(clamp(d) * INT16_MAX));
}
void TextCharacters::drawText(MonitorDevice *const pMonitorDevice,
const u_color textColor, const uint8_t shiftX, const uint8_t shiftY,
const int16_t horizontalShift)
{
if (m_size.width == 0 || m_size.height == 0)
return;
int16_t vAlign;
if (align & Alignment::Bottom)
vAlign = m_size.height - font().height;
else if (align & Alignment::VCenter)
vAlign = (m_size.height - font().height) / 2 ;
else // if (align & Alignment::Top)
vAlign = 0;
int16_t hAlign;
if (align & Alignment::Right)
hAlign = m_size.width - m_pxTextWidth;
else if (align & Alignment::HCenter)
hAlign = (m_size.width - m_pxTextWidth) / 2 ;
else // if (align & Alignment::Left)
hAlign = 0;
hAlign += horizontalShift;
const uint8_t vAlignPos = vAlign < 0 ? abs(vAlign) : 0;
int16_t posX = new_textAbsPosition.x + hAlign + shiftX;
const int16_t cx = new_textAbsPosition.x + m_size.width;
const int16_t cy = new_textAbsPosition.y + m_size.height;
for (size_t n = 0; n < m_text.length(); ++n)
{
const IFont::CHAR_INFO *pDescriptor = descriptor(m_text.at(n));
const int8_t _yPos = vAlignPos - pDescriptor->fstRow;
const uint8_t yPos = _yPos < 0 ? 0 : _yPos;
const uint8_t *pBitmaps = &font().bitmaps()[pDescriptor->position + yPos *
(IFont::Mode::Bitmap == font().mode ? pDescriptor->width / 8 : pDescriptor->width)];
const int16_t posY = new_textAbsPosition.y + vAlign + pDescriptor->fstRow + shiftY;
const int16_t yMax = std::min(cy, int16_t(pDescriptor->sizeRow + posY));
for (uint16_t y = posY + yPos; y < yMax; ++y)
{
uint8_t pt = 0;
int16_t x = posX;
for (uint8_t width = 0; width < pDescriptor->width; ++width)
{
if (IFont::Mode::Bitmap == font().mode)
{
if (x >= cx)
{
pBitmaps += (pDescriptor->width - width) / 8;
break;
}
const uint8_t nBit = 7 - (width % 8);
if (nBit == 7)
pt = *(pBitmaps++);
if ( x >= new_textAbsPosition.x)
{
const bool px = pt >> nBit & 0x01;
if (!px)
pMonitorDevice->drawPoint(x, y, textColor);
}
}
else // IFont::Mode::Antialias
{
if (x >= cx)
{
pBitmaps += pDescriptor->width - width;
break;
}
const uint8_t pt = *(pBitmaps++);
if (pt != 0xFFU && x >= new_textAbsPosition.x)
{
u_color foreground(textColor);
#ifdef RGB888
foreground.argb.A = 255 - pt;
foreground = pMonitorDevice->alphaBlending(foreground, background());
#elif RGB565
foreground = pMonitorDevice->alphaBlending(foreground, background(), 255 - pt);
#endif
pMonitorDevice->drawPoint(x, y, foreground);
}
}
x++;
}
}
posX += pDescriptor->width;
}
void pf_field::update(local_member* p_ac, AP_AHRS_DCM* ahrs, AP_Airspeed* airspeed)
{
#endif
#if HIL_MODE == HIL_MODE_ATTITUDE
void pf_field::update(local_member* p_ac, AP_AHRS_HIL* ahrs, AP_Airspeed* airspeed)
{
#endif
const Relative* tmp_p_rel = p_ac->get_rel(); //Gets the current relative information from the ac object
Vector3i tmp_gV_L = *p_ac->get_gV_L();
uint16_t tmp_psi_ac = *p_ac->get_hdg(); //Get our heading (deg)
//Calculate total repulsive function from i flock members
Vector3f tmp_r_phi; //initialize vector (NED components)
//Sum repulsive functions from i flock members
for(int i=0;i<tmp_p_rel->Num_members;i++)
{
//indexing convention to organize member pointers
int j = tmp_p_rel->Member_ids[i]-1;
//Relative distance to flock members (NED components)
Vector3f tmp_dX; //initialize vector each iteration
tmp_dX.x=tmp_p_rel->dX[j]/100.0; //[m]
tmp_dX.y=tmp_p_rel->dY[j]/100.0; //[m]
tmp_dX.z=tmp_p_rel->dZ[j]/100.0; //[m]
//Calculate repulsive parameters based on relative states
//Gaussian repulsive function accumulation
tmp_r_phi.x += (-2*(tmp_dX.x)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
tmp_r_phi.y += (-2*(tmp_dX.y)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
tmp_r_phi.z += (-2*(tmp_dX.z)*(_tau/100.0)/(_zeta/100.0))*exp(-(square(tmp_dX.x)+square(tmp_dX.y)+square(tmp_dX.z))/(_zeta/100.0));
}
_phi_r = tmp_r_phi; //Repulsive potential gradient vector (NED component)
//Relative distance vector to leader (NED components)
Vector3i tmp_dL; //initialize vector (NED components)
tmp_dL.x=tmp_p_rel->dXL; //[m*100]
tmp_dL.y=tmp_p_rel->dYL; //[m*100]
tmp_dL.z=tmp_p_rel->dZL; //[m*100]
//Relative distance magnitude to leader
int32_t tmp_d2L=tmp_p_rel->d2L; //[m*100]
//Relative velocity to leader (NED components)
//Vector3i tmp_dV; //initialize vector (NED components)
Vector3i tmp_gV_LNAV; //initialize dVaspd (Local Navigation frame)
//tmp_dV.x=tmp_p_rel->dVXL; //[m/s*100]
//tmp_dV.y=tmp_p_rel->dVYL; //[m/s*100]
//tmp_dV.z=tmp_p_rel->dVZL; //[m/s*100]
float Cpsi = cos(ToRad(tmp_psi_ac/100.0));
float Spsi = sin(ToRad(tmp_psi_ac/100.0));
tmp_gV_LNAV.x=int16_t((tmp_gV_L.x)*Cpsi+(tmp_gV_L.y)*Spsi); //[m/s*100]
tmp_gV_LNAV.y=int16_t(-(tmp_gV_L.x)*Spsi+(tmp_gV_L.y)*Cpsi); //[m/s*100]
tmp_gV_LNAV.z=tmp_p_rel->dVZL; //[m/s*100]
//Calculate the attractive potential gradient (NED components)
/*Note:
Attractive potential is comprised of two types of potential functions
-In the far-field regime we use a linear potential field
-In the near-field regime we use a quadratic potential field
These fields must be matched at _chi, the near-field, far-field regime cut-off such that
-Gradient of quadratic at _chi = gradient of linear at _chi
In far-field, we are guided towards the leader's position (no offset) - this should improve convergence
In near-field, we are guided towards the offset position, the side of which is dictated by swarm algorithm
*/
Vector3i tmp_pf_offset_NED; //Initialize vector
uint16_t tmp_psi_L = tmp_p_rel->hdgL; //Get the heading of the Leader
float CpsiL = cos(ToRad(tmp_p_rel->hdgL/100.0));
float SpsiL = sin(ToRad(tmp_p_rel->hdgL/100.0));
//set offset (Leader's Local frame)
Vector3i tmp_pf_offset = _pf_offset_l;
//update offset with side correction
tmp_pf_offset.y = tmp_pf_offset.y*_side;
//Rotate the offset distances (Leader's Local Navigation Frame) to NED frame
tmp_pf_offset_NED.x = tmp_pf_offset.x*CpsiL - tmp_pf_offset.y*SpsiL;
tmp_pf_offset_NED.y = tmp_pf_offset.x*SpsiL + tmp_pf_offset.y*CpsiL;
tmp_pf_offset_NED.z = tmp_pf_offset.z;
//Extrapolate Goal for NF case
Location tmp_goal_loc = *p_ac->get_loc();
Location tmp_current_loc = *p_ac->get_loc();
//A bunch of new stuff for this extrapolation technique
location_offset(&tmp_goal_loc,tmp_dL.x/100.0, tmp_dL.y/100.0);
location_offset(&tmp_goal_loc,-tmp_pf_offset_NED.x/100.0, -tmp_pf_offset_NED.y/100.0);
float tmp_N_extrap = 10*CpsiL;
float tmp_E_extrap = 10*SpsiL;
location_offset(&tmp_goal_loc,tmp_N_extrap, tmp_E_extrap);
int32_t tmp_B_extrap = get_bearing_cd(&tmp_current_loc,&tmp_goal_loc);
Vector3f extrap_uvec_NED;
extrap_uvec_NED.x = cos(ToRad(tmp_B_extrap/100.0));
extrap_uvec_NED.y = sin(ToRad(tmp_B_extrap/100.0));
extrap_uvec_NED.z = 0;
tmp_dL -=tmp_pf_offset_NED;
tmp_d2L = int32_t(100*safe_sqrt(square(tmp_dL.x/100.0)+square(tmp_dL.y/100.0)+square(tmp_dL.z/100.0)));
//Subtract offset distance vector from leader distance vector to get modified relative distance vector to leader
if(tmp_d2L<(_chi*100)) //Near-field regime condition
{
_phi_a.x = 2*(_lambda.x/100.0)*(tmp_dL.x/100.0); //Calculate Quad PF X gradient with weighting parameter (NED frame)
_phi_a.y = 2*(_lambda.y/100.0)*(tmp_dL.y/100.0); //Calculate Quad PF Y gradient with weighting parameter (NED frame)
_phi_a.z = 2*(_lambda.z/100.0)*(tmp_dL.z/100.0); //Calculate Quad PF Z gradient with weighting parameter (NED frame)
float tmp_phi_a_mag = _phi_a.length();
_phi_a_c.x = tmp_phi_a_mag*extrap_uvec_NED.x;
_phi_a_c.y = tmp_phi_a_mag*extrap_uvec_NED.y;
_phi_a_c.z = 0;
}
else
{
//float tmp_M =2*safe_sqrt(square(_lambda.x/100.0)+square(_lambda.y/100.0)+square(_lambda.z/100.0))*(_chi); //Slope of linear potential field (Magnitude of gradient) = nominal magnitude of quadratic gradient at chi
float tmp_M = 2*_chi;
float tmp_R = safe_sqrt(square((_lambda.x/100.0)*(tmp_dL.x/100.0))+square((_lambda.y/100.0)*(tmp_dL.y/100.0))+square((_lambda.z/100.0)*(tmp_dL.z/100)));
_phi_a.x = tmp_M*square(_lambda.x/100.0)*(tmp_dL.x/100.0)/tmp_R; //Calculate Lin PF X gradient with weighting parameter (NED frame)
_phi_a.y = tmp_M*square(_lambda.y/100.0)*(tmp_dL.y/100.0)/tmp_R; //Calculate Lin PF Y gradient with weighting parameter (NED frame)
_phi_a.z = tmp_M*square(_lambda.z/100.0)*(tmp_dL.z/100.0)/tmp_R; //Calculate Lin PF Z gradient with weighting parameter (NED frame)
}
//Calculate the total potential gradient components
_phi_NED = _phi_a + _phi_r;
_phi_c_NED = _phi_a_c + _phi_r;
//Calculate the magnitude of the total potential function gradient
if(_phi_NED.length() > 0) //divide-by-zero check (safety first)
{
//Calculate the normalized potential gradient components
_Nphi_NED = _phi_NED.normalized();
_Nphi_c_NED = _phi_c_NED.normalized();
//Find X component in Local Navigation frame by rotating NED vector about heading
_Nphi_l.x = _Nphi_NED.x*Cpsi + _Nphi_NED.y*Spsi;
if(tmp_d2L<_chi*100) //near-field consideration- We don't want the VWP behind the body
{
_regime_mask = 0x01; //Make first bit in regime mask 1 to reflect that we're in near-field regime
/* //Correct normalized vector for VWP in near-field
if(_Nphi_l.x<0) //Vector is pointing behind ac in Local Navigation frame
{
//Rotate NED pf gradiend components into Local Navigation components
_phi_l.x = _phi_NED.x*cos(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*sin(ToRad(tmp_psi_ac/100.0));
_phi_l.y = - _phi_NED.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_NED.y*cos(ToRad(tmp_psi_ac/100.0));
//Correct potential function to point forward
/* Note:
Corrected by using airspeed instead of relative distance... almost a predictor of where the goal "will" be in a second
/
float airspeed_est;
if(ahrs->airspeed_estimate(&airspeed_est))
{
_phi_l.x = 2*(_lambda.x/100.0)*(airspeed_est);
}
else
{
_phi_l.x = 2*(_lambda.x/100.0)*10; //Not the most elegant way to do this... 10 is an average airspeed for the skysurfer
}
//Corrected potential rotated back into NED frame from Local Navigation frame
_phi_c_NED.x= _phi_l.x*cos(ToRad(tmp_psi_ac/100.0)) - _phi_l.y*sin(ToRad(tmp_psi_ac/100.0));
_phi_c_NED.y= _phi_l.x*sin(ToRad(tmp_psi_ac/100.0)) + _phi_l.y*cos(ToRad(tmp_psi_ac/100.0));
//Normalize the corrected potential gradient vector
_Nphi_c_NED = _phi_c_NED.normalized();
//Modify regime mask to reflect that both the first bit and the second bit are 1 (near-field regime, and using a corrected
_regime_mask = 0x03;
}
//Make sure regime mask reflects that only
else _regime_mask = 0x01; */
}
else
{
//Regime mask should reflect that we are not in the near-field, and we therefore aren't using a corrected potential field
_regime_mask = 0x00;
}
}
else
{
//If the magnitude of the gradient is not greater than 0, make the norm 0 to avoid divide by 0 problems
_Nphi_NED.zero();
_Nphi_c_NED.zero();
}
float tmp_dNorth_com;
float tmp_dEast_com;
float tmp_dalt_com;
float tmp_dspd_com;
if(tmp_d2L<_chi*100)
{
//Calculate the North and East Offset [m]
tmp_dNorth_com = _VWP_offset*_Nphi_c_NED.x;
tmp_dEast_com = _VWP_offset*_Nphi_c_NED.y;
//Calculate the change in altitude [m]
//k_alt_V is the equivalent of a derivative gain- set to 0 for simplicity.
//tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z;
tmp_dalt_com = tmp_dL.z/100.0;
_phi_norm = _Nphi_c_NED;
}
else
{ //Far-Field Case
tmp_dNorth_com = _VWP_offset*_Nphi_NED.x;
tmp_dEast_com = _VWP_offset*_Nphi_NED.y;
//tmp_dalt_com = _VWP_Z_offset*_Nphi_NED.z;
tmp_dalt_com = tmp_dL.z/100.0;
_phi_norm = _Nphi_NED;
}
const Location* p_current_location = p_ac->get_loc();
_next_VWP = *p_current_location;
location_offset(&_next_VWP, tmp_dNorth_com, tmp_dEast_com);
_next_VWP.alt+=(int32_t)(tmp_dalt_com*100); //double-check sign here.
constrain(_next_VWP.alt,PFG_MIN_ALT,PFG_MAX_ALT);
if(tmp_d2L<_chi*100)
{ //Near-Field Case
//New speed matching/ Speed from PFG command algorithm
_next_airspeed_com = int32_t(100*((tmp_gV_LNAV.x/100.0*(PFG_K_P_dV/100.0)) +(_Nphi_l.x*(PFG_K_I_dV/100.0))));
constrain(_next_airspeed_com,PFG_MIN_AIRSPEED_CM,PFG_MAX_AIRSPEED_CM);
}
else
{ //Far-Field Case
_next_airspeed_com = PFG_MAX_AIRSPEED_CM; //In far-field, set airspeed to maximum allowable (to catch up)
}
}
const Location* pf_field::get_VWP(){
return (const Location*)&_next_VWP;
}
const int32_t* pf_field::get_new_speed(){
return (const int32_t*)&_next_airspeed_com;
}
const Vector3f* pf_field::get_pfg_att(){
return (const Vector3f*)&_phi_a;
}
const Vector3f* pf_field::get_pfg_rep(){
return (const Vector3f*)&_phi_r;
}
const Vector3f* pf_field::get_pfg_norm(){
return (const Vector3f*)&_phi_norm;
}
uint8_t pf_field::get_regime_mask(){
return _regime_mask;
}
pf_field PF_FIELD;
int SoapyAudio::readStream(
SoapySDR::Stream *stream,
void * const *buffs,
const size_t numElems,
int &flags,
long long &timeNs,
const long timeoutUs)
{
if (!dac.isStreamRunning()) {
return 0;
}
if (sampleRateChanged.load()) {
if (dac.isStreamRunning()) {
dac.stopStream();
}
if (dac.isStreamOpen()) {
dac.closeStream();
}
dac.openStream(NULL, &inputParameters, RTAUDIO_FLOAT32, sampleRate, &bufferLength, &_rx_callback, (void *) this, &opts);
dac.startStream();
sampleRateChanged.store(false);
}
//this is the user's buffer for channel 0
void *buff0 = buffs[0];
//are elements left in the buffer? if not, do a new read.
if (bufferedElems == 0 || (sampleOffset && (bufferedElems < abs(sampleOffset))))
{
int ret = this->acquireReadBuffer(stream, _currentHandle, (const void **)&_currentBuff, flags, timeNs, timeoutUs);
if (ret < 0) return ret;
bufferedElems = ret;
}
size_t returnedElems = std::min(bufferedElems, numElems);
if (sampleOffset && (bufferedElems < abs(sampleOffset))) {
return 0;
}
//convert into user's buff0
if (sampleOffset) {
if (asFormat == AUDIO_FORMAT_FLOAT32)
{
float *ftarget = (float *) buff0;
std::complex<float> tmp;
if (cSetup == FORMAT_MONO_L || cSetup == FORMAT_MONO_R) {
for (size_t i = 0; i < returnedElems; i++)
{
ftarget[i * 2] = _currentBuff[i];
ftarget[i * 2 + 1] = 0;
}
}
else if (cSetup == FORMAT_STEREO_IQ) {
if (sampleOffset > 0) {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
ftarget[i * 2] = sampleOffsetBuffer[i];
ftarget[i * 2 + 1] = _currentBuff[i * 2 + 1];
}
for (size_t i = iStart; i < returnedElems; i++) {
ftarget[i * 2] = _currentBuff[(i + iStart) * 2];
ftarget[i * 2 + 1] = _currentBuff[i * 2 + 1];
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2];
}
} else {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
ftarget[i * 2] = _currentBuff[i * 2];
ftarget[i * 2 + 1] = sampleOffsetBuffer[i];
}
for (size_t i = iStart; i < returnedElems; i++) {
ftarget[i * 2] = _currentBuff[i * 2];
ftarget[i * 2 + 1] = _currentBuff[(i + iStart) * 2 + 1];
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2 + 1];
}
}
}
else if (cSetup == FORMAT_STEREO_QI) {
if (sampleOffset > 0) {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
ftarget[i * 2 + 1] = sampleOffsetBuffer[i];
ftarget[i * 2] = _currentBuff[i * 2 + 1];
}
for (size_t i = iStart; i < returnedElems; i++) {
ftarget[i * 2 + 1] = _currentBuff[(i + iStart) * 2];
ftarget[i * 2] = _currentBuff[i * 2 + 1];
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2];
}
} else {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
ftarget[i * 2 + 1] = _currentBuff[i * 2];
ftarget[i * 2] = sampleOffsetBuffer[i];
}
for (size_t i = iStart; i < returnedElems; i++) {
ftarget[i * 2 + 1] = _currentBuff[i * 2];
ftarget[i * 2] = _currentBuff[(i + iStart) * 2 + 1];
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2 + 1];
}
}
}
}
else if (asFormat == AUDIO_FORMAT_INT16)
{
int16_t *itarget = (int16_t *) buff0;
std::complex<int16_t> tmp;
if (cSetup == FORMAT_MONO_L || cSetup == FORMAT_MONO_R) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int16_t(_currentBuff[i] * 32767.0);
itarget[i * 2 + 1] = 0;
}
}
else if (cSetup == FORMAT_STEREO_IQ) {
if (sampleOffset > 0) {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2] = int16_t(sampleOffsetBuffer[i] * 32767.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2 + 1] * 32767.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2] = int16_t(_currentBuff[(i + iStart) * 2] * 32767.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2 + 1] * 32767.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2];
}
} else {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2] = int16_t(_currentBuff[i * 2] * 32767.0);
itarget[i * 2 + 1] = int16_t(sampleOffsetBuffer[i] * 32767.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2] = int16_t(_currentBuff[i * 2] * 32767.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[(i + iStart) * 2 + 1] * 32767.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2 + 1];
}
}
}
if (cSetup == FORMAT_STEREO_QI) {
if (sampleOffset > 0) {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2 + 1] = int16_t(sampleOffsetBuffer[i] * 32767.0);
itarget[i * 2] = int16_t(_currentBuff[i * 2 + 1] * 32767.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2 + 1] = int16_t(_currentBuff[(i + iStart) * 2] * 32767.0);
itarget[i * 2] = int16_t(_currentBuff[i * 2 + 1] * 32767.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2];
}
} else {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2] * 32767.0);
itarget[i * 2] = int16_t(sampleOffsetBuffer[i] * 32767.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2] * 32767.0);
itarget[i * 2] = int16_t(_currentBuff[(i + iStart) * 2 + 1] * 32767.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2 + 1];
}
}
}
}
else if (asFormat == AUDIO_FORMAT_INT8)
{
int8_t *itarget = (int8_t *) buff0;
if (cSetup == FORMAT_MONO_L || cSetup == FORMAT_MONO_R) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int8_t(_currentBuff[i] * 127.0);
itarget[i * 2 + 1] = 0;
}
}
else if (cSetup == FORMAT_STEREO_IQ) {
if (sampleOffset > 0) {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2] = int16_t(sampleOffsetBuffer[i] * 127.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2 + 1] * 127.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2] = int16_t(_currentBuff[(i + iStart) * 2] * 127.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2 + 1] * 127.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2];
}
} else {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2] = int16_t(_currentBuff[i * 2] * 127.0);
itarget[i * 2 + 1] = int16_t(sampleOffsetBuffer[i] * 127.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2] = int16_t(_currentBuff[i * 2] * 127.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[(i + iStart) * 2 + 1] * 127.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2 + 1];
}
}
}
if (cSetup == FORMAT_STEREO_QI) {
if (sampleOffset > 0) {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2 + 1] = int16_t(sampleOffsetBuffer[i] * 127.0);
itarget[i * 2] = int16_t(_currentBuff[i * 2 + 1] * 127.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2 + 1] = int16_t(_currentBuff[(i + iStart) * 2] * 127.0);
itarget[i * 2] = int16_t(_currentBuff[i * 2 + 1] * 127.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2];
}
} else {
size_t iStart = abs(sampleOffset);
for (size_t i = 0; i < iStart; i++) {
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2] * 127.0);
itarget[i * 2] = int16_t(sampleOffsetBuffer[i] * 127.0);
}
for (size_t i = iStart; i < returnedElems; i++) {
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2] * 127.0);
itarget[i * 2] = int16_t(_currentBuff[(i + iStart) * 2 + 1] * 127.0);
}
for (int i = 0; i < iStart; i++) {
sampleOffsetBuffer[i] = _currentBuff[(returnedElems-iStart+i) * 2 + 1];
}
}
}
}
} else {
if (asFormat == AUDIO_FORMAT_FLOAT32)
{
float *ftarget = (float *) buff0;
std::complex<float> tmp;
if (cSetup == FORMAT_MONO_L || cSetup == FORMAT_MONO_R) {
for (size_t i = 0; i < returnedElems; i++)
{
ftarget[i * 2] = _currentBuff[i];
ftarget[i * 2 + 1] = 0;
}
}
else if (cSetup == FORMAT_STEREO_IQ) {
for (size_t i = 0; i < returnedElems; i++)
{
ftarget[i * 2] = _currentBuff[i * 2];
ftarget[i * 2 + 1] = _currentBuff[i * 2 + 1];
}
}
else if (cSetup == FORMAT_STEREO_QI) {
for (size_t i = 0; i < returnedElems; i++)
{
ftarget[i * 2] = _currentBuff[i * 2 + 1];
ftarget[i * 2 + 1] = _currentBuff[i * 2];
}
}
}
else if (asFormat == AUDIO_FORMAT_INT16)
{
int16_t *itarget = (int16_t *) buff0;
std::complex<int16_t> tmp;
if (cSetup == FORMAT_MONO_L || cSetup == FORMAT_MONO_R) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int16_t(_currentBuff[i] * 32767.0);
itarget[i * 2 + 1] = 0;
}
}
else if (cSetup == FORMAT_STEREO_IQ) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int16_t(_currentBuff[i * 2] * 32767.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2 + 1] * 32767.0);
}
}
else if (cSetup == FORMAT_STEREO_QI) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int16_t(_currentBuff[i * 2 + 1] * 32767.0);
itarget[i * 2 + 1] = int16_t(_currentBuff[i * 2] * 32767.0);
}
}
}
else if (asFormat == AUDIO_FORMAT_INT8)
{
int8_t *itarget = (int8_t *) buff0;
if (cSetup == FORMAT_MONO_L || cSetup == FORMAT_MONO_R) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int8_t(_currentBuff[i] * 127.0);
itarget[i * 2 + 1] = 0;
}
}
else if (cSetup == FORMAT_STEREO_IQ) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int8_t(_currentBuff[i * 2] * 127.0);
itarget[i * 2 + 1] = int8_t(_currentBuff[i * 2 + 1] * 127.0);
}
}
else if (cSetup == FORMAT_STEREO_QI) {
for (size_t i = 0; i < returnedElems; i++)
{
itarget[i * 2] = int8_t(_currentBuff[i * 2 + 1] * 127.0);
itarget[i * 2 + 1] = int8_t(_currentBuff[i * 2] * 127.0);
}
}
}
}
//bump variables for next call into readStream
bufferedElems -= returnedElems;
_currentBuff += returnedElems * elementsPerSample;
//return number of elements written to buff0
if (bufferedElems != 0) flags |= SOAPY_SDR_MORE_FRAGMENTS;
else this->releaseReadBuffer(stream, _currentHandle);
return returnedElems;
}
int16_t Int16() { return int16_t(Uint16()); }
bool AudioDecoder::resample()
{
if (_result.sampleRate == _sampleRate)
{
ALOGI("No need to resample since the sample rate (%d) of the decoded pcm data is the same as the device output sample rate",
_sampleRate);
return true;
}
ALOGV("Resample: %d --> %d", _result.sampleRate, _sampleRate);
auto r = _result;
PcmBufferProvider provider;
provider.init(r.pcmBuffer->data(), r.numFrames, r.pcmBuffer->size() / r.numFrames);
const int outFrameRate = _sampleRate;
int outputChannels = 2;
size_t outputFrameSize = outputChannels * sizeof(int32_t);
size_t outputFrames = ((int64_t) r.numFrames * outFrameRate) / r.sampleRate;
size_t outputSize = outputFrames * outputFrameSize;
void *outputVAddr = malloc(outputSize);
auto resampler = AudioResampler::create(AUDIO_FORMAT_PCM_16_BIT, r.numChannels, outFrameRate,
AudioResampler::MED_QUALITY);
resampler->setSampleRate(r.sampleRate);
resampler->setVolume(AudioResampler::UNITY_GAIN_FLOAT, AudioResampler::UNITY_GAIN_FLOAT);
memset(outputVAddr, 0, outputSize);
ALOGV("resample() %zu output frames", outputFrames);
std::vector<int> Ovalues;
if (Ovalues.empty())
{
Ovalues.push_back(outputFrames);
}
for (size_t i = 0, j = 0; i < outputFrames;)
{
size_t thisFrames = Ovalues[j++];
if (j >= Ovalues.size())
{
j = 0;
}
if (thisFrames == 0 || thisFrames > outputFrames - i)
{
thisFrames = outputFrames - i;
}
int outFrames = resampler->resample((int *) outputVAddr + outputChannels * i, thisFrames,
&provider);
ALOGV("outFrames: %d", outFrames);
i += thisFrames;
}
ALOGV("resample() complete");
resampler->reset();
ALOGV("reset() complete");
delete resampler;
resampler = nullptr;
// mono takes left channel only (out of stereo output pair)
// stereo and multichannel preserve all channels.
int channels = r.numChannels;
int32_t *out = (int32_t *) outputVAddr;
int16_t *convert = (int16_t *) malloc(outputFrames * channels * sizeof(int16_t));
const int volumeShift = 12; // shift requirement for Q4.27 to Q.15
// round to half towards zero and saturate at int16 (non-dithered)
const int roundVal = (1 << (volumeShift - 1)) - 1; // volumePrecision > 0
for (size_t i = 0; i < outputFrames; i++)
{
for (int j = 0; j < channels; j++)
{
int32_t s = out[i * outputChannels + j] + roundVal; // add offset here
if (s < 0)
{
s = (s + 1) >> volumeShift; // round to 0
if (s < -32768)
{
s = -32768;
}
} else
{
s = s >> volumeShift;
if (s > 32767)
{
s = 32767;
}
}
convert[i * channels + j] = int16_t(s);
}
// uint8_t -> int16_t, lossless
inline void sampleConv(const uint8_t& theSrcSample, int16_t& theOutSample) {
theOutSample = (int16_t(theSrcSample) - 127) << 8;
}
static void QueryValueCompare( DatabaseConnectionSPtr connection, const CDatabaseValue< ValueType > & value, const ParamType & val )
{
BOOST_CHECK_EQUAL( value.GetQueryValue(), StringUtils::ToString( int16_t( val ) ) );
}
int main(int argc, char** argv) {
double temp = 1.0;
// output command for documentation:
int i;
for (i = 0; i < argc; ++i)
printf("%s ", argv[i] );
printf("\n");
if (argc > 1) iterations = atoi(argv[1]);
if (argc > 2) init_value = (double) atof(argv[2]);
if (argc > 3) temp = (double)atof(argv[3]);
// int8_t
::fill(data8, data8+SIZE, int8_t(init_value));
int8_t var1int8_1, var1int8_2, var1int8_3, var1int8_4;
var1int8_1 = int8_t(temp);
var1int8_2 = var1int8_1 * int8_t(2);
var1int8_3 = var1int8_1 + int8_t(2);
var1int8_4 = var1int8_1 + var1int8_2 / var1int8_3;
// test moving redundant calcs out of loop
test_variable1< int8_t, custom_add_variable<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable add");
test_hoisted_variable1< int8_t, custom_add_variable<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable add hoisted");
test_variable4< int8_t, custom_add_multiple_variable<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable adds");
test_variable1< int8_t, custom_sub_variable<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable subtract");
test_variable4< int8_t, custom_sub_multiple_variable<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable subtracts");
test_variable1< int8_t, custom_multiply_variable<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable multiply");
test_variable4< int8_t, custom_multiply_multiple_variable<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable multiplies");
test_variable4< int8_t, custom_multiply_multiple_variable2<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable multiplies2");
test_variable1< int8_t, custom_divide_variable<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable divide");
test_variable4< int8_t, custom_divide_multiple_variable<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable divides");
test_variable4< int8_t, custom_divide_multiple_variable2<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable divides2");
test_variable4< int8_t, custom_mixed_multiple_variable<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable mixed");
test_variable1< int8_t, custom_variable_and<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable and");
test_variable4< int8_t, custom_multiple_variable_and<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable and");
test_variable1< int8_t, custom_variable_or<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable or");
test_variable4< int8_t, custom_multiple_variable_or<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable or");
test_variable1< int8_t, custom_variable_xor<int8_t> > (data8, SIZE, var1int8_1, "int8_t variable xor");
test_variable4< int8_t, custom_multiple_variable_xor<int8_t> > (data8, SIZE, var1int8_1, var1int8_2, var1int8_3, var1int8_4, "int8_t multiple variable xor");
summarize("int8_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// unsigned8
::fill(data8unsigned, data8unsigned+SIZE, uint8_t(init_value));
uint8_t var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4;
var1uint8_1 = uint8_t(temp);
var1uint8_2 = var1uint8_1 * uint8_t(2);
var1uint8_3 = var1uint8_1 + uint8_t(2);
var1uint8_4 = var1uint8_1 + var1uint8_2 / var1uint8_3;
// test moving redundant calcs out of loop
test_variable1< uint8_t, custom_add_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable add");
test_hoisted_variable1< uint8_t, custom_add_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable add hoisted");
test_variable4< uint8_t, custom_add_multiple_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable adds");
test_variable1< uint8_t, custom_sub_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable subtract");
test_variable4< uint8_t, custom_sub_multiple_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable subtracts");
test_variable1< uint8_t, custom_multiply_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable multiply");
test_variable4< uint8_t, custom_multiply_multiple_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable multiplies");
test_variable4< uint8_t, custom_multiply_multiple_variable2<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable multiplies2");
test_variable1< uint8_t, custom_divide_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable divide");
test_variable4< uint8_t, custom_divide_multiple_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable divides");
test_variable4< uint8_t, custom_divide_multiple_variable2<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable divides2");
test_variable4< uint8_t, custom_mixed_multiple_variable<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable mixed");
test_variable1< uint8_t, custom_variable_and<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable and");
test_variable4< uint8_t, custom_multiple_variable_and<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable and");
test_variable1< uint8_t, custom_variable_or<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable or");
test_variable4< uint8_t, custom_multiple_variable_or<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable or");
test_variable1< uint8_t, custom_variable_xor<uint8_t> > (data8unsigned, SIZE, var1uint8_1, "uint8_t variable xor");
test_variable4< uint8_t, custom_multiple_variable_xor<uint8_t> > (data8unsigned, SIZE, var1uint8_1, var1uint8_2, var1uint8_3, var1uint8_4, "uint8_t multiple variable xor");
summarize("uint8_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// int16_t
::fill(data16, data16+SIZE, int16_t(init_value));
int16_t var1int16_1, var1int16_2, var1int16_3, var1int16_4;
var1int16_1 = int16_t(temp);
var1int16_2 = var1int16_1 * int16_t(2);
var1int16_3 = var1int16_1 + int16_t(2);
var1int16_4 = var1int16_1 + var1int16_2 / var1int16_3;
// test moving redundant calcs out of loop
test_variable1< int16_t, custom_add_variable<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable add");
test_hoisted_variable1< int16_t, custom_add_variable<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable add hoisted");
test_variable4< int16_t, custom_add_multiple_variable<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable adds");
test_variable1< int16_t, custom_sub_variable<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable subtract");
test_variable4< int16_t, custom_sub_multiple_variable<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable subtracts");
test_variable1< int16_t, custom_multiply_variable<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable multiply");
test_variable4< int16_t, custom_multiply_multiple_variable<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable multiplies");
test_variable4< int16_t, custom_multiply_multiple_variable2<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable multiplies2");
test_variable1< int16_t, custom_divide_variable<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable divide");
test_variable4< int16_t, custom_divide_multiple_variable<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable divides");
test_variable4< int16_t, custom_divide_multiple_variable2<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable divides2");
test_variable4< int16_t, custom_mixed_multiple_variable<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable mixed");
test_variable1< int16_t, custom_variable_and<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable and");
test_variable4< int16_t, custom_multiple_variable_and<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable and");
test_variable1< int16_t, custom_variable_or<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable or");
test_variable4< int16_t, custom_multiple_variable_or<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable or");
test_variable1< int16_t, custom_variable_xor<int16_t> > (data16, SIZE, var1int16_1, "int16_t variable xor");
test_variable4< int16_t, custom_multiple_variable_xor<int16_t> > (data16, SIZE, var1int16_1, var1int16_2, var1int16_3, var1int16_4, "int16_t multiple variable xor");
summarize("int16_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// unsigned16
::fill(data16unsigned, data16unsigned+SIZE, uint16_t(init_value));
uint16_t var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4;
var1uint16_1 = uint16_t(temp);
var1uint16_2 = var1uint16_1 * uint16_t(2);
var1uint16_3 = var1uint16_1 + uint16_t(2);
var1uint16_4 = var1uint16_1 + var1uint16_2 / var1uint16_3;
// test moving redundant calcs out of loop
test_variable1< uint16_t, custom_add_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable add");
test_hoisted_variable1< uint16_t, custom_add_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable add hoisted");
test_variable4< uint16_t, custom_add_multiple_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable adds");
test_variable1< uint16_t, custom_sub_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable subtract");
test_variable4< uint16_t, custom_sub_multiple_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable subtracts");
test_variable1< uint16_t, custom_multiply_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable multiply");
test_variable4< uint16_t, custom_multiply_multiple_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable multiplies");
test_variable4< uint16_t, custom_multiply_multiple_variable2<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable multiplies2");
test_variable1< uint16_t, custom_divide_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable divide");
test_variable4< uint16_t, custom_divide_multiple_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable divides");
test_variable4< uint16_t, custom_divide_multiple_variable2<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable divides2");
test_variable4< uint16_t, custom_mixed_multiple_variable<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable mixed");
test_variable1< uint16_t, custom_variable_and<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable and");
test_variable4< uint16_t, custom_multiple_variable_and<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable and");
test_variable1< uint16_t, custom_variable_or<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable or");
test_variable4< uint16_t, custom_multiple_variable_or<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable or");
test_variable1< uint16_t, custom_variable_xor<uint16_t> > (data16unsigned, SIZE, var1uint16_1, "uint16_t variable xor");
test_variable4< uint16_t, custom_multiple_variable_xor<uint16_t> > (data16unsigned, SIZE, var1uint16_1, var1uint16_2, var1uint16_3, var1uint16_4, "uint16_t multiple variable xor");
summarize("uint16_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// int32_t
::fill(data32, data32+SIZE, int32_t(init_value));
int32_t var1int32_1, var1int32_2, var1int32_3, var1int32_4;
var1int32_1 = int32_t(temp);
var1int32_2 = var1int32_1 * int32_t(2);
var1int32_3 = var1int32_1 + int32_t(2);
var1int32_4 = var1int32_1 + var1int32_2 / var1int32_3;
// test moving redundant calcs out of loop
test_variable1< int32_t, custom_add_variable<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable add");
test_hoisted_variable1< int32_t, custom_add_variable<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable add hoisted");
test_variable4< int32_t, custom_add_multiple_variable<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable adds");
test_variable1< int32_t, custom_sub_variable<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable subtract");
test_variable4< int32_t, custom_sub_multiple_variable<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable subtracts");
test_variable1< int32_t, custom_multiply_variable<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable multiply");
test_variable4< int32_t, custom_multiply_multiple_variable<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable multiplies");
test_variable4< int32_t, custom_multiply_multiple_variable2<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable multiplies2");
test_variable1< int32_t, custom_divide_variable<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable divide");
test_variable4< int32_t, custom_divide_multiple_variable<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable divides");
test_variable4< int32_t, custom_divide_multiple_variable2<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable divides2");
test_variable4< int32_t, custom_mixed_multiple_variable<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable mixed");
test_variable1< int32_t, custom_variable_and<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable and");
test_variable4< int32_t, custom_multiple_variable_and<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable and");
test_variable1< int32_t, custom_variable_or<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable or");
test_variable4< int32_t, custom_multiple_variable_or<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable or");
test_variable1< int32_t, custom_variable_xor<int32_t> > (data32, SIZE, var1int32_1, "int32_t variable xor");
test_variable4< int32_t, custom_multiple_variable_xor<int32_t> > (data32, SIZE, var1int32_1, var1int32_2, var1int32_3, var1int32_4, "int32_t multiple variable xor");
summarize("int32_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// unsigned32
::fill(data32unsigned, data32unsigned+SIZE, uint32_t(init_value));
uint32_t var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4;
var1uint32_1 = uint32_t(temp);
var1uint32_2 = var1uint32_1 * uint32_t(2);
var1uint32_3 = var1uint32_1 + uint32_t(2);
var1uint32_4 = var1uint32_1 + var1uint32_2 / var1uint32_3;
// test moving redundant calcs out of loop
test_variable1< uint32_t, custom_add_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable add");
test_hoisted_variable1< uint32_t, custom_add_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable add hoisted");
test_variable4< uint32_t, custom_add_multiple_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable adds");
test_variable1< uint32_t, custom_sub_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable subtract");
test_variable4< uint32_t, custom_sub_multiple_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable subtracts");
test_variable1< uint32_t, custom_multiply_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable multiply");
test_variable4< uint32_t, custom_multiply_multiple_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable multiplies");
test_variable4< uint32_t, custom_multiply_multiple_variable2<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable multiplies2");
test_variable1< uint32_t, custom_divide_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable divide");
test_variable4< uint32_t, custom_divide_multiple_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable divides");
test_variable4< uint32_t, custom_divide_multiple_variable2<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable divides2");
test_variable4< uint32_t, custom_mixed_multiple_variable<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable mixed");
test_variable1< uint32_t, custom_variable_and<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable and");
test_variable4< uint32_t, custom_multiple_variable_and<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable and");
test_variable1< uint32_t, custom_variable_or<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable or");
test_variable4< uint32_t, custom_multiple_variable_or<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable or");
test_variable1< uint32_t, custom_variable_xor<uint32_t> > (data32unsigned, SIZE, var1uint32_1, "uint32_t variable xor");
test_variable4< uint32_t, custom_multiple_variable_xor<uint32_t> > (data32unsigned, SIZE, var1uint32_1, var1uint32_2, var1uint32_3, var1uint32_4, "uint32_t multiple variable xor");
summarize("uint32_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// int64_t
::fill(data64, data64+SIZE, int64_t(init_value));
int64_t var1int64_1, var1int64_2, var1int64_3, var1int64_4;
var1int64_1 = int64_t(temp);
var1int64_2 = var1int64_1 * int64_t(2);
var1int64_3 = var1int64_1 + int64_t(2);
var1int64_4 = var1int64_1 + var1int64_2 / var1int64_3;
// test moving redundant calcs out of loop
test_variable1< int64_t, custom_add_variable<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable add");
test_hoisted_variable1< int64_t, custom_add_variable<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable add hoisted");
test_variable4< int64_t, custom_add_multiple_variable<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable adds");
test_variable1< int64_t, custom_sub_variable<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable subtract");
test_variable4< int64_t, custom_sub_multiple_variable<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable subtracts");
test_variable1< int64_t, custom_multiply_variable<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable multiply");
test_variable4< int64_t, custom_multiply_multiple_variable<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable multiplies");
test_variable4< int64_t, custom_multiply_multiple_variable2<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable multiplies2");
test_variable1< int64_t, custom_divide_variable<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable divide");
test_variable4< int64_t, custom_divide_multiple_variable<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable divides");
test_variable4< int64_t, custom_divide_multiple_variable2<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable divides2");
test_variable4< int64_t, custom_mixed_multiple_variable<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable mixed");
test_variable1< int64_t, custom_variable_and<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable and");
test_variable4< int64_t, custom_multiple_variable_and<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable and");
test_variable1< int64_t, custom_variable_or<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable or");
test_variable4< int64_t, custom_multiple_variable_or<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable or");
test_variable1< int64_t, custom_variable_xor<int64_t> > (data64, SIZE, var1int64_1, "int64_t variable xor");
test_variable4< int64_t, custom_multiple_variable_xor<int64_t> > (data64, SIZE, var1int64_1, var1int64_2, var1int64_3, var1int64_4, "int64_t multiple variable xor");
summarize("int64_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// unsigned64
::fill(data64unsigned, data64unsigned+SIZE, uint64_t(init_value));
uint64_t var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4;
var1uint64_1 = uint64_t(temp);
var1uint64_2 = var1uint64_1 * uint64_t(2);
var1uint64_3 = var1uint64_1 + uint64_t(2);
var1uint64_4 = var1uint64_1 + var1uint64_2 / var1uint64_3;
// test moving redundant calcs out of loop
test_variable1< uint64_t, custom_add_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable add");
test_hoisted_variable1< uint64_t, custom_add_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable add hoisted");
test_variable4< uint64_t, custom_add_multiple_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable adds");
test_variable1< uint64_t, custom_sub_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable subtract");
test_variable4< uint64_t, custom_sub_multiple_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable subtracts");
test_variable1< uint64_t, custom_multiply_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable multiply");
test_variable4< uint64_t, custom_multiply_multiple_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable multiplies");
test_variable4< uint64_t, custom_multiply_multiple_variable2<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable multiplies2");
test_variable1< uint64_t, custom_divide_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable divide");
test_variable4< uint64_t, custom_divide_multiple_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable divides");
test_variable4< uint64_t, custom_divide_multiple_variable2<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable divides2");
test_variable4< uint64_t, custom_mixed_multiple_variable<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable mixed");
test_variable1< uint64_t, custom_variable_and<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable and");
test_variable4< uint64_t, custom_multiple_variable_and<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable and");
test_variable1< uint64_t, custom_variable_or<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable or");
test_variable4< uint64_t, custom_multiple_variable_or<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable or");
test_variable1< uint64_t, custom_variable_xor<uint64_t> > (data64unsigned, SIZE, var1uint64_1, "uint64_t variable xor");
test_variable4< uint64_t, custom_multiple_variable_xor<uint64_t> > (data64unsigned, SIZE, var1uint64_1, var1uint64_2, var1uint64_3, var1uint64_4, "uint64_t multiple variable xor");
summarize("uint64_t loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// float
::fill(dataFloat, dataFloat+SIZE, float(init_value));
float var1Float_1, var1Float_2, var1Float_3, var1Float_4;
var1Float_1 = float(temp);
var1Float_2 = var1Float_1 * float(2.0);
var1Float_3 = var1Float_1 + float(2.0);
var1Float_4 = var1Float_1 + var1Float_2 / var1Float_3;
// test moving redundant calcs out of loop
test_variable1< float, custom_add_variable<float> > (dataFloat, SIZE, var1Float_1, "float variable add");
test_hoisted_variable1< float, custom_add_variable<float> > (dataFloat, SIZE, var1Float_1, "float variable add hoisted");
test_variable4< float, custom_add_multiple_variable<float> > (dataFloat, SIZE, var1Float_1, var1Float_2, var1Float_3, var1Float_4, "float multiple variable adds");
test_variable1< float, custom_sub_variable<float> > (dataFloat, SIZE, var1Float_1, "float variable subtract");
test_variable4< float, custom_sub_multiple_variable<float> > (dataFloat, SIZE, var1Float_1, var1Float_2, var1Float_3, var1Float_4, "float multiple variable subtracts");
test_variable1< float, custom_multiply_variable<float> > (dataFloat, SIZE, var1Float_1, "float variable multiply");
test_variable4< float, custom_multiply_multiple_variable<float> > (dataFloat, SIZE, var1Float_1, var1Float_2, var1Float_3, var1Float_4, "float multiple variable multiplies");
test_variable4< float, custom_multiply_multiple_variable2<float> > (dataFloat, SIZE, var1Float_1, var1Float_2, var1Float_3, var1Float_4, "float multiple variable multiplies2");
test_variable1< float, custom_divide_variable<float> > (dataFloat, SIZE, var1Float_1, "float variable divide");
test_variable4< float, custom_divide_multiple_variable<float> > (dataFloat, SIZE, var1Float_1, var1Float_2, var1Float_3, var1Float_4, "float multiple variable divides");
test_variable4< float, custom_divide_multiple_variable2<float> > (dataFloat, SIZE, var1Float_1, var1Float_2, var1Float_3, var1Float_4, "float multiple variable divides2");
test_variable4< float, custom_mixed_multiple_variable<float> > (dataFloat, SIZE, var1Float_1, var1Float_2, var1Float_3, var1Float_4, "float multiple variable mixed");
summarize("float loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
// double
::fill(dataDouble, dataDouble+SIZE, double(init_value));
double var1Double_1, var1Double_2, var1Double_3, var1Double_4;
var1Double_1 = double(temp);
var1Double_2 = var1Double_1 * double(2.0);
var1Double_3 = var1Double_1 + double(2.0);
var1Double_4 = var1Double_1 + var1Double_2 / var1Double_3;
// test moving redundant calcs out of loop
test_variable1< double, custom_add_variable<double> > (dataDouble, SIZE, var1Double_1, "double variable add");
test_hoisted_variable1< double, custom_add_variable<double> > (dataDouble, SIZE, var1Double_1, "double variable add hoisted");
test_variable4< double, custom_add_multiple_variable<double> > (dataDouble, SIZE, var1Double_1, var1Double_2, var1Double_3, var1Double_4, "double multiple variable adds");
test_variable1< double, custom_sub_variable<double> > (dataDouble, SIZE, var1Double_1, "double variable subtract");
test_variable4< double, custom_sub_multiple_variable<double> > (dataDouble, SIZE, var1Double_1, var1Double_2, var1Double_3, var1Double_4, "double multiple variable subtracts");
test_variable1< double, custom_multiply_variable<double> > (dataDouble, SIZE, var1Double_1, "double variable multiply");
test_variable4< double, custom_multiply_multiple_variable<double> > (dataDouble, SIZE, var1Double_1, var1Double_2, var1Double_3, var1Double_4, "double multiple variable multiplies");
test_variable4< double, custom_multiply_multiple_variable2<double> > (dataDouble, SIZE, var1Double_1, var1Double_2, var1Double_3, var1Double_4, "double multiple variable multiplies2");
test_variable1< double, custom_divide_variable<double> > (dataDouble, SIZE, var1Double_1, "double variable divide");
test_variable4< double, custom_divide_multiple_variable<double> > (dataDouble, SIZE, var1Double_1, var1Double_2, var1Double_3, var1Double_4, "double multiple variable divides");
test_variable4< double, custom_divide_multiple_variable2<double> > (dataDouble, SIZE, var1Double_1, var1Double_2, var1Double_3, var1Double_4, "double multiple variable divides2");
test_variable4< double, custom_mixed_multiple_variable<double> > (dataDouble, SIZE, var1Double_1, var1Double_2, var1Double_3, var1Double_4, "double multiple variable mixed");
summarize("double loop invariant", SIZE, iterations, kDontShowGMeans, kDontShowPenalty );
return 0;
}
IReadFloat(s, dst, fFloats[field][chan].fOffset, kQuanta[field]);
else
{
*((float*)dst) = fFloats[field][chan].fOffset;
dst += 4;
}
fFloats[field][chan].fCount--;
}
static inline int INumWeights(const uint8_t format)
{
return (format & plGBufferGroup::kSkinWeightMask) >> 4;
}
static const float kNormalScale(int16_t(0x7fff));
static const float kInvNormalScale(1.f / kNormalScale);
inline void plVertCoder::IEncodeNormal(hsStream* s, const uint8_t*& src, const uint32_t stride)
{
float x = *(float*)src;
s->WriteByte((uint8_t)((x / 2.f + .5f) * 255.9f));
src += 4;
x = *(float*)src;
s->WriteByte((uint8_t)((x / 2.f + .5f) * 255.9f));
src += 4;
x = *(float*)src;
s->WriteByte((uint8_t)((x / 2.f + .5f) * 255.9f));
extern "C" void stm32_adc12(void) {
PERF_COUNT_START
static float last_v_ab[2];
static float prev_v_ab[2];
static uint8_t last_pwm_sector = 1u;
static uint8_t prev_pwm_sector = 1u;
int16_t phase_current_lsb[3];
uint16_t phase_oc[3];
float out_v_ab[2], i_ab[2];
float temp;
hal_read_phase_shunts_(phase_current_lsb, prev_pwm_sector);
/*
Clarke transformation for balanced systems
i_alpha = i_a,
i_beta = (2 * i_b + i_a) / sqrt(3)
Multiply by 8 because the phase current readings are right-aligned.
*/
i_ab[0] = float(phase_current_lsb[0]) *
float(hal_full_scale_current_a * 8.0 / 32768.0);
temp = float(phase_current_lsb[1]) *
float(hal_full_scale_current_a * 8.0 / 32768.0);
i_ab[1] = (0.57735026919f * i_ab[0] + 1.15470053838f * temp);
out_v_ab[0] = out_v_ab[1] = 0.0f;
if (high_frequency_task_) {
high_frequency_task_(out_v_ab, prev_v_ab, i_ab, vbus_v_);
}
prev_v_ab[0] = last_v_ab[0];
prev_v_ab[1] = last_v_ab[1];
last_v_ab[0] = out_v_ab[0];
last_v_ab[1] = out_v_ab[1];
prev_pwm_sector = last_pwm_sector;
/*
Convert alpha-beta frame voltage fractions to SVM output compare values
for each phase.
*/
temp = vbus_inv_;
last_pwm_sector = svm_duty_cycle_from_v_alpha_beta(
phase_oc,
int16_t(__SSAT(int32_t(temp * out_v_ab[0]), 16u)),
int16_t(__SSAT(int32_t(temp * out_v_ab[1]), 16u)),
hal_pwm_period_ticks);
/* Update the timer */
hal_update_timer_(last_pwm_sector, phase_oc);
/*
Clear the JEOS event, and prepare for the next hardware trigger
ADC_ClearFlag(ADC1, ADC_FLAG_JEOS);
*/
putreg32(ADC_INT_JEOS, STM32_ADC1_ISR);
/*
Allow the next ADC conversion to happen based on the TIM1 CC4 event
ADC_StartInjectedConversion(ADC1);
*/
putreg32(getreg32(STM32_ADC1_CR) | ADC_CR_JADSTART, STM32_ADC1_CR);
PERF_COUNT_END
}
Joystick Joystick::deserialize(std::string input){
Joystick joy;
joy.is_xbox = stob(pullObject("\"is_xbox\"", input));
joy.type = std::stoi(pullObject("\"type\"",input));
joy.name = unquote(pullObject("\"name\"", input));
joy.buttons = std::stoi(pullObject("\"buttons\"", input));
joy.button_count = std::stoi(pullObject("\"button_count\"", input));
std::vector<int8_t> axes_deserialized = deserializeList(pullObject("\"axes\"",input), std::function<int8_t(std::string)>([&](std::string input){ return std::stoi(input);}), true);
if(axes_deserialized.size() == joy.axes.size()){
joy.axes = axes_deserialized;
} else {
throw std::out_of_range("Exception: deserialization resulted in array of " + std::to_string(axes_deserialized.size()) + " axes, expected " + std::to_string(joy.axes.size()));
}
joy.axis_count = std::stoi(pullObject("\"axis_count\"", input));
std::vector<uint8_t> axis_types_deserialized = deserializeList(pullObject("\"axis_types\"",input), std::function<uint8_t(std::string)>([&](std::string input){ return std::stoi(input);}), true);
if(axis_types_deserialized.size() == joy.axis_types.size()){
joy.axis_types = axis_types_deserialized;
} else {
throw std::out_of_range("Exception: deserialization resulted in array of " + std::to_string(axis_types_deserialized.size()) + " axis types, expected " + std::to_string(joy.axis_types.size()));
}
std::vector<int16_t> povs_deserialized = deserializeList(pullObject("\"povs\"",input), std::function<int16_t(std::string)>([&](std::string input){ return std::stoi(input);}), true);
if(povs_deserialized.size() == joy.povs.size()){
joy.povs = povs_deserialized;
} else {
throw std::out_of_range("Exception: deserialization resulted in array of " + std::to_string(povs_deserialized.size()) + " povs, expected " + std::to_string(joy.povs.size()));
}
joy.pov_count = std::stoi(pullObject("\"pov_count\"", input));
joy.outputs = std::stoi(pullObject("\"outputs\"", input));
joy.left_rumble = std::stoi(pullObject("\"left_rumble\"", input));
joy.right_rumble = std::stoi(pullObject("\"right_rumble\"", input));
return joy;
}
int16_t int16()
{
return int16_t();
}
// Returns the raw gyro data
void TiltyIMU::getRawGyro(int *data)
{
data[0] = int16_t((fifoBuffer[16] << 8) | fifoBuffer[17]);
data[1] = int16_t((fifoBuffer[20] << 8) | fifoBuffer[21]);
data[2] = int16_t((fifoBuffer[24] << 8) | fifoBuffer[25]);
}
Entry* probe(const Position& pos) {
Key key = pos.material_key();
Entry* e = pos.this_thread()->materialTable[key];
if (e->key == key)
return e;
std::memset(e, 0, sizeof(Entry));
e->key = key;
e->factor[WHITE] = e->factor[BLACK] = (uint8_t)SCALE_FACTOR_NORMAL;
e->gamePhase = pos.game_phase();
// Let's look if we have a specialized evaluation function for this particular
// material configuration. Firstly we look for a fixed configuration one, then
// for a generic one if the previous search failed.
if ((e->evaluationFunction = pos.this_thread()->endgames.probe<Value>(key)) != nullptr)
return e;
for (Color c = WHITE; c <= BLACK; ++c)
if (is_KXK(pos, c))
{
e->evaluationFunction = &EvaluateKXK[c];
return e;
}
// OK, we didn't find any special evaluation function for the current material
// configuration. Is there a suitable specialized scaling function?
EndgameBase<ScaleFactor>* sf;
if ((sf = pos.this_thread()->endgames.probe<ScaleFactor>(key)) != nullptr)
{
e->scalingFunction[sf->strong_side()] = sf; // Only strong color assigned
return e;
}
// We didn't find any specialized scaling function, so fall back on generic
// ones that refer to more than one material distribution. Note that in this
// case we don't return after setting the function.
for (Color c = WHITE; c <= BLACK; ++c)
{
if (is_KBPsKs(pos, c))
e->scalingFunction[c] = &ScaleKBPsK[c];
else if (is_KQKRPs(pos, c))
e->scalingFunction[c] = &ScaleKQKRPs[c];
}
Value npm_w = pos.non_pawn_material(WHITE);
Value npm_b = pos.non_pawn_material(BLACK);
if (npm_w + npm_b == VALUE_ZERO && pos.pieces(PAWN)) // Only pawns on the board
{
if (!pos.count<PAWN>(BLACK))
{
assert(pos.count<PAWN>(WHITE) >= 2);
e->scalingFunction[WHITE] = &ScaleKPsK[WHITE];
}
else if (!pos.count<PAWN>(WHITE))
{
assert(pos.count<PAWN>(BLACK) >= 2);
e->scalingFunction[BLACK] = &ScaleKPsK[BLACK];
}
else if (pos.count<PAWN>(WHITE) == 1 && pos.count<PAWN>(BLACK) == 1)
{
// This is a special case because we set scaling functions
// for both colors instead of only one.
e->scalingFunction[WHITE] = &ScaleKPKP[WHITE];
e->scalingFunction[BLACK] = &ScaleKPKP[BLACK];
}
}
// Zero or just one pawn makes it difficult to win, even with a small material
// advantage. This catches some trivial draws like KK, KBK and KNK and gives a
// drawish scale factor for cases such as KRKBP and KmmKm (except for KBBKN).
if (!pos.count<PAWN>(WHITE) && npm_w - npm_b <= BishopValueMg)
e->factor[WHITE] = uint8_t(npm_w < RookValueMg ? SCALE_FACTOR_DRAW :
npm_b <= BishopValueMg ? 4 : 14);
if (!pos.count<PAWN>(BLACK) && npm_b - npm_w <= BishopValueMg)
e->factor[BLACK] = uint8_t(npm_b < RookValueMg ? SCALE_FACTOR_DRAW :
npm_w <= BishopValueMg ? 4 : 14);
if (pos.count<PAWN>(WHITE) == 1 && npm_w - npm_b <= BishopValueMg)
e->factor[WHITE] = (uint8_t) SCALE_FACTOR_ONEPAWN;
if (pos.count<PAWN>(BLACK) == 1 && npm_b - npm_w <= BishopValueMg)
e->factor[BLACK] = (uint8_t) SCALE_FACTOR_ONEPAWN;
// Evaluate the material imbalance. We use PIECE_TYPE_NONE as a place holder
// for the bishop pair "extended piece", which allows us to be more flexible
// in defining bishop pair bonuses.
const int PieceCount[COLOR_NB][PIECE_TYPE_NB] = {
{ pos.count<BISHOP>(WHITE) > 1, pos.count<PAWN>(WHITE), pos.count<KNIGHT>(WHITE),
pos.count<BISHOP>(WHITE) , pos.count<ROOK>(WHITE), pos.count<QUEEN >(WHITE) },
{ pos.count<BISHOP>(BLACK) > 1, pos.count<PAWN>(BLACK), pos.count<KNIGHT>(BLACK),
pos.count<BISHOP>(BLACK) , pos.count<ROOK>(BLACK), pos.count<QUEEN >(BLACK) } };
e->value = int16_t((imbalance<WHITE>(PieceCount) - imbalance<BLACK>(PieceCount)) / 16);
return e;
}
// Returns the raw accelerometer data
void TiltyIMU::getRawAccel(int *data)
{
data[0] = int16_t((fifoBuffer[28] << 8) | fifoBuffer[29]);
data[1] = int16_t((fifoBuffer[32] << 8) | fifoBuffer[33]);
data[2] = int16_t((fifoBuffer[36] << 8) | fifoBuffer[37]);
}
template <size_t channelCount> void ResizeBilinear(
const uint8_t *src, size_t srcWidth, size_t srcHeight, size_t srcStride,
uint8_t *dst, size_t dstWidth, size_t dstHeight, size_t dstStride)
{
assert(dstWidth >= A);
struct One { uint8_t channels[channelCount]; };
struct Two { uint8_t channels[channelCount*2]; };
size_t size = 2*dstWidth*channelCount;
size_t bufferSize = AlignHi(dstWidth, A)*channelCount*2;
size_t alignedSize = AlignHi(size, DA) - DA;
Buffer buffer(bufferSize, dstWidth, dstHeight);
const size_t stepB = A/channelCount;
const size_t stepA = DA/channelCount;
size_t bufferWidth = AlignHi(dstWidth, stepB);
Base::EstimateAlphaIndex(srcHeight, dstHeight, buffer.iy, buffer.ay, 1);
EstimateAlphaIndexX(srcWidth, dstWidth, buffer.ix, buffer.ax);
ptrdiff_t previous = -2;
__m128i a[2];
for(size_t yDst = 0; yDst < dstHeight; yDst++, dst += dstStride)
{
a[0] = _mm_set1_epi16(int16_t(Base::FRACTION_RANGE - buffer.ay[yDst]));
a[1] = _mm_set1_epi16(int16_t(buffer.ay[yDst]));
ptrdiff_t sy = buffer.iy[yDst];
int k = 0;
if(sy == previous)
k = 2;
else if(sy == previous + 1)
{
Swap(buffer.bx[0], buffer.bx[1]);
k = 1;
}
previous = sy;
for(; k < 2; k++)
{
Two * pb = (Two *)buffer.bx[k];
const One * ps = (const One *)(src + (sy + k)*srcStride);
for(size_t x = 0; x < dstWidth; x++)
pb[x] = *(Two *)(ps + buffer.ix[x]);
for(size_t ib = 0, ia = 0; ib < bufferWidth; ib += stepB, ia += stepA)
InterpolateX<channelCount>((__m128i*)(buffer.ax + ia), (__m128i*)(pb + ib));
}
for(size_t ib = 0, id = 0; ib < alignedSize; ib += DA, id += A)
InterpolateY<true>(buffer.bx[0] + ib, buffer.bx[1] + ib, a, dst + id);
size_t i = size - DA;
InterpolateY<false>(buffer.bx[0] + i, buffer.bx[1] + i, a, dst + i/2);
}
}
/* This just improves encoder performance, it's not part of the spec */
void x264_mb_predict_mv_ref16x16( x264_t *h, int i_ref, int16_t mvc[9][2], int *i_mvc )
{
int16_t (*mvr)[2] = h->mb.mvr[i_ref];
int i = 0;
#define SET_MVP(mvp) \
{ \
CP32( mvc[i], mvp ); \
i++; \
}
#define SET_IMVP(xy) \
if( xy >= 0 ) \
{ \
int shift = 1 - h->mb.field[xy]; \
int16_t *mvp = h->mb.mvr[i_ref<<1>>shift][xy]; \
mvc[i][0] = mvp[0]; \
mvc[i][1] = mvp[1]<<1>>shift; \
i++; \
}
if( i_ref == 0 && h->frames.b_have_lowres )
{
int idx = h->fenc->i_frame-h->fref[0]->i_frame-1;
if( idx <= 0 )
{
int16_t (*lowres_mv)[2] = h->fenc->lowres_mvs[0][idx];
if( lowres_mv[0][0] != 0x7fff )
{
M32( mvc[i] ) = (M32( lowres_mv[h->mb.i_mb_xy] )*2)&0xfffeffff;
i++;
}
}
}
/* spatial predictors */
{
SET_MVP( mvr[h->mb.i_mb_left_xy[0]] );
SET_MVP( mvr[h->mb.i_mb_top_xy] );
SET_MVP( mvr[h->mb.i_mb_topleft_xy] );
SET_MVP( mvr[h->mb.i_mb_topright_xy] );
}
#undef SET_IMVP
#undef SET_MVP
/* temporal predictors */
if( h->fref[0]->i_ref > 0 )
{
x264_frame_t *l0 = h->fref[0];
int field = h->mb.i_mb_y&1;
int curpoc = h->fdec->i_poc + h->fdec->i_delta_poc[field];
int refpoc = h->fref[i_ref]->i_poc;
refpoc += l0->i_delta_poc[field^(i_ref&1)];
#define SET_TMVP( dx, dy ) \
{ \
int mb_index = h->mb.i_mb_xy + dx + dy*h->mb.i_mb_stride; \
int scale = (curpoc - refpoc) * l0->inv_ref_poc; \
mvc[i][0] = (l0->mv16x16[mb_index][0]*scale + 128) >> 8; \
mvc[i][1] = (l0->mv16x16[mb_index][1]*scale + 128) >> 8; \
i++; \
}
SET_TMVP(0,0);
if( h->mb.i_mb_x < h->mb.i_mb_width-1 )
SET_TMVP(1,0);
if( h->mb.i_mb_y < h->mb.i_mb_height-1 )
SET_TMVP(0,1);
#undef SET_TMVP
}
*i_mvc = i;
}
static bool
ParseData(NMEAInputLine &line, NMEAInfo &info)
{
// $PCPROBE,T,Q0,Q1,Q2,Q3,ax,ay,az,temp,rh,batt,delta_press,abs_press,C,
// see http://www.compassitaly.com/CMS/index.php/en/download/c-probe/231-c-probeusermanual/download
unsigned _q[4];
bool q_available = line.ReadHexChecked(_q[0]);
if (!line.ReadHexChecked(_q[1]))
q_available = false;
if (!line.ReadHexChecked(_q[2]))
q_available = false;
if (!line.ReadHexChecked(_q[3]))
q_available = false;
if (q_available) {
double q[4];
for (unsigned i = 0; i < 4; ++i)
// Cast to int16_t to interpret the 16th bit as the sign bit
q[i] = int16_t(_q[i]) / 1000.;
double sin_pitch = -2 * (q[0] * q[2] - q[3] * q[1]);
if (sin_pitch <= 1 && sin_pitch >= -1) {
info.attitude.pitch_angle_available.Update(info.clock);
info.attitude.pitch_angle = Angle::asin(sin_pitch);
Angle heading = Angle::HalfCircle() +
Angle::FromXY(Square(q[3]) - Square(q[0]) - Square(q[1]) + Square(q[2]),
2 * (q[1] * q[2] + q[3] * q[0]));
info.attitude.heading_available.Update(info.clock);
info.attitude.heading = heading;
Angle roll = Angle::FromXY(Square(q[3]) + Square(q[0]) - Square(q[1]) - Square(q[2]),
2 * (q[0] * q[1] + q[3] * q[2]));
info.attitude.bank_angle_available.Update(info.clock);
info.attitude.bank_angle = roll;
}
}
unsigned _a[3];
bool a_available = line.ReadHexChecked(_a[0]);
if (!line.ReadHexChecked(_a[1]))
a_available = false;
if (!line.ReadHexChecked(_a[2]))
a_available = false;
if (a_available) {
double a[3];
for (unsigned i = 0; i < 3; ++i)
// Cast to int16_t to interpret the 16th bit as the sign bit
a[i] = int16_t(_a[i]) / 1000.;
info.acceleration.ProvideGLoad(SpaceDiagonal(a[0], a[1], a[2]),
true);
}
unsigned temperature;
if (line.ReadHexChecked(temperature)) {
info.temperature_available = true;
info.temperature = Temperature::FromCelsius(int16_t(temperature) / 10.);
}
unsigned humidity;
if (line.ReadHexChecked(humidity)) {
info.humidity_available = true;
info.humidity = int16_t(humidity) / 10.;
}
unsigned battery_level;
if (line.ReadHexChecked(battery_level)) {
info.battery_level_available.Update(info.clock);
info.battery_level = int16_t(battery_level);
}
unsigned _delta_pressure;
if (line.ReadHexChecked(_delta_pressure)) {
auto delta_pressure = int16_t(_delta_pressure) / 10.;
info.ProvideDynamicPressure(AtmosphericPressure::Pascal(delta_pressure));
}
return true;
}
void Plane::read_radio()
{
if (!hal.rcin->new_input()) {
control_failsafe(channel_throttle->get_radio_in());
return;
}
if(!failsafe.ch3_failsafe)
{
failsafe.AFS_last_valid_rc_ms = millis();
}
failsafe.last_valid_rc_ms = millis();
elevon.ch1_temp = channel_roll->read();
elevon.ch2_temp = channel_pitch->read();
uint16_t pwm_roll, pwm_pitch;
if (g.mix_mode == 0) {
pwm_roll = elevon.ch1_temp;
pwm_pitch = elevon.ch2_temp;
}else{
pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2)
- BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2)
+ BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500;
}
RC_Channel::set_pwm_all();
if (control_mode == TRAINING) {
// in training mode we don't want to use a deadzone, as we
// want manual pass through when not exceeding attitude limits
channel_roll->set_pwm_no_deadzone(pwm_roll);
channel_pitch->set_pwm_no_deadzone(pwm_pitch);
channel_throttle->set_pwm_no_deadzone(channel_throttle->read());
channel_rudder->set_pwm_no_deadzone(channel_rudder->read());
} else {
channel_roll->set_pwm(pwm_roll);
channel_pitch->set_pwm(pwm_pitch);
}
control_failsafe(channel_throttle->get_radio_in());
channel_throttle->set_servo_out(channel_throttle->get_control_in());
if (g.throttle_nudge && channel_throttle->get_servo_out() > 50 && geofence_stickmixing()) {
float nudge = (channel_throttle->get_servo_out() - 50) * 0.02f;
if (ahrs.airspeed_sensor_enabled()) {
airspeed_nudge_cm = (aparm.airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
} else {
throttle_nudge = (aparm.throttle_max - aparm.throttle_cruise) * nudge;
}
} else {
airspeed_nudge_cm = 0;
throttle_nudge = 0;
}
rudder_arm_disarm_check();
if (g.rudder_only != 0) {
// in rudder only mode we discard rudder input and get target
// attitude from the roll channel.
rudder_input = 0;
} else {
rudder_input = channel_rudder->get_control_in();
}
// check for transmitter tuning changes
tuning.check_input(control_mode);
}
bool DecodeADPCMBlock()
{
ASSERT_M(m_iBufferUsed == m_iBufferAvail, ssprintf("%i", m_iBufferUsed));
m_iBufferUsed = m_iBufferAvail = 0;
int8_t iPredictor[2];
int16_t iDelta[2], iSamp1[2], iSamp2[2];
for (int i = 0; i < m_WavData.m_iChannels; ++i)
iPredicator[i] = FileReading::read_8(m_File, m_sError);
for (int i = 0; i < m_WavData.m_iChannels; ++i)
iDelta[i] = FileReading::read_16_le(m_File, m_sError);
for (int i = 0; i < m_WavData.m_iChannels; ++i)
iSamp1[i] = FileReading::read_16_le(m_File, m_sError);
for (int i = 0; i < m_WavData.m_iChannels; ++i)
iSamp2[i] = FileReading::read_16_le(m_File, m_sError);
if (m_sError.size() != 0)
return false;
if (m_File.Tell() >= m_WavData.m_iDataChunkSize + m_WavData.m_iDataChunkPos || m_File.AtEOF())
return true;
float* pBuffer = m_pBuffer;
int iCoef1[2], iCoef2[2];
for (int i = 0; i < m_WavData.m_iChannels; ++i)
{
if (iPredictor[i] >= (int)m_iaCoef1.size())
{
LOG->Trace( "%s: predictor out of range", m_File.GetDisplayPath().c_str() );
iPredictor[i] = 0;
}
iCoef1[i] = m_iaCoef1[iPredicator[i]];
iCoef2[i] = m_iaCoef2[iPredicator[i]];
}
int iMaxSize = min((int)m_WavData.m_iBlockAlign - 7 * m_WavData.m_iChannels, (m_WavData.m_iDataChunkSize + m_WavData.m_iDataChunkPos) - m_File.Tell())
char* pBuf = (char*)alloca(iMaxSize);
ASSERT(pBuf != NULL);
int iBlockSize = m_File.Read(pBuf, iMaxSize);
if (iBlockSize == 0)
return true;
if (iBlockSize == -1)
{
m_sError = m_File.GetError();
return false;
}
for (int i = 0; i < m_WavData.m_iChannels; ++i)
pBuffer[m_iBufferAvail++] = iSamp2[i] / 32768.0f;
for (int i = 0; i < m_WavData.m_iChannels; ++i)
pBuffer[m_iBufferAvail++] = iSamp1[i] / 32768.0f;
int8_t iBuf = 0, iBufSize = 0;
bool bDone = false;
for (int i = 2; !bDone && i < m_iFramePerBlock; ++i)
{
for (int c = 0; !bDone && c < m_WavData.m_iChannels; ++c)
{
if (iBufSize == 0)
{
if (!BlockSize)
{
bDone = true;
continue;
}
iBuf = *pBuf;
++pBuf;
--iBlockSize;
iBufSize = 2;
}
int iErrorDelta = iBuf >> 4;
iBuf <<= 4;
--iBufSize;
int32_t pPredSample = (iSamp1[c] * iCoef1[c] + iSamp2[c] * iCoef2[c]) / (1 << 8);
int32_t iNewSample = iPredSample + (iDelta[c] * iErrorDelta);
iNewSample = clamp(iNewSample, -32768, 32768);
pBuffer[m_iBufferAvail++] = iNewSample / 32768.0f;
static const int aAdaptionTable[] = {
768, 614, 512, 409, 307, 230, 230, 230,
230, 230, 230, 230, 307, 409, 512, 614
}
iDelta[c] = int16_t( (iDelta[c] * aAdaptionTable[iErrorDelta+8]) / (1<<8) );
iDelta[c] = max( (int16_t) 16, iDelta[c] );
iSamp2[c] = iSamp1[c];
iSamp1[c] = (int16_t)iNewSample;
}
}
m_iBufferAvail *= sizeof(int16_t);
return true;
}
void LSM9DS0::readTemp()
{
uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp
xmReadBytes(OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M
temperature = int16_t(temp[0]) + (int16_t(temp[1])<<8) ; // Temperature is a 12-bit signed integer
}
template <> inline int16_t
UnsignedByteToAudioSample<int16_t>(uint8_t aValue)
{
return int16_t(aValue * UINT16_MAX / UINT8_MAX + INT16_MIN);
}
//Add NRPN event with only the MSB of value used
inline void add_NRPN(int chn, int16_t type, char value)
{
add_NRPN(chn, type, int16_t(value<<7));
}
void GxEPD2_213c::writeImagePart(const uint8_t* black, const uint8_t* color, int16_t x_part, int16_t y_part, int16_t w_bitmap, int16_t h_bitmap,
int16_t x, int16_t y, int16_t w, int16_t h, bool invert, bool mirror_y, bool pgm)
{
if (_initial_write) writeScreenBuffer(); // initial full screen buffer clean
delay(1); // yield() to avoid WDT on ESP8266 and ESP32
if ((w_bitmap < 0) || (h_bitmap < 0) || (w < 0) || (h < 0)) return;
if ((x_part < 0) || (x_part >= w_bitmap)) return;
if ((y_part < 0) || (y_part >= h_bitmap)) return;
int16_t wb_bitmap = (w_bitmap + 7) / 8; // width bytes, bitmaps are padded
x_part -= x_part % 8; // byte boundary
w = w_bitmap - x_part < w ? w_bitmap - x_part : w; // limit
h = h_bitmap - y_part < h ? h_bitmap - y_part : h; // limit
x -= x % 8; // byte boundary
w = 8 * ((w + 7) / 8); // byte boundary, bitmaps are padded
int16_t x1 = x < 0 ? 0 : x; // limit
int16_t y1 = y < 0 ? 0 : y; // limit
int16_t w1 = x + w < int16_t(WIDTH) ? w : int16_t(WIDTH) - x; // limit
int16_t h1 = y + h < int16_t(HEIGHT) ? h : int16_t(HEIGHT) - y; // limit
int16_t dx = x1 - x;
int16_t dy = y1 - y;
w1 -= dx;
h1 -= dy;
if ((w1 <= 0) || (h1 <= 0)) return;
if (!_using_partial_mode) _Init_Part();
_writeCommand(0x91); // partial in
_setPartialRamArea(x1, y1, w1, h1);
_writeCommand(0x10);
for (int16_t i = 0; i < h1; i++)
{
for (int16_t j = 0; j < w1 / 8; j++)
{
uint8_t data;
// use wb_bitmap, h_bitmap of bitmap for index!
int16_t idx = mirror_y ? x_part / 8 + j + dx / 8 + ((h_bitmap - 1 - (y_part + i + dy))) * wb_bitmap : x_part / 8 + j + dx / 8 + (y_part + i + dy) * wb_bitmap;
if (pgm)
{
#if defined(__AVR) || defined(ESP8266) || defined(ESP32)
data = pgm_read_byte(&black[idx]);
#else
data = black[idx];
#endif
}
else
{
data = black[idx];
}
if (invert) data = ~data;
_writeData(data);
}
}
_writeCommand(0x13);
for (int16_t i = 0; i < h1; i++)
{
for (int16_t j = 0; j < w1 / 8; j++)
{
uint8_t data = 0xFF;
if (color)
{
// use wb_bitmap, h_bitmap of bitmap for index!
int16_t idx = mirror_y ? x_part / 8 + j + dx / 8 + ((h_bitmap - 1 - (y_part + i + dy))) * wb_bitmap : x_part / 8 + j + dx / 8 + (y_part + i + dy) * wb_bitmap;
if (pgm)
{
#if defined(__AVR) || defined(ESP8266) || defined(ESP32)
data = pgm_read_byte(&color[idx]);
#else
data = color[idx];
#endif
}
else
{
data = color[idx];
}
if (invert) data = ~data;
}
_writeData(data);
}
}
_writeCommand(0x92); // partial out
delay(1); // yield() to avoid WDT on ESP8266 and ESP32
}
/* Write data to be converted. */
void RageSoundResampler::write(const void *data_, int bytes)
{
ASSERT(!at_eof);
const int16_t *data = (const int16_t *) data_;
const unsigned samples = bytes / sizeof(int16_t);
const unsigned frames = samples / Channels;
if(InputRate == OutputRate)
{
/* Optimization. */
outbuf.insert(outbuf.end(), data, data+samples);
return;
}
#if 0
/* Much higher quality for 44100->48000 resampling, but way too slow. */
const int upsamp = 8;
const float div = float(InputRate*upsamp) / OutputRate;
for(unsigned f = 0; f < frames; ++f)
{
for(int u = 0; u < upsamp; ++u)
{
int ipos_begin = (int) roundf(ipos / div);
int ipos_end = (int) roundf((ipos+1) / div);
for(int c = 0; c < Channels; ++c)
{
const float f = 0.5f;
prev[c] = int16_t(prev[c] * (f) + data[c] * (1-f));
for(int i = ipos_begin; i < ipos_end; ++i)
outbuf.push_back(prev[c]);
}
ipos++;
}
data += Channels;
}
#else
/* Lerp. */
const float div = float(InputRate) / OutputRate;
for(unsigned f = 0; f < frames; ++f)
{
for(int c = 0; c < Channels; ++c)
{
while(t[c] < 1.0f)
{
int16_t s = int16_t(prev[c]*(1-t[c]) + data[c]*t[c]);
outbuf.push_back(s);
t[c] += div;
}
t[c] -= 1;
prev[c] = data[c];
}
ipos++;
data += Channels;
}
#endif
}
void GxEPD2_213c::writeImage(const uint8_t* black, const uint8_t* color, int16_t x, int16_t y, int16_t w, int16_t h, bool invert, bool mirror_y, bool pgm)
{
if (_initial_write) writeScreenBuffer(); // initial full screen buffer clean
delay(1); // yield() to avoid WDT on ESP8266 and ESP32
int16_t wb = (w + 7) / 8; // width bytes, bitmaps are padded
x -= x % 8; // byte boundary
w = wb * 8; // byte boundary
int16_t x1 = x < 0 ? 0 : x; // limit
int16_t y1 = y < 0 ? 0 : y; // limit
int16_t w1 = x + w < int16_t(WIDTH) ? w : int16_t(WIDTH) - x; // limit
int16_t h1 = y + h < int16_t(HEIGHT) ? h : int16_t(HEIGHT) - y; // limit
int16_t dx = x1 - x;
int16_t dy = y1 - y;
w1 -= dx;
h1 -= dy;
if ((w1 <= 0) || (h1 <= 0)) return;
_Init_Part();
_writeCommand(0x91); // partial in
_setPartialRamArea(x1, y1, w1, h1);
_writeCommand(0x10);
for (int16_t i = 0; i < h1; i++)
{
for (int16_t j = 0; j < w1 / 8; j++)
{
uint8_t data = 0xFF;
if (black)
{
// use wb, h of bitmap for index!
int16_t idx = mirror_y ? j + dx / 8 + ((h - 1 - (i + dy))) * wb : j + dx / 8 + (i + dy) * wb;
if (pgm)
{
#if defined(__AVR) || defined(ESP8266) || defined(ESP32)
data = pgm_read_byte(&black[idx]);
#else
data = black[idx];
#endif
}
else
{
data = black[idx];
}
if (invert) data = ~data;
}
_writeData(data);
}
}
_writeCommand(0x13);
for (int16_t i = 0; i < h1; i++)
{
for (int16_t j = 0; j < w1 / 8; j++)
{
uint8_t data = 0xFF;
if (color)
{
// use wb, h of bitmap for index!
int16_t idx = mirror_y ? j + dx / 8 + ((h - 1 - (i + dy))) * wb : j + dx / 8 + (i + dy) * wb;
if (pgm)
{
#if defined(__AVR) || defined(ESP8266) || defined(ESP32)
data = pgm_read_byte(&color[idx]);
#else
data = color[idx];
#endif
}
else
{
data = color[idx];
}
if (invert) data = ~data;
}
_writeData(data);
}
}
_writeCommand(0x92); // partial out
delay(1); // yield() to avoid WDT on ESP8266 and ESP32
}
void vertexPack(const float _input[4], bool _inputNormalized, Attrib::Enum _attr, const VertexDecl& _decl, void* _data, uint32_t _index)
{
if (!_decl.has(_attr) )
{
return;
}
uint32_t stride = _decl.getStride();
uint8_t* data = (uint8_t*)_data + _index*stride + _decl.getOffset(_attr);
uint8_t num;
AttribType::Enum type;
bool normalized;
bool asInt;
_decl.decode(_attr, num, type, normalized, asInt);
switch (type)
{
default:
case AttribType::Uint8:
{
uint8_t* packed = (uint8_t*)data;
if (_inputNormalized)
{
if (asInt)
{
switch (num)
{
default: *packed++ = uint8_t(*_input++ * 127.0f + 128.0f); BX_FALLTHROUGH;
case 3: *packed++ = uint8_t(*_input++ * 127.0f + 128.0f); BX_FALLTHROUGH;
case 2: *packed++ = uint8_t(*_input++ * 127.0f + 128.0f); BX_FALLTHROUGH;
case 1: *packed++ = uint8_t(*_input++ * 127.0f + 128.0f);
}
}
else
{
switch (num)
{
default: *packed++ = uint8_t(*_input++ * 255.0f); BX_FALLTHROUGH;
case 3: *packed++ = uint8_t(*_input++ * 255.0f); BX_FALLTHROUGH;
case 2: *packed++ = uint8_t(*_input++ * 255.0f); BX_FALLTHROUGH;
case 1: *packed++ = uint8_t(*_input++ * 255.0f);
}
}
}
else
{
switch (num)
{
default: *packed++ = uint8_t(*_input++); BX_FALLTHROUGH;
case 3: *packed++ = uint8_t(*_input++); BX_FALLTHROUGH;
case 2: *packed++ = uint8_t(*_input++); BX_FALLTHROUGH;
case 1: *packed++ = uint8_t(*_input++);
}
}
}
break;
case AttribType::Uint10:
{
uint32_t packed = 0;
if (_inputNormalized)
{
if (asInt)
{
switch (num)
{
default: BX_FALLTHROUGH;
case 3: packed |= uint32_t(*_input++ * 511.0f + 512.0f); BX_FALLTHROUGH;
case 2: packed <<= 10; packed |= uint32_t(*_input++ * 511.0f + 512.0f); BX_FALLTHROUGH;
case 1: packed <<= 10; packed |= uint32_t(*_input++ * 511.0f + 512.0f);
}
}
else
{
switch (num)
{
default: BX_FALLTHROUGH;
case 3: packed |= uint32_t(*_input++ * 1023.0f); BX_FALLTHROUGH;
case 2: packed <<= 10; packed |= uint32_t(*_input++ * 1023.0f); BX_FALLTHROUGH;
case 1: packed <<= 10; packed |= uint32_t(*_input++ * 1023.0f);
}
}
}
else
{
switch (num)
{
default: BX_FALLTHROUGH;
case 3: packed |= uint32_t(*_input++); BX_FALLTHROUGH;
case 2: packed <<= 10; packed |= uint32_t(*_input++); BX_FALLTHROUGH;
case 1: packed <<= 10; packed |= uint32_t(*_input++);
}
}
*(uint32_t*)data = packed;
}
break;
case AttribType::Int16:
{
int16_t* packed = (int16_t*)data;
if (_inputNormalized)
{
if (asInt)
{
switch (num)
{
default: *packed++ = int16_t(*_input++ * 32767.0f); BX_FALLTHROUGH;
case 3: *packed++ = int16_t(*_input++ * 32767.0f); BX_FALLTHROUGH;
case 2: *packed++ = int16_t(*_input++ * 32767.0f); BX_FALLTHROUGH;
case 1: *packed++ = int16_t(*_input++ * 32767.0f);
}
}
else
{
switch (num)
{
default: *packed++ = int16_t(*_input++ * 65535.0f - 32768.0f); BX_FALLTHROUGH;
case 3: *packed++ = int16_t(*_input++ * 65535.0f - 32768.0f); BX_FALLTHROUGH;
case 2: *packed++ = int16_t(*_input++ * 65535.0f - 32768.0f); BX_FALLTHROUGH;
case 1: *packed++ = int16_t(*_input++ * 65535.0f - 32768.0f);
}
}
}
else
{
switch (num)
{
default: *packed++ = int16_t(*_input++); BX_FALLTHROUGH;
case 3: *packed++ = int16_t(*_input++); BX_FALLTHROUGH;
case 2: *packed++ = int16_t(*_input++); BX_FALLTHROUGH;
case 1: *packed++ = int16_t(*_input++);
}
}
}
break;
case AttribType::Half:
{
uint16_t* packed = (uint16_t*)data;
switch (num)
{
default: *packed++ = bx::halfFromFloat(*_input++); BX_FALLTHROUGH;
case 3: *packed++ = bx::halfFromFloat(*_input++); BX_FALLTHROUGH;
case 2: *packed++ = bx::halfFromFloat(*_input++); BX_FALLTHROUGH;
case 1: *packed++ = bx::halfFromFloat(*_input++);
}
}
break;
case AttribType::Float:
bx::memCopy(data, _input, num*sizeof(float) );
break;
}
}
void validateKeyPoints(const std::vector<cv::KeyPoint> & in_keypoints, const cv::Mat &in_mask, const cv::Mat & depth,
const cv::Mat &in_K, const cv::Mat & descriptors, std::vector<cv::KeyPoint> & final_keypoints,
cv::Mat &final_points, cv::Mat & final_descriptors) {
cv::Mat K;
in_K.convertTo(K, CV_32FC1);
size_t n_points = descriptors.rows;
cv::Mat clean_descriptors = cv::Mat(descriptors.size(), descriptors.type());
cv::Mat_<cv::Vec2f> clean_points(1, n_points);
final_keypoints.clear();
final_keypoints.reserve(n_points);
cv::Mat_<uchar> mask;
in_mask.convertTo(mask, CV_8U);
// Erode just because of the possible rescaling
cv::erode(mask, mask, cv::Mat(), cv::Point(-1, -1), 4);
int width = mask.cols, height = mask.rows;
size_t clean_row_index = 0;
for (size_t keypoint_index = 0; keypoint_index < n_points; ++keypoint_index) {
// First, make sure that the keypoint belongs to the mask
const cv::KeyPoint & in_keypoint = in_keypoints[keypoint_index];
unsigned int x = roundWithinBounds(in_keypoint.pt.x, 0, width), y = roundWithinBounds(in_keypoint.pt.y, 0, height);
float z;
bool is_good = false;
if (mask(y, x))
is_good = true;
else {
// If the keypoint does not belong to the mask, look in a slightly bigger neighborhood
int window_size = 2;
float min_dist_sq = std::numeric_limits<float>::max();
// Look into neighborhoods of different sizes to see if we have a point in the mask
for (unsigned int i = roundWithinBounds(x - window_size, 0, width);
i <= roundWithinBounds(x + window_size, 0, width); ++i)
for (unsigned int j = roundWithinBounds(y - window_size, 0, height);
j <= roundWithinBounds(y + window_size, 0, height); ++j)
if (mask(j, i)) {
float dist_sq = (i - in_keypoint.pt.x) * (i - in_keypoint.pt.x)
+ (j - in_keypoint.pt.y) * (j - in_keypoint.pt.y);
if (dist_sq < min_dist_sq) {
// If the point is in the mask and the closest from the keypoint
x = i;
y = j;
min_dist_sq = dist_sq;
is_good = true;
}
}
}
if (!is_good)
continue;
// Now, check that the depth of the keypoint is valid
switch (depth.depth()) {
case CV_16U:
z = depth.at<uint16_t>(y, x);
if (!cv::isValidDepth(uint16_t(z)))
is_good = false;
z /= 1000;
break;
case CV_16S:
z = depth.at<int16_t>(y, x);
if (!cv::isValidDepth(int16_t(z)))
is_good = false;
z /= 1000;
break;
case CV_32F:
z = depth.at<float>(y, x);
if (!cv::isValidDepth(float(z)))
is_good = false;
break;
default:
continue;
break;
}
if (!is_good)
continue;
// Store the keypoint and descriptor
clean_points.at<cv::Vec2f>(0, clean_row_index) = cv::Vec2f(x, y);
cv::Mat clean_descriptor_row = clean_descriptors.row(clean_row_index++);
descriptors.row(keypoint_index).copyTo(clean_descriptor_row);
final_keypoints.push_back(in_keypoint);
}
if (clean_row_index > 0) {
clean_points.colRange(0, clean_row_index).copyTo(final_points);
clean_descriptors.rowRange(0, clean_row_index).copyTo(final_descriptors);
}
}
