void NetAudioBuffer::RenderToJackPorts ( int subcycle )
{
for ( int port_index = 0; port_index < fNPorts; port_index++ ) {
jack_default_audio_sample_t* src = (jack_default_audio_sample_t*)(fNetBuffer + port_index * fSubPeriodBytesSize);
jack_default_audio_sample_t* dst = (jack_default_audio_sample_t*)(fPortBuffer[port_index] + subcycle * fSubPeriodSize);
for (unsigned int sample = 0; sample < fSubPeriodBytesSize / sizeof(jack_default_audio_sample_t); sample++) {
dst[sample] = SwapFloat(src[sample]);
}
}
}
// will need to read from text file instead
YEARDATAHDL GetYearData(short year)
{
// IMPORTANT: The calling function should NOT dispose the handle it gets
YEARDATAHDL yrHdl=nil;
short yearMinus1990 = year-1990;
long i,n,resSize=0;
if(0<= yearMinus1990 && yearMinus1990 <kMAXNUMSAVEDYEARS)
{
if(gYearDataHdl1990Plus[yearMinus1990]) return gYearDataHdl1990Plus[yearMinus1990];
}
#ifdef MAC
Handle r = nil;
r=GetResource('YEAR',(long)year);
#ifdef SWAP_BINARY
resSize = GetMaxResourceSize(r);
if(resSize > 0 && r)
{
yrHdl = (YEARDATAHDL)_NewHandle(resSize);
if(yrHdl)
{
_HLock(r); // so it can't be purged !!!
YEARDATAHDL rHdl = (YEARDATAHDL)_NewHandle(resSize);
DetachResource(r);
rHdl = (YEARDATAHDL) r;
// copy and swap the bytes
n = resSize/sizeof(YEARDATA);
for(i = 0; i< n; i++)
{
YEARDATA yrd = (YEARDATA)INDEXH(rHdl,i);
SwapFloat(&yrd.XODE);
SwapFloat(&yrd.VPU);
INDEXH(yrHdl,i) = yrd;
}
// I don't think we free something gotten from a resource
}
ReleaseResource(r);// don't dispose of a resource handle !!!
r = 0;
}
#else
if(r)
{
DetachResource(r);
yrHdl = (YEARDATAHDL) r;
}
#endif
#else
char numStr[32];
HRSRC hResInfo =0;
HGLOBAL r = 0;
sprintf(numStr,"#%ld",year);
hResInfo = FindResource(hInst,numStr,"YEAR");
if(hResInfo)
{
// copy the handle so we can be
// just like the mac
//
//also we need to swap the bytes
//
// be careful r is a HGLOBAL, not one of our special fake handles
resSize = SizeofResource(hInst,hResInfo);
if(resSize > 0) r = LoadResource(hInst,hResInfo);
if(resSize > 0 && r)
{
yrHdl = (YEARDATAHDL)_NewHandle(resSize);
if(yrHdl)
{
YEARDATAPTR rPtr = (YEARDATAPTR) LockResource(r);
// copy and swap the bytes
n = resSize/sizeof(YEARDATA);
for(i = 0; i< n; i++)
{
YEARDATA yrd = rPtr[i];
SwapFloat(&yrd.XODE);
SwapFloat(&yrd.VPU);
INDEXH(yrHdl,i) = yrd;
}
// WIN32 applications do not have to unlock resources locked by LockResource
// I don't think we free something gotten from a resource
}
}
}
#endif
if(yrHdl && 0<= yearMinus1990 && yearMinus1990 <kMAXNUMSAVEDYEARS)
{
gYearDataHdl1990Plus[yearMinus1990] = yrHdl;
}
return(yrHdl);
}
bool
AP_GPS_GSOF::process_message(void)
{
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html
if (gsof_msg.packettype == 0x40) { // GSOF
#if gsof_DEBUGGING
uint8_t trans_number = gsof_msg.data[0];
uint8_t pageidx = gsof_msg.data[1];
uint8_t maxpageidx = gsof_msg.data[2];
Debug("GSOF page: %u of %u (trans_number=%u)",
pageidx, maxpageidx, trans_number);
#endif
int valid = 0;
// want 1 2 8 9 12
for (uint32_t a = 3; a < gsof_msg.length; a++)
{
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
//Debug("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 1) // pos time
{
state.time_week_ms = SwapUint32(gsof_msg.data, a);
state.time_week = SwapUint16(gsof_msg.data, a + 4);
state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
//Debug("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1)) { // New position
state.status = AP_GPS::GPS_OK_FIX_3D;
if ((posf2 & 1)) { // Differential position
state.status = AP_GPS::GPS_OK_FIX_3D_DGPS;
if (posf2 & 2) { // Differential position method
if (posf2 & 4) {// Differential position method
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED;
} else {
state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FLOAT;
}
}
}
} else {
state.status = AP_GPS::NO_FIX;
}
valid++;
}
else if (output_type == 2) // position
{
state.location.lat = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * (double)1e7);
state.location.lng = (int32_t)(RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * (double)1e7);
state.location.alt = (int32_t)(SwapDouble(gsof_msg.data, a + 16) * 100);
state.last_gps_time_ms = AP_HAL::millis();
valid++;
}
else if (output_type == 8) // velocity
{
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1)
{
state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
state.ground_course = degrees(SwapFloat(gsof_msg.data, a + 5));
fill_3d_velocity();
state.velocity.z = -SwapFloat(gsof_msg.data, a + 9);
state.have_vertical_velocity = true;
}
valid++;
}
else if (output_type == 9) //dop
{
state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
valid++;
}
else if (output_type == 12) // position sigma
{
state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
state.have_horizontal_accuracy = true;
state.have_vertical_accuracy = true;
valid++;
}
a += output_length-1u;
}
if (valid == 5) {
return true;
} else {
state.status = AP_GPS::NO_FIX;
}
}
return false;
}
bool apm::AP_GPS_GSOF::process_message(void)
{
//http://www.trimble.com/OEM_ReceiverHelp/V4.81/en/default.html#welcome.html
if (gsof_msg.packettype == 0x40) { // GSOF
int valid = 0;
// want 1 2 8 9 12
for (uint32_t a = 3; a < gsof_msg.length; a++) {
uint8_t output_type = gsof_msg.data[a];
a++;
uint8_t output_length = gsof_msg.data[a];
a++;
//Debug("GSOF type: " + output_type + " len: " + output_length);
if (output_type == 1) { // pos time
gps.state.time_week_ms = SwapUint32(gsof_msg.data, a);
gps.state.time_week = SwapUint16(gsof_msg.data, a + 4);
gps.state.num_sats = gsof_msg.data[a + 6];
uint8_t posf1 = gsof_msg.data[a + 7];
uint8_t posf2 = gsof_msg.data[a + 8];
//Debug("POSTIME: " + posf1 + " " + posf2);
if ((posf1 & 1) == 1) {
gps.state.status = gps_t::FIX_3D;
if ((posf2 & 1) == 1) {
gps.state.status = gps_t::FIX_3D_DGPS;
if ((posf2 & 4) == 4) {
gps.state.status = gps_t::FIX_3D_RTK;
}
}
} else {
gps.state.status = gps_t::NO_FIX;
}
valid++;
} else if (output_type == 2) { // position
gps.state.location.lat = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a)) * 1e7}; // deg1e7
gps.state.location.lon = gps_t::lat_lon_type {RAD_TO_DEG_DOUBLE * (SwapDouble(gsof_msg.data, a + 8)) * 1e7}; // deg1e7
gps.state.location.alt = gps_t::altitude_type {SwapDouble(gsof_msg.data, a + 16) * 1e2}; //cm
gps.state.last_gps_time_ms = gps.state.time_week_ms;
valid++;
} else if (output_type == 8) { // velocity
uint8_t vflag = gsof_msg.data[a];
if ((vflag & 1) == 1) {
gps.state.ground_speed = SwapFloat(gsof_msg.data, a + 1);
gps.state.ground_course_cd = (int32_t)(ToDeg(SwapFloat(gsof_msg.data, a + 5)) * 100);
fill_3d_velocity();
gps.state.velocity.z = gps_t::velocity_type {-SwapFloat(gsof_msg.data, a + 9)};
gps.state.have_vertical_velocity = true;
}
valid++;
} else if (output_type == 9) { //dop
gps.state.hdop = (uint16_t)(SwapFloat(gsof_msg.data, a + 4) * 100);
valid++;
} else if (output_type == 12) { // position sigma
gps.state.horizontal_accuracy = (SwapFloat(gsof_msg.data, a + 4) + SwapFloat(gsof_msg.data, a + 8)) / 2;
gps.state.vertical_accuracy = SwapFloat(gsof_msg.data, a + 16);
gps.state.have_horizontal_accuracy = true;
gps.state.have_vertical_accuracy = true;
valid++;
}
a += output_length-1u;
}
if (valid == 5) {
return true;
} else {
gps.state.status = gps_t::NO_FIX;
}
}
return false;
}
