/* output solution status for PPP --------------------------------------------*/
extern void pppoutsolstat(rtk_t *rtk, int level, FILE *fp)
{
ssat_t *ssat;
double tow,pos[3],vel[3],acc[3];
int i,j,week,nfreq=1;
char id[32];
if (level<=0||!fp) return;
trace(3,"pppoutsolstat:\n");
tow=time2gpst(rtk->sol.time,&week);
/* receiver position */
fprintf(fp,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow,
rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],0.0,0.0,0.0);
/* receiver velocity and acceleration */
if (rtk->opt.dynamics) {
ecef2pos(rtk->sol.rr,pos);
ecef2enu(pos,rtk->x+3,vel);
ecef2enu(pos,rtk->x+6,acc);
fprintf(fp,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2],
0.0,0.0,0.0,0.0,0.0,0.0);
}
/* receiver clocks */
i=IC(0,&rtk->opt);
fprintf(fp,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week,tow,rtk->sol.stat,1,rtk->x[i]*1E9/CLIGHT,rtk->x[i+1]*1E9/CLIGHT,
0.0,0.0);
/* tropospheric parameters */
if (rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG) {
i=IT(&rtk->opt);
fprintf(fp,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
1,rtk->x[i],0.0);
}
if (rtk->opt.tropopt==TROPOPT_ESTG) {
i=IT(&rtk->opt);
fprintf(fp,"$TRPG,%d,%.3f,%d,%d,%.5f,%.5f,%.5f,%.5f\n",week,tow,
rtk->sol.stat,1,rtk->x[i+1],rtk->x[i+2],0.0,0.0);
}
if (rtk->sol.stat==SOLQ_NONE||level<=1) return;
/* residuals and status */
for (i=0;i<MAXSAT;i++) {
ssat=rtk->ssat+i;
if (!ssat->vs) continue;
satno2id(i+1,id);
for (j=0;j<nfreq;j++) {
fprintf(fp,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D,
ssat->resp[j],ssat->resc[j],ssat->vsat[j],ssat->snr[j]*0.25,
ssat->fix[j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j],
ssat->slipc[j],ssat->rejc[j]);
}
}
}
int test_connect_properly_formatted() {
IT("sends a properly formatted connect packet and succeeds");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte expectServer[] = { 172, 16, 0, 2 };
shimClient.expectConnect(expectServer,1883);
byte connect[] = {0x10,0x18,0x0,0x4,0x4d,0x51,0x54,0x54,0x4,0x2,0x0,0xf,0x0,0xc,0x63,0x6c,0x69,0x65,0x6e,0x74,0x5f,0x74,0x65,0x73,0x74,0x31};
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.expect(connect,26);
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int state = client.state();
IS_TRUE(state == MQTT_DISCONNECTED);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
state = client.state();
IS_TRUE(state == MQTT_CONNECTED);
END_IT
}
int test_receive_max_sized_message() {
IT("receives an max-sized message");
reset_callback();
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte length = MQTT_MAX_PACKET_SIZE;
byte publish[] = {0x30,length-2,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x70,0x61,0x79,0x6c,0x6f,0x61,0x64};
byte bigPublish[length];
memset(bigPublish,'A',length);
bigPublish[length] = 'B';
memcpy(bigPublish,publish,16);
shimClient.respond(bigPublish,length);
rc = client.loop();
IS_TRUE(rc);
IS_TRUE(callback_called);
IS_TRUE(strcmp(lastTopic,"topic")==0);
IS_TRUE(lastLength == length-9);
IS_TRUE(memcmp(lastPayload,bigPublish+9,lastLength)==0);
IS_FALSE(shimClient.error());
END_IT
}
int test_keepalive_pings_idle() {
IT("keeps an idle connection alive (takes 1 minute)");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte pingreq[] = { 0xC0,0x0 };
shimClient.expect(pingreq,2);
byte pingresp[] = { 0xD0,0x0 };
shimClient.respond(pingresp,2);
for (int i = 0; i < 50; i++) {
sleep(1);
if ( i == 15 || i == 31 || i == 47) {
shimClient.expect(pingreq,2);
shimClient.respond(pingresp,2);
}
rc = client.loop();
IS_TRUE(rc);
}
IS_FALSE(shimClient.error());
END_IT
}
int test_receive_qos1() {
IT("receives a qos1 message");
reset_callback();
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte publish[] = {0x32,0x10,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x12,0x34,0x70,0x61,0x79,0x6c,0x6f,0x61,0x64};
shimClient.respond(publish,18);
byte puback[] = {0x40,0x2,0x12,0x34};
shimClient.expect(puback,4);
rc = client.loop();
IS_TRUE(rc);
IS_TRUE(callback_called);
IS_TRUE(strcmp(lastTopic,"topic")==0);
IS_TRUE(memcmp(lastPayload,"payload",7)==0);
IS_TRUE(lastLength == 7);
IS_FALSE(shimClient.error());
END_IT
}
int test_keepalive_no_pings_inbound_qos1() {
IT("does not send pings for connections with inbound qos1 (takes 1 minute)");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte publish[] = {0x32,0x10,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x12,0x34,0x70,0x61,0x79,0x6c,0x6f,0x61,0x64};
byte puback[] = {0x40,0x2,0x12,0x34};
for (int i = 0; i < 50; i++) {
shimClient.respond(publish,18);
shimClient.expect(puback,4);
sleep(1);
rc = client.loop();
IS_TRUE(rc);
IS_FALSE(shimClient.error());
}
END_IT
}
/* temporal update of tropospheric parameters --------------------------------*/
static void udtrop_ppp(rtk_t *rtk)
{
double pos[3],azel[]={0.0,PI/2.0},ztd,var;
int i=IT(&rtk->opt),j;
trace(3,"udtrop_ppp:\n");
if (rtk->x[i]==0.0) {
ecef2pos(rtk->sol.rr,pos);
ztd=sbstropcorr(rtk->sol.time,pos,azel,&var);
initx(rtk,ztd,var,i);
if (rtk->opt.tropopt>=TROPOPT_ESTG) {
for (j=0;j<2;j++) initx(rtk,1E-6,VAR_GRA,++i);
}
}
else {
rtk->P[i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2])*fabs(rtk->tt);
if (rtk->opt.tropopt>=TROPOPT_ESTG) {
for (j=0;j<2;j++) {
rtk->P[++i*(1+rtk->nx)]+=SQR(rtk->opt.prn[2]*0.1)*fabs(rtk->tt);
}
}
}
}
int main( int argc, /* number of arguments */ char *argv[] /* arguments */){
int j,k;
unsigned int p;
myuint *X=rng_alloc();
//myuint X[N+1];
fprintf(stderr,"Welcome to the Yerevan random number generator!\nThe curent matrix size is %u\n"
"(the actual matrix is not kept in memory in this new efficient implementation)",N);
fprintf(stderr,"\nWe will now generate a power of our matrix A^m\n ");
fprintf(stderr,"\nHow many iterations m do you want?\n(2^10=1024, 2^20=1048576, 2^30=1073741824)\nEnter m: "); scanf("%u", &p);
printf("\nHere is convenient Mathematica Input:\n AT={");
for (k=1; (k<=N); k++) {
seedvector(X,k);
for (j=0; (j<p); j++) {
IT(X);
}
printf("\n{");
printf("%llu", X[1] );
for (j=2; (j<=N); j++) {
printf(",\t%llu", X[j] );
}
printf("},");
}
printf("\b} \n A = Transpose[AT] \n d = Det[A] \n AM = Mod[A, 2^61-1] \n cp = CharacteristicPolynomial[AM, x] \n "
"N[Solve[cp == 0, x]] \n N[Eigenvalues[AM]] \n Abs[%%]\n"
"M1K = Mod[MatrixPower[A, 2^10], 2^61 - 1]\n ");
//\n Maxima input: eq:charpoly(N,x);\n p:radcan(eq); r:allroots(p);
printf("\n\nok\n");
rng_free(X);
return 1;
}
int test_keepalive_disconnects_hung() {
IT("disconnects a hung connection (takes 30 seconds)");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte pingreq[] = { 0xC0,0x0 };
shimClient.expect(pingreq,2);
for (int i = 0; i < 32; i++) {
sleep(1);
rc = client.loop();
}
IS_FALSE(rc);
int state = client.state();
IS_TRUE(state == MQTT_CONNECTION_TIMEOUT);
IS_FALSE(shimClient.error());
END_IT
}
int test_publish_retained() {
IT("publishes retained");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte payload[] = { 0x01,0x02,0x03,0x0,0x05 };
int length = 5;
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte publish[] = {0x31,0xc,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x1,0x2,0x3,0x0,0x5};
shimClient.expect(publish,14);
rc = client.publish((char*)"topic",payload,length,true);
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_receive_stream() {
IT("receives a streamed callback message");
reset_callback();
Stream stream;
stream.expect((uint8_t*)"payload",7);
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient, stream);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte publish[] = {0x30,0xe,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x70,0x61,0x79,0x6c,0x6f,0x61,0x64};
shimClient.respond(publish,16);
rc = client.loop();
IS_TRUE(rc);
IS_TRUE(callback_called);
IS_TRUE(strcmp(lastTopic,"topic")==0);
IS_TRUE(lastLength == 7);
IS_FALSE(stream.error());
IS_FALSE(shimClient.error());
END_IT
}
int test_keepalive_pings_with_inbound_qos0() {
IT("keeps a connection alive that only receives qos0 (takes 1 minute)");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte publish[] = {0x30,0xe,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x70,0x61,0x79,0x6c,0x6f,0x61,0x64};
for (int i = 0; i < 50; i++) {
TRACE(i<<":");
sleep(1);
if ( i == 15 || i == 31 || i == 47) {
byte pingreq[] = { 0xC0,0x0 };
shimClient.expect(pingreq,2);
byte pingresp[] = { 0xD0,0x0 };
shimClient.respond(pingresp,2);
}
shimClient.respond(publish,16);
rc = client.loop();
IS_TRUE(rc);
IS_FALSE(shimClient.error());
}
END_IT
}
int test_connect_fails_on_no_response() {
IT("fails to connect if no response received after 15 seconds");
ShimClient shimClient;
shimClient.setAllowConnect(true);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_FALSE(rc);
END_IT
}
int test_connect_fails_no_network() {
IT("fails to connect if underlying client doesn't connect");
ShimClient shimClient;
shimClient.setAllowConnect(false);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_FALSE(rc);
END_IT
}
int test_connect_fails_on_bad_rc() {
IT("fails to connect if a bad return code is received");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x01 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_FALSE(rc);
END_IT
}
int test_publish_not_connected() {
IT("publish fails when not connected");
ShimClient shimClient;
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.publish((char*)"topic",(char*)"payload");
IS_FALSE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_subscribe_not_connected() {
IT("subscribe fails when not connected");
ShimClient shimClient;
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.subscribe((char*)"topic");
IS_FALSE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_connect_properly_formatted_hostname() {
IT("accepts a hostname");
ShimClient shimClient;
shimClient.setAllowConnect(true);
shimClient.expectConnect((char* const)"localhost",1883);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client((char* const)"localhost", 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_connect_disconnect_connect() {
IT("connects, disconnects and connects again");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte expectServer[] = { 172, 16, 0, 2 };
shimClient.expectConnect(expectServer,1883);
byte connect[] = {0x10,0x18,0x0,0x4,0x4d,0x51,0x54,0x54,0x4,0x2,0x0,0xf,0x0,0xc,0x63,0x6c,0x69,0x65,0x6e,0x74,0x5f,0x74,0x65,0x73,0x74,0x31};
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.expect(connect,26);
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int state = client.state();
IS_TRUE(state == MQTT_DISCONNECTED);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
state = client.state();
IS_TRUE(state == MQTT_CONNECTED);
byte disconnect[] = {0xE0,0x00};
shimClient.expect(disconnect,2);
client.disconnect();
IS_FALSE(client.connected());
IS_FALSE(shimClient.connected());
IS_FALSE(shimClient.error());
state = client.state();
IS_TRUE(state == MQTT_DISCONNECTED);
shimClient.expect(connect,28);
shimClient.respond(connack,4);
rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
state = client.state();
IS_TRUE(state == MQTT_CONNECTED);
END_IT
}
int test_connect_accepts_username_password() {
IT("accepts a username and password");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connect[] = { 0x10,0x26,0x0,0x6,0x4d,0x51,0x49,0x73,0x64,0x70,0x3,0xc2,0x0,0xf,0x0,0xc,0x63,0x6c,0x69,0x65,0x6e,0x74,0x5f,0x74,0x65,0x73,0x74,0x31,0x0,0x4,0x75,0x73,0x65,0x72,0x0,0x4,0x70,0x61,0x73,0x73};
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.expect(connect,0x28);
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1",(char*)"user",(char*)"pass");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_connect_ignores_password_no_username() {
IT("ignores a password but no username");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connect[] = {0x10,0x1a,0x0,0x6,0x4d,0x51,0x49,0x73,0x64,0x70,0x3,0x2,0x0,0xf,0x0,0xc,0x63,0x6c,0x69,0x65,0x6e,0x74,0x5f,0x74,0x65,0x73,0x74,0x31};
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.expect(connect,28);
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1",'\0',(char*)"pass");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_connect_with_will_username_password() {
IT("accepts a will, username and password");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connect[] = {0x10,0x42,0x0,0x6,0x4d,0x51,0x49,0x73,0x64,0x70,0x3,0xce,0x0,0xf,0x0,0xc,0x63,0x6c,0x69,0x65,0x6e,0x74,0x5f,0x74,0x65,0x73,0x74,0x31,0x0,0x9,0x77,0x69,0x6c,0x6c,0x54,0x6f,0x70,0x69,0x63,0x0,0xb,0x77,0x69,0x6c,0x6c,0x4d,0x65,0x73,0x73,0x61,0x67,0x65,0x0,0x4,0x75,0x73,0x65,0x72,0x0,0x8,0x70,0x61,0x73,0x73,0x77,0x6f,0x72,0x64};
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.expect(connect,0x44);
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1",(char*)"user",(char*)"password",(char*)"willTopic",1,0,(char*)"willMessage");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
nodeCameras::nodeCameras(ros::NodeHandle *NodeROS)
{
m_pNode = NodeROS;
image_transport::ImageTransport myImageTransporter(*NodeROS);
image_transport::ImageTransport IT(*m_pNode);
m_ImagePublisherUpward = myImageTransporter.advertise("imageUpward_bus", 100);
m_ImagePublisherForward = myImageTransporter.advertise("imageForward_bus", 100);
m_CameraServer = m_pNode->advertiseService("cameras_service", &nodeCameras::CamerasServerCallback, this);
m_AllDone = false;
m_pCameras = new Cameras;
if (m_pCameras->InitializeCameras()) std::cout << "Cameras initialized" << std::endl;
else std::cout << "Cameras not initialized" << std::endl;
m_image = NULL;
m_RotatedImage = NULL;
}
llvm::Expected<std::vector<CodeTemplate>>
generateSelfAliasingCodeTemplates(const Instruction &Instr) {
const AliasingConfigurations SelfAliasing(Instr, Instr);
if (SelfAliasing.empty())
return llvm::make_error<SnippetGeneratorFailure>("empty self aliasing");
std::vector<CodeTemplate> Result;
Result.emplace_back();
CodeTemplate &CT = Result.back();
InstructionTemplate IT(Instr);
if (SelfAliasing.hasImplicitAliasing()) {
CT.Info = "implicit Self cycles, picking random values.";
} else {
CT.Info = "explicit self cycles, selecting one aliasing Conf.";
// This is a self aliasing instruction so defs and uses are from the same
// instance, hence twice IT in the following call.
setRandomAliasing(SelfAliasing, IT, IT);
}
CT.Instructions.push_back(std::move(IT));
return std::move(Result);
}
int test_subscribe_invalid_qos() {
IT("subscribe fails with invalid qos values");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
rc = client.subscribe((char*)"topic",2);
IS_FALSE(rc);
rc = client.subscribe((char*)"topic",254);
IS_FALSE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_publish() {
IT("publishes a null-terminated string");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte publish[] = {0x30,0xe,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x70,0x61,0x79,0x6c,0x6f,0x61,0x64};
shimClient.expect(publish,16);
rc = client.publish((char*)"topic",(char*)"payload");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_unsubscribe() {
IT("unsubscribes");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte unsubscribe[] = { 0xA2,0x9,0x0,0x2,0x0,0x5,0x74,0x6f,0x70,0x69,0x63 };
shimClient.expect(unsubscribe,12);
byte unsuback[] = { 0xB0,0x2,0x0,0x2 };
shimClient.respond(unsuback,4);
rc = client.unsubscribe((char*)"topic");
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
int test_subscribe_qos_1() {
IT("subscribes qos 1");
ShimClient shimClient;
shimClient.setAllowConnect(true);
byte connack[] = { 0x20, 0x02, 0x00, 0x00 };
shimClient.respond(connack,4);
PubSubClient client(server, 1883, callback, shimClient);
int rc = client.connect((char*)"client_test1");
IS_TRUE(rc);
byte subscribe[] = { 0x82,0xa,0x0,0x2,0x0,0x5,0x74,0x6f,0x70,0x69,0x63,0x1 };
shimClient.expect(subscribe,12);
byte suback[] = { 0x90,0x3,0x0,0x2,0x1 };
shimClient.respond(suback,5);
rc = client.subscribe((char*)"topic",1);
IS_TRUE(rc);
IS_FALSE(shimClient.error());
END_IT
}
static IT step() {
return IT(ST);
}
/* phase and code residuals --------------------------------------------------*/
static int res_ppp(int iter, const obsd_t *obs, int n, const double *rs,
const double *dts, const double *vare, const int *svh,
const nav_t *nav, const double *x, rtk_t *rtk, double *v,
double *H, double *R, double *azel)
{
prcopt_t *opt=&rtk->opt;
double r,rr[3],disp[3],pos[3],e[3],meas[2],dtdx[3],dantr[NFREQ]={0};
double dants[NFREQ]={0},var[MAXOBS*2],dtrp=0.0,vart=0.0,varm[2]={0};
int i,j,k,sat,sys,nv=0,nx=rtk->nx,brk,tideopt;
trace(3,"res_ppp : n=%d nx=%d\n",n,nx);
for (i=0;i<MAXSAT;i++) rtk->ssat[i].vsat[0]=0;
for (i=0;i<3;i++) rr[i]=x[i];
/* earth tides correction */
if (opt->tidecorr) {
tideopt=opt->tidecorr==1?1:7; /* 1:solid, 2:solid+otl+pole */
tidedisp(gpst2utc(obs[0].time),rr,tideopt,&nav->erp,opt->odisp[0],
disp);
for (i=0;i<3;i++) rr[i]+=disp[i];
}
ecef2pos(rr,pos);
for (i=0;i<n&&i<MAXOBS;i++) {
sat=obs[i].sat;
if (!(sys=satsys(sat,NULL))||!rtk->ssat[sat-1].vs) continue;
/* geometric distance/azimuth/elevation angle */
if ((r=geodist(rs+i*6,rr,e))<=0.0||
satazel(pos,e,azel+i*2)<opt->elmin) continue;
/* excluded satellite? */
if (satexclude(obs[i].sat,svh[i],opt)) continue;
/* tropospheric delay correction */
if (opt->tropopt==TROPOPT_SAAS) {
dtrp=tropmodel(obs[i].time,pos,azel+i*2,REL_HUMI);
vart=SQR(ERR_SAAS);
}
else if (opt->tropopt==TROPOPT_SBAS) {
dtrp=sbstropcorr(obs[i].time,pos,azel+i*2,&vart);
}
else if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {
dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x+IT(opt),dtdx,&vart);
}
else if (opt->tropopt==TROPOPT_COR||opt->tropopt==TROPOPT_CORG) {
dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x,dtdx,&vart);
}
/* satellite antenna model */
if (opt->posopt[0]) {
satantpcv(rs+i*6,rr,nav->pcvs+sat-1,dants);
}
/* receiver antenna model */
antmodel(opt->pcvr,opt->antdel[0],azel+i*2,opt->posopt[1],dantr);
/* phase windup correction */
if (opt->posopt[2]) {
windupcorr(rtk->sol.time,rs+i*6,rr,&rtk->ssat[sat-1].phw);
}
/* ionosphere and antenna phase corrected measurements */
if (!corrmeas(obs+i,nav,pos,azel+i*2,&rtk->opt,dantr,dants,
rtk->ssat[sat-1].phw,meas,varm,&brk)) {
continue;
}
/* satellite clock and tropospheric delay */
r+=-CLIGHT*dts[i*2]+dtrp;
trace(5,"sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n",
sat,azel[i*2]*R2D,azel[1+i*2]*R2D,dtrp,dantr[0],dantr[1],dants[0],
dants[1],rtk->ssat[sat-1].phw);
for (j=0;j<2;j++) { /* for phase and code */
if (meas[j]==0.0) continue;
for (k=0;k<nx;k++) H[k+nx*nv]=0.0;
v[nv]=meas[j]-r;
for (k=0;k<3;k++) H[k+nx*nv]=-e[k];
if (sys!=SYS_GLO) {
v[nv]-=x[IC(0,opt)];
H[IC(0,opt)+nx*nv]=1.0;
}
else {
v[nv]-=x[IC(1,opt)];
H[IC(1,opt)+nx*nv]=1.0;
}
if (opt->tropopt>=TROPOPT_EST) {
for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) {
H[IT(opt)+k+nx*nv]=dtdx[k];
}
}
if (j==0) {
v[nv]-=x[IB(obs[i].sat,opt)];
H[IB(obs[i].sat,opt)+nx*nv]=1.0;
}
var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j,opt)+varm[j]+vare[i]+vart;
if (j==0) rtk->ssat[sat-1].resc[0]=v[nv];
else rtk->ssat[sat-1].resp[0]=v[nv];
/* test innovation */
#if 0
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
#else
if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno&&sys!=SYS_GLO) {
#endif
trace(2,"ppp outlier rejected %s sat=%2d type=%d v=%.3f\n",
time_str(obs[i].time,0),sat,j,v[nv]);
rtk->ssat[sat-1].rejc[0]++;
continue;
}
if (j==0) rtk->ssat[sat-1].vsat[0]=1;
nv++;
}
}
for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
R[i+j*nv]=i==j?var[i]:0.0;
}
trace(5,"x=\n"); tracemat(5,x, 1,nx,8,3);
trace(5,"v=\n"); tracemat(5,v, 1,nv,8,3);
trace(5,"H=\n"); tracemat(5,H,nx,nv,8,3);
trace(5,"R=\n"); tracemat(5,R,nv,nv,8,5);
return nv;
}
/* number of estimated states ------------------------------------------------*/
extern int pppnx(const prcopt_t *opt)
{
return NX(opt);
}
/* precise point positioning -------------------------------------------------*/
extern void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
const prcopt_t *opt=&rtk->opt;
double *rs,*dts,*var,*v,*H,*R,*azel,*xp,*Pp;
int i,nv,info,svh[MAXOBS],stat=SOLQ_SINGLE;
trace(3,"pppos : nx=%d n=%d\n",rtk->nx,n);
rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel=zeros(2,n);
for (i=0;i<MAXSAT;i++) rtk->ssat[i].fix[0]=0;
/* temporal update of states */
udstate_ppp(rtk,obs,n,nav);
trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4);
/* satellite positions and clocks */
satposs(obs[0].time,obs,n,nav,rtk->opt.sateph,rs,dts,var,svh);
/* exclude measurements of eclipsing satellite */
if (rtk->opt.posopt[3]) {
testeclipse(obs,n,nav,rs);
}
xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx);
matcpy(xp,rtk->x,rtk->nx,1);
nv=n*rtk->opt.nf*2; v=mat(nv,1); H=mat(rtk->nx,nv); R=mat(nv,nv);
for (i=0;i<rtk->opt.niter;i++) {
/* phase and code residuals */
if ((nv=res_ppp(i,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel))<=0) break;
/* measurement update */
matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {
trace(2,"ppp filter error %s info=%d\n",time_str(rtk->sol.time,0),
info);
break;
}
trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4);
stat=SOLQ_PPP;
}
if (stat==SOLQ_PPP) {
/* postfit residuals */
res_ppp(1,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel);
/* update state and covariance matrix */
matcpy(rtk->x,xp,rtk->nx,1);
matcpy(rtk->P,Pp,rtk->nx,rtk->nx);
/* ambiguity resolution in ppp */
if (opt->modear==ARMODE_PPPAR||opt->modear==ARMODE_PPPAR_ILS) {
if (pppamb(rtk,obs,n,nav,azel)) stat=SOLQ_FIX;
}
/* update solution status */
rtk->sol.ns=0;
for (i=0;i<n&&i<MAXOBS;i++) {
if (!rtk->ssat[obs[i].sat-1].vsat[0]) continue;
rtk->ssat[obs[i].sat-1].lock[0]++;
rtk->ssat[obs[i].sat-1].outc[0]=0;
rtk->ssat[obs[i].sat-1].fix [0]=4;
rtk->sol.ns++;
}
rtk->sol.stat=stat;
for (i=0;i<3;i++) {
rtk->sol.rr[i]=rtk->x[i];
rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx];
}
rtk->sol.qr[3]=(float)rtk->P[1];
rtk->sol.qr[4]=(float)rtk->P[2+rtk->nx];
rtk->sol.qr[5]=(float)rtk->P[2];
rtk->sol.dtr[0]=rtk->x[IC(0,opt)];
rtk->sol.dtr[1]=rtk->x[IC(1,opt)]-rtk->x[IC(0,opt)];
for (i=0;i<n&&i<MAXOBS;i++) {
rtk->ssat[obs[i].sat-1].snr[0]=MIN(obs[i].SNR[0],obs[i].SNR[1]);
}
for (i=0;i<MAXSAT;i++) {
if (rtk->ssat[i].slip[0]&3) rtk->ssat[i].slipc[0]++;
}
}
free(rs); free(dts); free(var); free(azel);
free(xp); free(Pp); free(v); free(H); free(R);
}
