void verify_retry_results(wa::storage::retry_policy policy, web::http::status_code primary_status_code, web::http::status_code secondary_status_code, wa::storage::location_mode mode, std::function<std::chrono::milliseconds (int)> allowed_delta, std::vector<wa::storage::retry_info> expected_retry_info_list)
{
auto initial_location = get_initial_location(mode);
auto next_location = get_next_location(mode, initial_location);
wa::storage::operation_context op_context;
wa::storage::request_result result(utility::datetime::utc_now(),
initial_location,
web::http::http_response(initial_location == wa::storage::storage_location::secondary ? secondary_status_code : primary_status_code),
false);
int retry_count = 0;
for (auto iter = expected_retry_info_list.cbegin(); iter != expected_retry_info_list.cend(); ++iter)
{
auto retry_info = policy.evaluate(wa::storage::retry_context(retry_count++, result, next_location, mode), op_context);
CHECK(retry_info.should_retry());
CHECK(iter->target_location() == retry_info.target_location());
CHECK(iter->updated_location_mode() == retry_info.updated_location_mode());
CHECK_CLOSE(iter->retry_interval().count(), retry_info.retry_interval().count(), allowed_delta(retry_count).count());
std::this_thread::sleep_for(retry_info.retry_interval());
result = wa::storage::request_result(utility::datetime::utc_now(),
retry_info.target_location(),
web::http::http_response(retry_info.target_location() == wa::storage::storage_location::secondary ? secondary_status_code : primary_status_code),
false);
mode = retry_info.updated_location_mode();
next_location = get_next_location(mode, next_location);
}
auto retry_info = policy.evaluate(wa::storage::retry_context(retry_count++, result, next_location, mode), op_context);
CHECK(!retry_info.should_retry());
}
int main(int argc, char **argv) {
if (!bcm2835_init()) return 1;
if (argc < 3) {
printf("usage: %s {11|22|2302} GPIOpin# [ROS Parameters]\n", argv[0]);
printf("example: %s 2302 4 - Read from an AM2302 connected to GPIO #4\n", argv[0]);
return 2;
}
int type = 0;
if (strcmp(argv[1], "11") == 0) type = DHT11;
if (strcmp(argv[1], "22") == 0) type = DHT22;
if (strcmp(argv[1], "2302") == 0) type = AM2302;
if (type == 0) {
printf("Select 11, 22, 2302 as type!\n");
return 3;
}
int dhtpin = atoi(argv[2]);
if (dhtpin <= 0) {
printf("Please select a valid GPIO pin #\n");
return 3;
}
printf("Using pin #%d\n", dhtpin);
ros::init(argc, argv, "raspi_dht");
ros::NodeHandle n;
ros::Publisher temperature_pub = n.advertise<std_msgs::Float32>("temperature", 2);
ros::Publisher humidity_pub = n.advertise<std_msgs::Float32>("humidity", 2);
ros::Rate loop_rate(0.016);
ros::Duration retry_interval(5.0);
ros::Duration backoff_interval(200);
bool last_had_data = true;
while (ros::ok()) {
std_msgs::Float32 temperature_msg, humidity_msg;
float t, h;
if (readDHT(type, dhtpin, &t, &h)) {
temperature_msg.data = t;
humidity_msg.data = h;
#ifdef DEBUG
printf("Publishing data: t=%f, h=%f\n", t, h);
ROS_INFO("BOS_RASPI_DHT_NODE: %f, %f", t, h);
#endif
temperature_pub.publish(temperature_msg);
humidity_pub.publish(humidity_msg);
ros::spinOnce();
loop_rate.sleep();
last_had_data = true;
}
else {
if (last_had_data) {
ROS_INFO("BOS_raspi_DHT_node: no data, retrying soon");
retry_interval.sleep();
}
else {
ROS_INFO("BOS_raspi_DHT_node: no data, waiting a while");
backoff_interval.sleep();
}
last_had_data = false;
}
}
return 0;
} // main
int Usb::recv_measure() //copies device format data into the embedded measure_struct data type of the driver instance
{
int bytes_read=0, last_bytes_read=0, bytes_to_read=sizeof(measure_struct), status=ERROR;
int i, num_retry = 5;
unsigned char buf[6] = { 0 , 0 , 0 , 0 , 0 , 0 } ;
std::chrono::milliseconds retry_interval( 10 ) ; //sleep amount in case of soft fail
if(d!=NULL)
{
cerr<<" D| Procedura di lettura iniziata."<<endl;
hid_set_nonblocking(d,1); //Default - read bloccante (settare 1 per NON bloccante)
while( last_bytes_read != bytes_to_read && bytes_read!=-1 && num_retry>0) //Questo ciclo si ripete fino a 5 letture errate E SE NON ho un errore critico (-1) -- OLD: !get_stop() &&
{
num_retry--;
//Re-init
for(i=0;i<6;i++) buf[i] = 0;
i=0;
cerr<<" D! Tentativo "<<++i<<endl;
do
{
last_bytes_read=bytes_read; //Memorizza il numero di byte letti dal report precedente
bytes_read = hid_read(d,buf,bytes_to_read); //Leggi il report successivo:
//cerr<<bytes_read<<endl; // - se bytes_read>0 allora esiste un report più recente nel buffer -> scaricalo
// - se bytes_read=0, non ci sono più report -> l'ultimo scaricato è quello buono
// - se bytes_read=-1 c'é un errore critico con la device -> interrompi tutto
//La read si blocca AL PIU' per 5 secondi, dopodiché restituisce errore critico (-1)
}while(bytes_read>0); //Cicla finché il buffer non è stato svuotato (0) oppure c'è stato errore critico (-1)
//HARD FAIL
if (bytes_read == -1) //Se c'è stato errore...
{
cerr<<" D! Lettura fallita."<<endl;
cerr<<" D! ERRORE: Periferica non pronta o scollegata prematuramente.\n D| Disconnessione in corso..."<<endl;
hid_close(d); //chiudi la handle. -> ESCE dal ciclo.
cerr<<" D| Device disconnessa."<<endl;
d=NULL; //Resets handle pointer for safety
}
else //..altrimenti analizziamo per bene l'ultimo report scaricato...
{
//SOFT FAIL
if (last_bytes_read < bytes_to_read) //se i byte letti dell'ultima misura nel buffer (la più recente) non sono del numero giusto
{
cerr<<" D! Lettura fallita."<<endl; //-> la lettura è fallita, bisogna riprovare. -> NON ESCE dal ciclo.
std::this_thread::sleep_for( retry_interval ) ;
}
//GOOD
else //altrimenti -> stampa a video il risultato e memorizzalo in "m" -> ESCE dal ciclo.
{
//Debug dump visualization
cerr<<" D! "<<last_bytes_read<<" bytes letti: ";
cerr<<(int)buf[0]<<" "<<(int)buf[1]<<" "<<(int)buf[2]<<" "<<(int)buf[3]<<" "<<(int)buf[4]<<" "<<(int)buf[5]<<" "<<endl;
/* SWAP
for(i=0;i<6;i=i+2)
{
temp=buf[i];
buf[i]=buf[i+1];
buf[i+1]=temp;
}
*/
//Old approach
//memcpy( (void*) &m, (void*) buf, bytes_to_read);
//Lo shift dei registri del PIC è stato spostato lato driver
m.temp = (( buf[4] << 8 ) + buf[5])>>2 ;
m.humid = ((buf[2]<< 8 ) + buf[3]) & 0x3fff;
m.dust = ( buf[0] << 8 ) + buf[1] ;
if ( m.humid == 0xFFFF || m.temp == 0xFFFF )
{
std::cerr<<" D! WARNING: La device ha ritornato valori di Temperatura e Umidita' non validi." <<std::endl;
}
else status=NICE; //In conclusione, la funzione ritorna NICE solo se ha letto esattamente 6byte validi!
}
}
}
cerr<<" D| Procedura di lettura conclusa."<<endl;
}
