void SpecificWorker::searchTags(const cv::Mat &image_gray) { vector< ::AprilTags::TagDetection> detections = m_tagDetector->extractTags(image_gray); // print out each detection // cout << detections.size() << " tags detected:" << endl; print_detection(detections); // // if (m_draw) // { // for (uint i=0; i<detections.size(); i++) // { // detections[i].draw(image_gray); // } // //imshow("AprilTags", image_gray); // OpenCV call // } }
void stop_current_scan(struct ath_softc *sc) { struct ath_spectral *spectral = sc->sc_spectral; if (spectral == NULL) { SPECTRAL_DPRINTK(sc, ATH_DEBUG_SPECTRAL1, "%s: sc_spectral is NULL, HW may not be spectral capable\n", __func__); return; } ath_hal_stop_spectral_scan(sc->sc_ah); #ifdef SPECTRAL_CLASSIFIER_IN_KERNEL OS_CANCEL_TIMER(&spectral->classify_timer); #endif #ifdef SPECTRAL_DEBUG_TIMER OS_CANCEL_TIMER(&spectral->debug_timer); #endif if( spectral->classify_scan) { printk("%s - control chan detects=%d extension channel detects=%d above_dc_detects=%d below_dc_detects=%d\n", __func__,spectral->detects_control_channel, spectral->detects_extension_channel, spectral->detects_above_dc, spectral->detects_below_dc); spectral->detects_control_channel=0; spectral->detects_extension_channel=0; spectral->detects_above_dc=0; spectral->detects_below_dc=0; spectral->classify_scan = 0; print_detection(sc); } spectral->this_scan_phy_err = sc->sc_phy_stats[sc->sc_curmode].ast_rx_phyerr - spectral->cached_phy_err; spectral->send_single_packet = 0; spectral->ctl_eacs_avg_rssi=SPECTRAL_EACS_RSSI_THRESH; spectral->ext_eacs_avg_rssi=SPECTRAL_EACS_RSSI_THRESH; spectral->ctl_eacs_spectral_reports=0; spectral->ext_eacs_spectral_reports=0; }
void ath_process_spectraldata(struct ath_spectral *spectral, struct ath_buf *bf, struct ath_rx_status *rxs, u_int64_t fulltsf) { #define EXT_CH_RADAR_FOUND 0x02 #define PRI_CH_RADAR_FOUND 0x01 #define EXT_CH_RADAR_EARLY_FOUND 0x04 #define SPECTRAL_SCAN_DATA 0x10 #define DEFAULT_CHAN_NOISE_FLOOR -110 int i = 0; struct samp_msg_params params; u_int8_t rssi = 0; u_int8_t control_rssi = 0; u_int8_t extension_rssi = 0; u_int8_t combined_rssi = 0; u_int8_t max_exp = 0; u_int8_t max_scale = 0; u_int8_t dc_index = 0; u_int8_t lower_dc = 0; u_int8_t upper_dc = 0; u_int8_t ext_rssi = 0; int8_t inv_control_rssi = 0; int8_t inv_combined_rssi = 0; int8_t inv_extension_rssi = 0; int8_t temp_nf = 0; u_int8_t pulse_bw_info = 0; u_int8_t pulse_length_ext = 0; u_int8_t pulse_length_pri = 0; u_int32_t tstamp = 0; u_int16_t datalen = 0; u_int16_t max_mag = 0; u_int16_t newdatalen = 0; u_int16_t already_copied = 0; u_int16_t maxmag_upper = 0; u_int8_t maxindex_upper = 0; u_int8_t max_index = 0; int bin_pwr_count = 0; u_int32_t *last_word_ptr = NULL; u_int32_t *secondlast_word_ptr = NULL; u_int8_t *byte_ptr = NULL; u_int8_t *fft_data_end_ptr = NULL; u_int8_t last_byte_0 = 0; u_int8_t last_byte_1 = 0; u_int8_t last_byte_2 = 0; u_int8_t last_byte_3 = 0; u_int8_t secondlast_byte_0 = 0; u_int8_t secondlast_byte_1 = 0; u_int8_t secondlast_byte_2 = 0; u_int8_t secondlast_byte_3 = 0; HT20_FFT_PACKET fft_20; HT40_FFT_PACKET fft_40; u_int8_t *fft_data_ptr = NULL; u_int8_t *fft_src_ptr = NULL; u_int8_t *data_ptr = NULL; int8_t cmp_rssi = -110; int8_t rssi_up = 0; int8_t rssi_low = 0; static u_int64_t last_tsf = 0; static u_int64_t send_tstamp = 0; int8_t nfc_control_rssi = 0; int8_t nfc_extension_rssi = 0; int peak_freq = 0; int nb_lower = 0; int nb_upper = 0; int8_t ctl_chan_noise_floor = DEFAULT_CHAN_NOISE_FLOOR; int8_t ext_chan_noise_floor = DEFAULT_CHAN_NOISE_FLOOR; struct ath_softc* sc = NULL; SPECTRAL_OPS* p_sops = NULL; if (((rxs->rs_phyerr != HAL_PHYERR_RADAR)) && ((rxs->rs_phyerr != HAL_PHYERR_FALSE_RADAR_EXT)) && ((rxs->rs_phyerr != HAL_PHYERR_SPECTRAL))) { printk("%s : Unknown PHY error (0x%x)\n", __func__, rxs->rs_phyerr); return; } if (spectral == NULL) { printk("Spectral Module not attached\n"); return; } sc = GET_SPECTRAL_ATHSOFTC(spectral); p_sops = GET_SPECTRAL_OPS(spectral); spectral->ath_spectral_stats.total_phy_errors++; /* Get pointer to data & timestamp*/ datalen = rxs->rs_datalen; tstamp = (rxs->rs_tstamp & SPECTRAL_TSMASK); /* check for valid data length */ if ((!datalen) || (datalen < spectral->spectral_data_len - 1)) { spectral->ath_spectral_stats.datalen_discards++; return; } /* WAR: Never trust combined RSSI on radar pulses for <= * OWL2.0. For short pulses only the chain 0 rssi is present * and remaining descriptor data is all 0x80, for longer * pulses the descriptor is present, but the combined value is * inaccurate. This HW capability is queried in spectral_attach and stored in * the sc_spectral_combined_rssi_ok flag.*/ if (spectral->sc_spectral_combined_rssi_ok) { rssi = (u_int8_t) rxs->rs_rssi; } else { rssi = (u_int8_t) rxs->rs_rssi_ctl0; } /* get rssi values */ combined_rssi = rssi; control_rssi = (u_int8_t) rxs->rs_rssi_ctl0; extension_rssi = (u_int8_t) rxs->rs_rssi_ext0; ext_rssi = (u_int8_t) rxs->rs_rssi_ext0; /* * If the combined RSSI is less than a particular threshold, this pulse is of no * interest to the classifier, so discard it here itself * except when noise power cal is required (then we want all rssi values) */ inv_combined_rssi = fix_rssi_inv_only(combined_rssi); if (inv_combined_rssi < 5 && !spectral->sc_spectral_noise_pwr_cal) { return; } last_word_ptr = (u_int32_t *)(((u_int8_t*)bf->bf_vdata) + datalen - (datalen%4)); secondlast_word_ptr = last_word_ptr-1; byte_ptr = (u_int8_t*)last_word_ptr; last_byte_0 = (*(byte_ptr) & 0xff); last_byte_1 = (*(byte_ptr+1) & 0xff); last_byte_2 = (*(byte_ptr+2) & 0xff); last_byte_3 = (*(byte_ptr+3) & 0xff); byte_ptr = (u_int8_t*)secondlast_word_ptr; secondlast_byte_0 = (*(byte_ptr) & 0xff); secondlast_byte_1 = (*(byte_ptr+1) & 0xff); secondlast_byte_2 = (*(byte_ptr+2) & 0xff); secondlast_byte_3 = (*(byte_ptr+3) & 0xff); switch((datalen & 0x3)) { case 0: pulse_bw_info = secondlast_byte_3; pulse_length_ext = secondlast_byte_2; pulse_length_pri = secondlast_byte_1; byte_ptr = (u_int8_t*)secondlast_word_ptr; fft_data_end_ptr = (byte_ptr); break; case 1: pulse_bw_info = last_byte_0; pulse_length_ext = secondlast_byte_3; pulse_length_pri = secondlast_byte_2; byte_ptr = (u_int8_t*)secondlast_word_ptr; fft_data_end_ptr = (byte_ptr+2); break; case 2: pulse_bw_info = last_byte_1; pulse_length_ext = last_byte_0; pulse_length_pri = secondlast_byte_3; byte_ptr = (u_int8_t*)secondlast_word_ptr; fft_data_end_ptr = (byte_ptr+3); break; case 3: pulse_bw_info = last_byte_2; pulse_length_ext = last_byte_1; pulse_length_pri = last_byte_0; byte_ptr = (u_int8_t*)last_word_ptr; fft_data_end_ptr = (byte_ptr); break; default: printk( "datalen mod4=%d spectral_data_len=%d\n", (datalen%4), spectral->spectral_data_len); } /* * Only the last 3 bits of the BW info are relevant, * they indicate which channel the radar was detected in. */ pulse_bw_info &= 0x17; if (pulse_bw_info & SPECTRAL_SCAN_DATA) { if (datalen > spectral->spectral_data_len + 2) { //printk("Invalid spectral scan datalen = %d\n", datalen); return; } if (spectral->num_spectral_data == 0) { spectral->first_tstamp = tstamp; spectral->max_rssi = -110; //printk( "First FFT data tstamp = %u rssi=%d\n", tstamp, fix_rssi_inv_only(combined_rssi)); } spectral->num_spectral_data++; OS_MEMZERO(&fft_40, sizeof (fft_40)); OS_MEMZERO(&fft_20, sizeof (fft_20)); if (spectral->sc_spectral_20_40_mode) { fft_data_ptr = (u_int8_t*)&fft_40.lower_bins.bin_magnitude[0]; } else { fft_data_ptr = (u_int8_t*)&fft_20.lower_bins.bin_magnitude[0]; } byte_ptr = fft_data_ptr; if (datalen == spectral->spectral_data_len) { if (spectral->sc_spectral_20_40_mode) { // HT40 packet, correct length OS_MEMCPY(&fft_40, (u_int8_t*)(bf->bf_vdata), datalen); } else { // HT20 packet, correct length OS_MEMCPY(&fft_20, (u_int8_t*)(bf->bf_vdata), datalen); } } /* This happens when there is a missing byte because CCK is enabled. * This may happen with or without the second bug of the MAC inserting * 2 bytes */ if ((datalen == (spectral->spectral_data_len - 1)) || (datalen == (spectral->spectral_data_len + 1))) { printk( "%s %d missing 1 byte datalen=%d expected=%d\n", __func__, __LINE__, datalen, spectral->spectral_data_len); already_copied++; if (spectral->sc_spectral_20_40_mode) { // HT40 packet, missing 1 byte // Use the beginning byte as byte 0 and byte 1 fft_40.lower_bins.bin_magnitude[0]=*(u_int8_t*)(bf->bf_vdata); // Now copy over the rest fft_data_ptr = (u_int8_t*)&fft_40.lower_bins.bin_magnitude[1]; OS_MEMCPY(fft_data_ptr, (u_int8_t*)(bf->bf_vdata), datalen); } else { // HT20 packet, missing 1 byte // Use the beginning byte as byte 0 and byte 1 fft_20.lower_bins.bin_magnitude[0]=*(u_int8_t*)(bf->bf_vdata); // Now copy over the rest fft_data_ptr = (u_int8_t*)&fft_20.lower_bins.bin_magnitude[1]; OS_MEMCPY(fft_data_ptr, (u_int8_t*)(bf->bf_vdata), datalen); } } if ((datalen == (spectral->spectral_data_len + 1)) || (datalen == (spectral->spectral_data_len + 2))) { //printk( "%s %d extra bytes datalen=%d expected=%d\n", __func__, __LINE__, datalen, spectral->spectral_data_len); if (spectral->sc_spectral_20_40_mode) {// HT40 packet, MAC added 2 extra bytes fft_src_ptr = (u_int8_t*)(bf->bf_vdata); fft_data_ptr = (u_int8_t*)&fft_40.lower_bins.bin_magnitude[already_copied]; for( i = 0, newdatalen=0; newdatalen < (SPECTRAL_HT40_DATA_LEN - already_copied); i++) { if (i == 30 || i == 32) { continue; } *(fft_data_ptr+newdatalen)=*(fft_src_ptr+i); newdatalen++; } } else { //HT20 packet, MAC added 2 extra bytes fft_src_ptr = (u_int8_t*)(bf->bf_vdata); fft_data_ptr = (u_int8_t*)&fft_20.lower_bins.bin_magnitude[already_copied]; for(i=0, newdatalen=0; newdatalen < (SPECTRAL_HT20_DATA_LEN - already_copied); i++) { if (i == 30 || i == 32) continue; *(fft_data_ptr+newdatalen)=*(fft_src_ptr+i); newdatalen++; } } } spectral->total_spectral_data++; dc_index = spectral->spectral_dc_index; if (spectral->sc_spectral_20_40_mode) { max_exp = (fft_40.max_exp & 0x07); byte_ptr = (u_int8_t*)&fft_40.lower_bins.bin_magnitude[0]; /* Correct the DC bin value */ lower_dc = *(byte_ptr+dc_index-1); upper_dc = *(byte_ptr+dc_index+1); *(byte_ptr+dc_index)=((upper_dc + lower_dc)/2); } else { max_exp = (fft_20.max_exp & 0x07); byte_ptr = (u_int8_t*)&fft_20.lower_bins.bin_magnitude[0]; /* Correct the DC bin value */ lower_dc = *(byte_ptr+dc_index-1); upper_dc = *(byte_ptr+dc_index+1); *(byte_ptr+dc_index)=((upper_dc + lower_dc)/2); } if (p_sops->get_ent_spectral_mask(spectral)) { *(byte_ptr+dc_index) &= ~((1 << p_sops->get_ent_spectral_mask(spectral)) - 1); } if (max_exp < 1) { max_scale = 1; } else { max_scale = (2) << (max_exp - 1); } bin_pwr_count = spectral->spectral_numbins; if ((last_tsf > fulltsf) && (!spectral->classify_scan)) { spectral->send_single_packet = 1; last_tsf = fulltsf; } inv_combined_rssi = fix_rssi_inv_only(combined_rssi); inv_control_rssi = fix_rssi_inv_only(control_rssi); inv_extension_rssi = fix_rssi_inv_only(extension_rssi); { if (spectral->upper_is_control) rssi_up = control_rssi; else rssi_up = extension_rssi; if (spectral->lower_is_control) rssi_low = control_rssi; else rssi_low = extension_rssi; nfc_control_rssi = get_nfc_ctl_rssi(spectral, inv_control_rssi, &temp_nf); ctl_chan_noise_floor = temp_nf; rssi_low = fix_rssi_for_classifier(spectral, rssi_low, spectral->lower_is_control, spectral->lower_is_extension); if (spectral->sc_spectral_20_40_mode) { rssi_up = fix_rssi_for_classifier(spectral, rssi_up,spectral->upper_is_control,spectral->upper_is_extension); nfc_extension_rssi = get_nfc_ext_rssi(spectral, inv_extension_rssi, &temp_nf); ext_chan_noise_floor = temp_nf; } } if(sc->sc_ieee_ops->spectral_eacs_update) { sc->sc_ieee_ops->spectral_eacs_update(sc->sc_ieee, nfc_control_rssi, nfc_extension_rssi, ctl_chan_noise_floor, ext_chan_noise_floor); } if (!spectral->sc_spectral_20_40_mode) { rssi_up = 0; extension_rssi = 0; inv_extension_rssi = 0; nb_upper = 0; maxindex_upper = 0; maxmag_upper = 0; } params.rssi = inv_combined_rssi; params.lower_rssi = rssi_low; params.upper_rssi = rssi_up; if (spectral->sc_spectral_noise_pwr_cal) { params.chain_ctl_rssi[0] = fix_rssi_inv_only(rxs->rs_rssi_ctl0); params.chain_ctl_rssi[1] = fix_rssi_inv_only(rxs->rs_rssi_ctl1); params.chain_ctl_rssi[2] = fix_rssi_inv_only(rxs->rs_rssi_ctl2); params.chain_ext_rssi[0] = fix_rssi_inv_only(rxs->rs_rssi_ext0); params.chain_ext_rssi[1] = fix_rssi_inv_only(rxs->rs_rssi_ext1); params.chain_ext_rssi[2] = fix_rssi_inv_only(rxs->rs_rssi_ext2); } params.bwinfo = pulse_bw_info; params.tstamp = tstamp; params.max_mag = max_mag; params.max_index = max_index; params.max_exp = max_scale; params.peak = peak_freq; params.pwr_count = bin_pwr_count; params.bin_pwr_data = &byte_ptr; params.freq = p_sops->get_current_channel(spectral); if(sc->sc_ieee_ops->spectral_get_freq_loading) { params.freq_loading = sc->sc_ieee_ops->spectral_get_freq_loading(sc->sc_ieee); } else { params.freq_loading = 0; } params.interf_list.count = 0; params.max_lower_index = 0;//maxindex_lower; params.max_upper_index = 0;//maxindex_upper; params.nb_lower = nb_lower; params.nb_upper = nb_upper; params.last_tstamp = spectral->last_tstamp; get_nfc_ctl_rssi(spectral, inv_control_rssi, &temp_nf); params.noise_floor = (int16_t)temp_nf; OS_MEMCPY(¶ms.classifier_params, &spectral->classifier_params, sizeof(SPECTRAL_CLASSIFIER_PARAMS)); cmp_rssi = fix_rssi_inv_only (combined_rssi); spectral->send_single_packet = 0; #ifdef SPECTRAL_DEBUG_TIMER OS_CANCEL_TIMER(&spectral->debug_timer); #endif spectral->spectral_sent_msg++; params.datalen = datalen; if (spectral->sc_spectral_20_40_mode) { data_ptr = (u_int8_t*)&fft_40.lower_bins.bin_magnitude; } else { data_ptr = (u_int8_t*)&fft_20.lower_bins.bin_magnitude; } byte_ptr = (u_int8_t*)data_ptr; params.bin_pwr_data = &byte_ptr; send_tstamp = p_sops->get_tsf64(spectral); spectral_create_samp_msg(spectral, ¶ms); #ifdef SPECTRAL_CLASSIFIER_IN_KERNEL if (spectral->classify_scan && maxindex_lower != (SPECTRAL_HT20_DC_INDEX + 1)) { classifier(&spectral->bd_lower, tstamp, spectral->last_tstamp, rssi_low, nb_lower, maxindex_lower); if (spectral->sc_spectral_20_40_mode && maxindex_upper != (SPECTRAL_HT40_DC_INDEX)) { classifier(&spectral->bd_upper, tstamp, spectral->last_tstamp, rssi_up, nb_upper, maxindex_upper); } print_detection(sc); } #endif /* SPECTRAL_CLASSIFIER_IN_KERNEL */ spectral->last_tstamp=tstamp; return; } else { /* * Add a HAL capability that tests if chip is capable of spectral scan. * Probably just a check if its a Merlin and above. */ printk("Non Spectral error\n"); spectral->ath_spectral_stats.owl_phy_errors++; } #undef EXT_CH_RADAR_FOUND #undef PRI_CH_RADAR_FOUND #undef EXT_CH_RADAR_EARLY_FOUND #undef SPECTRAL_SCAN_DATA }
// TODO: This is currently a dirty, filthy, no-good duplicate of the callback. void run_april_tag_detection_and_processing(cv::Mat& image_gray) { #ifdef DEBUG_APRIL_LATENCY_PROFILING // Need a new way to profile #endif //DEBUG_APRIL_LATENCY_PROFILING #ifdef DEBUG_APRIL_PROFILING static int n_count = 0; static double t_accum = 0; ros::Time start = ros::Time::now(); if ( (ros::Time::now() - prev_t).toSec() > 1.0 ) { crop_image = false; } #endif // DEBUG_APRIL_PROFILING #ifdef DEBUG_ROS_APRIL bool curr_frame_cropped = false; #endif vector<AprilTags::TagDetection> detections; vector<cv::Point> rect_corners; if (!new_tag_pos_prediction) { crop_image = false; } // Clear flag so we don't window the same place again new_tag_pos_prediction = false; if (crop_image) { rect_corners = calculate_roi_from_pose_estimate(window_centre_point); crop_image = fix_roi_boundaries(image_gray, rect_corners); } detections= m_tagDetector->extractTags(image_gray); for(int i = 0; i < (int) detections.size(); i++) { print_detection(detections[i]); } #ifdef DEBUG_ROS_APRIL for (int i=0; i < (int) detections.size(); i++) { // also highlight in the image if(detections[i].id == 0) { detections[i].draw(image_gray); } } if (curr_frame_cropped) { cv::rectangle(image_gray, rect_corners[0], rect_corners[1], cv::Scalar(0,255,255), 3); } imshow("AprilResult", image_gray); // OpenCV call cv::waitKey(30); #endif #ifdef DEBUG_APRIL_PROFILING ros::Time end = ros::Time::now(); n_count++; t_accum += (end - start).toSec(); if (n_count >= 100) { ROS_DEBUG("Avg april tag run time: %f", t_accum/100.0); std::cerr << "Avg april tag run time: " << t_accum/100.0 << std::endl; n_count = 0; t_accum = 0; } #endif // DEBUG_APRIL_PROFILING #ifdef DEBUG_APRIL_LATENCY_PROFILING // Need a new way to profile #endif //DEBUG_APRIL_LATENCY_PROFILING }