/* * set_auto_WP - sets the target location the vehicle should drive to in Auto mode */ void Rover::set_auto_WP(const struct Location& loc) { // copy the current WP into the OldWP slot // --------------------------------------- prev_WP = next_WP; // Load the next_WP slot // --------------------- next_WP = loc; // are we already past the waypoint? This happens when we jump // waypoints, and it can cause us to skip a waypoint. If we are // past the waypoint when we start on a leg, then use the current // location as the previous waypoint, to prevent immediately // considering the waypoint complete if (location_passed_point(current_loc, prev_WP, next_WP)) { gcs().send_text(MAV_SEVERITY_NOTICE, "Resetting previous WP"); prev_WP = current_loc; } // this is handy for the groundstation wp_totalDistance = get_distance(current_loc, next_WP); wp_distance = wp_totalDistance; }
void PointerMapper::compute_calibration_points() { vector<float> x_vec; vector<float> y_vec; vector<float> z_vec; float x_median; float y_median; float z_median; if (pt_calib_vec0.size() > 0 && pt_calib_vec0.size() > 1 && pt_calib_vec0.size() > 2 && pt_calib_vec0.size() > 3) { x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec0) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib0 = Point3f(x_median, y_median, z_median); x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec1) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib1 = Point3f(x_median, y_median, z_median); x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec2) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib2 = Point3f(x_median, y_median, z_median); x_vec = vector<float>(); y_vec = vector<float>(); z_vec = vector<float>(); for (Point3f& pt : pt_calib_vec3) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); z_vec.push_back(pt.z); } sort(x_vec.begin(), x_vec.end()); sort(y_vec.begin(), y_vec.end()); sort(z_vec.begin(), z_vec.end()); x_median = x_vec[x_vec.size() * 0.5]; y_median = y_vec[y_vec.size() * 0.5]; z_median = z_vec[z_vec.size() * 0.5]; pt_calib3 = Point3f(x_median, y_median, z_median); const float x0_plane = pt_calib0.x; const float y0_plane = pt_calib0.y; const float z0_plane = pt_calib0.z + 100; const float x1_plane = pt_calib1.x; const float y1_plane = pt_calib1.y; const float z1_plane = pt_calib1.z + 100; const float x2_plane = pt_calib2.x; const float y2_plane = pt_calib2.y; const float z2_plane = pt_calib2.z + 100; const float x3_plane = pt_calib3.x; const float y3_plane = pt_calib3.y; const float z3_plane = pt_calib3.z + 100; plane = Plane(Point3f(x0_plane, y0_plane, z0_plane), Point3f(x1_plane, y1_plane, z1_plane), Point3f(x2_plane, y2_plane, z2_plane)); direction_plane = cross_product(Point3f(x0_plane - x1_plane, y0_plane - y1_plane, z0_plane - z1_plane), Point3f(x2_plane - x1_plane, y2_plane - y1_plane, z2_plane - z1_plane)); direction_plane.x = -direction_plane.x; direction_plane.y = -direction_plane.y; direction_plane.z = -direction_plane.z; project_to_plane(pt_calib0, pt_calib0_projected, dist_calib0_plane); project_to_plane(pt_calib1, pt_calib1_projected, dist_calib1_plane); project_to_plane(pt_calib2, pt_calib2_projected, dist_calib2_plane); project_to_plane(pt_calib3, pt_calib3_projected, dist_calib3_plane); const float d0 = get_distance(pt_calib0_projected.x, pt_calib0_projected.y, pt_calib2_projected.x, pt_calib2_projected.y); const float d1 = get_distance(pt_calib1_projected.x, pt_calib1_projected.y, pt_calib3_projected.x, pt_calib3_projected.y); d_max = std::max(d0, d1); rect_warper.setSource(pt_calib0_projected.x, pt_calib0_projected.y, pt_calib1_projected.x, pt_calib1_projected.y, pt_calib2_projected.x, pt_calib2_projected.y, pt_calib3_projected.x, pt_calib3_projected.y); rect_warper.setDestination(0, 0, 1000, 0, 1000, 1000, 0, 1000); calibrated = true; } else COUT << "not calibrated" << endl; }
bool ToolMonoProcessor::compute(Mat& image_active_light_in, Mat& image_preprocessed_in, const string name) { Mat image_preprocessed_old = value_store.get_mat("image_preprocessed_old", true); Mat image_subtraction = Mat::zeros(HEIGHT_SMALL, WIDTH_SMALL, CV_8UC1); int diff_max = 0; for (int i = 0; i < WIDTH_SMALL; ++i) for (int j = 0; j < HEIGHT_SMALL; ++j) { int diff = abs(image_preprocessed_in.ptr<uchar>(j, i)[0] - image_preprocessed_old.ptr<uchar>(j, i)[0]); if (diff > diff_max) diff_max = diff; image_subtraction.ptr<uchar>(j, i)[0] = diff; } value_store.set_mat("image_preprocessed_old", image_preprocessed_in); threshold(image_subtraction, image_subtraction, diff_max * 0.25, 254, THRESH_BINARY); BlobDetectorNew* blob_detector_image_subtraction = value_store.get_blob_detector("blob_detector_image_subtraction"); blob_detector_image_subtraction->compute(image_subtraction, 254, 0, WIDTH_SMALL, 0, HEIGHT_SMALL, true); const int width_result = blob_detector_image_subtraction->x_max_result - blob_detector_image_subtraction->x_min_result; const int height_result = blob_detector_image_subtraction->y_max_result - blob_detector_image_subtraction->y_min_result; const int area_result = width_result * height_result; bool proceed = true; if (blob_detector_image_subtraction->blobs->size() > 50 || blob_detector_image_subtraction->blobs->size() == 0/*|| area_result > 8000*/) proceed = false; if (proceed) { vector<int> x_vec; vector<int> y_vec; for (BlobNew& blob : (*(blob_detector_image_subtraction->blobs))) for (Point& pt : blob.data) { x_vec.push_back(pt.x); y_vec.push_back(pt.y); } int x_mean = 0; for (int& x : x_vec) x_mean += x; int y_mean = 0; for (int& y : y_vec) y_mean += y; x_mean /= x_vec.size(); y_mean /= y_vec.size(); pt_motion_center = Point(x_mean, y_mean); Point pt_motion_center_old = value_store.get_point("pt_motion_center_old", pt_motion_center); pt_motion_center.x = (pt_motion_center.x - pt_motion_center_old.x) * 0.25 + pt_motion_center.x; pt_motion_center.y = (pt_motion_center.y - pt_motion_center_old.y) * 0.25 + pt_motion_center.y; if (pt_motion_center.x < 0) pt_motion_center.x = 0; if (pt_motion_center.x > WIDTH_SMALL_MINUS) pt_motion_center.x = WIDTH_SMALL_MINUS; if (pt_motion_center.y < 0) pt_motion_center.y = 0; if (pt_motion_center.y > HEIGHT_SMALL_MINUS) pt_motion_center.y = HEIGHT_SMALL_MINUS; value_store.set_point("pt_motion_center_old", pt_motion_center); vector<float> dist_vec; for (BlobNew& blob : (*(blob_detector_image_subtraction->blobs))) for (Point& pt : blob.data) { float dist = get_distance(pt, pt_motion_center); dist_vec.push_back(dist); } sort(dist_vec.begin(), dist_vec.end()); radius = dist_vec[dist_vec.size() * 0.8] * 1.5; LowPassFilter* low_pass_filter = value_store.get_low_pass_filter("low_pass_filter"); low_pass_filter->compute(radius, 0.5, "radius"); } Mat image_active_light_subtraction = Mat(HEIGHT_SMALL, WIDTH_SMALL, CV_8UC1); for (int i = 0; i < WIDTH_SMALL; ++i) for (int j = 0; j < HEIGHT_SMALL; ++j) if (image_background_static.ptr<uchar>(j, i)[0] == 255) image_active_light_subtraction.ptr<uchar>(j, i)[0] = image_active_light_in.ptr<uchar>(j, i)[0]; else { int diff = image_active_light_in.ptr<uchar>(j, i)[0] - image_background_static.ptr<uchar>(j, i)[0]; if (diff < 0) diff = 0; image_active_light_subtraction.ptr<uchar>(j, i)[0] = diff; } Mat image_thresholded; threshold(image_active_light_subtraction, image_thresholded, 200, 254, THRESH_BINARY); dilate(image_thresholded, image_thresholded, Mat(), Point(-1, -1), 1); BlobDetectorNew* blob_detector_image_thresholded = value_store.get_blob_detector("blob_detector_image_thresholded"); blob_detector_image_thresholded->compute(image_thresholded, 254, 0, WIDTH_SMALL, 0, HEIGHT_SMALL, true); if (blob_detector_image_thresholded->blobs->size() == 0) return false; Mat image_circle = Mat::zeros(HEIGHT_SMALL, WIDTH_SMALL, CV_8UC1); circle(image_circle, pt_motion_center, radius, Scalar(254), 1); floodFill(image_circle, pt_motion_center, Scalar(254)); for (int i = 0; i < WIDTH_SMALL; ++i) for (int j = 0; j < HEIGHT_SMALL; ++j) if (image_circle.ptr<uchar>(j, i)[0] == 0) fill_image_background_static(i, j, image_active_light_in); BlobDetectorNew* blob_detector_image_circle = value_store.get_blob_detector("blob_detector_image_circle"); blob_detector_image_circle->compute_location(image_circle, 254, pt_motion_center.x, pt_motion_center.y, true); blob_vec.clear(); for (BlobNew& blob : *blob_detector_image_thresholded->blobs) for (Point& pt : blob_detector_image_circle->blob_max_size->data) if (blob.image_atlas.ptr<ushort>(pt.y, pt.x)[0] == blob.atlas_id) { blob_vec.push_back(&blob); break; } if (blob_vec.size() >= 4) { vector<int> blob_x_vec; vector<int> blob_y_vec; for (BlobNew* blob0 : blob_vec) for (BlobNew* blob1 : blob_vec) { blob_x_vec.push_back((blob0->x + blob1->x) / 2); blob_y_vec.push_back((blob0->y + blob1->y) / 2); } sort(blob_x_vec.begin(), blob_x_vec.end()); sort(blob_y_vec.begin(), blob_y_vec.end()); pt_led_center = Point(blob_x_vec[blob_x_vec.size() / 2], blob_y_vec[blob_y_vec.size() / 2]); } else return false; if (blob_vec.size() == 5) { float dist_min = 9999; BlobNew* blob_dist_min = NULL; for (BlobNew* blob : blob_vec) { float dist = blob->compute_min_dist(pt_led_center); if (dist < dist_min) { dist_min = dist; blob_dist_min = blob; } } vector<BlobNew*> blob_vec_new; for (BlobNew* blob : blob_vec) if (blob != blob_dist_min) blob_vec_new.push_back(blob); blob_vec = blob_vec_new; blob_dist_min->fill(image_thresholded, 127); } pt_led0 = Point(blob_vec[0]->x, blob_vec[0]->y); pt_led1 = Point(blob_vec[1]->x, blob_vec[1]->y); pt_led2 = Point(blob_vec[2]->x, blob_vec[2]->y); pt_led3 = Point(blob_vec[3]->x, blob_vec[3]->y); x_min0 = blob_vec[0]->x_min; x_max0 = blob_vec[0]->x_max; y_min0 = blob_vec[0]->y_min; y_max0 = blob_vec[0]->y_max; x_min1 = blob_vec[1]->x_min; x_max1 = blob_vec[1]->x_max; y_min1 = blob_vec[1]->y_min; y_max1 = blob_vec[1]->y_max; x_min2 = blob_vec[2]->x_min; x_max2 = blob_vec[2]->x_max; y_min2 = blob_vec[2]->y_min; y_max2 = blob_vec[2]->y_max; x_min3 = blob_vec[3]->x_min; x_max3 = blob_vec[3]->x_max; y_min3 = blob_vec[3]->y_min; y_max3 = blob_vec[3]->y_max; circle(image_thresholded, pt_motion_center, radius, Scalar(127), 2); imshow("image_thresholded" + name, image_thresholded); return true; }
/* * Determine if we have crashed */ void Plane::crash_detection_update(void) { if (control_mode != AUTO || !g.crash_detection_enable) { // crash detection is only available in AUTO mode crash_state.debounce_timer_ms = 0; crash_state.is_crashed = false; return; } uint32_t now_ms = AP_HAL::millis(); bool crashed_near_land_waypoint = false; bool crashed = false; bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) && (now_ms - auto_state.started_flying_in_auto_ms >= 2500); if (!is_flying() && been_auto_flying) { switch (flight_stage) { case AP_SpdHgtControl::FLIGHT_TAKEOFF: case AP_SpdHgtControl::FLIGHT_NORMAL: if (!in_preLaunch_flight_stage()) { crashed = true; } // TODO: handle auto missions without NAV_TAKEOFF mission cmd break; case AP_SpdHgtControl::FLIGHT_VTOL: // we need a totally new method for this crashed = false; break; case AP_SpdHgtControl::FLIGHT_LAND_APPROACH: crashed = true; // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL // so ground crashes most likely can not be triggered from here. However, // a crash into a tree would be caught here. break; case AP_SpdHgtControl::FLIGHT_LAND_PREFLARE: case AP_SpdHgtControl::FLIGHT_LAND_FINAL: // We should be nice and level-ish in this flight stage. If not, we most // likely had a crazy landing. Throttle is inhibited already at the flare // but go ahead and notify GCS and perform any additional post-crash actions. // Declare a crash if we are oriented more that 60deg in pitch or roll if (!crash_state.checkedHardLanding && // only check once (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) { crashed = true; // did we "crash" within 75m of the landing location? Probably just a hard landing crashed_near_land_waypoint = get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75; // trigger hard landing event right away, or never again. This inhibits a false hard landing // event when, for example, a minute after a good landing you pick the plane up and // this logic is still running and detects the plane is on its side as you carry it. crash_state.debounce_timer_ms = now_ms + CRASH_DETECTION_DELAY_MS; } crash_state.checkedHardLanding = true; break; default: break; } // switch } else { crash_state.checkedHardLanding = false; } if (!crashed) { // reset timer crash_state.debounce_timer_ms = 0; } else if (crash_state.debounce_timer_ms == 0) { // start timer crash_state.debounce_timer_ms = now_ms; } else if ((now_ms - crash_state.debounce_timer_ms >= CRASH_DETECTION_DELAY_MS) && !crash_state.is_crashed) { crash_state.is_crashed = true; if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) { if (crashed_near_land_waypoint) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected. No action taken"); } else { gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected. No action taken"); } } else { if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) { disarm_motors(); } auto_state.land_complete = true; if (crashed_near_land_waypoint) { gcs_send_text(MAV_SEVERITY_CRITICAL, "Hard landing detected"); } else { gcs_send_text(MAV_SEVERITY_EMERGENCY, "Crash detected"); } } } }
virtual bool Run(RobotInfo ri, NavInfo ni, EventFlag evf, RobotCmd& cmd ){ bool stay_mission = true; switch (local_status_){ case P_REACHING: //シーソーに近づく speed_ = 10; if( evf.is_SetEvent(Robot::EVENT_GYRO)){ evf.Set(Robot::EVENT_GYRO, false); Robot::Instance().Beep(); set_current_position(pos, ni); local_status_ = S_APPROACH_BACK; // 助走するために少し下がる tec_s_.Init(ri); tecType_ = TEC_S; speed_ = -30; } break; case S_APPROACH_BACK: if( get_distance(pos, ni) > 100*100){ set_current_position(pos, ni); Robot::Instance().Beep(); local_status_ = S_STAGING; // 一気に登る tec_s_.Init(ri); tecType_ = TEC_S; speed_ = 100; } break; case S_STAGING: // 一気にシーソーへ登る if( get_distance(pos, ni) > 180*180){ set_current_position(pos, ni); Robot::Instance().Beep(Robot::B); local_status_ = P_RECOVER_ON_SEESAW; // とりあえずシーソー上でラインに復帰 tecType_ = TEC_P; tec_p_.Init(); speed_ = 10; } break; case P_RECOVER_ON_SEESAW: // とりあえずシーソー上でラインに復帰 if( get_distance(pos, ni) > 200*200 ){ set_current_position(pos, ni); Robot::Instance().Beep(); local_status_ = S_GET_OFF; //左にそれながらシーソーを降りる tec_s_.Init(ri); tecType_ = TEC_S; speed_ = 40; } break; case S_GET_OFF: if(get_distance(pos, ni) > 250*250){ Robot::Instance().Beep(Robot::B); local_status_ = S_RECOVER_ON_GROUND; // ラインに復帰を試みる tec_s_.Init(ri); tecType_ = TEC_S; speed_ = 10; } break; case S_RECOVER_ON_GROUND: /* 終了条件 */ if(evf.is_SetEvent(Robot::EVENT_LIGHT_BLACK)){ set_current_position(pos, ni); local_status_ = P_FOR_NEXT_MISSION; Robot::Instance().Beep(Robot::G); tecType_ = TEC_P; tec_p_.Init(); speed_ = 20; TimerStart(10000); } break; case P_FOR_NEXT_MISSION: // 最後に安定するまでライントレースして完全復帰する /* ミッション終了の条件 */ if(get_distance(pos, ni) > 200*200){ Robot::Instance().Beep(Robot::E); stay_mission = false; } break; default: stay_mission = true; break; } // end of switch-case(local_status_) /* 走行計算 */ if(tecType_ == TEC_S){ cmd = tec_s_.Calclate(speed_, ri.rot_encoderL_val, ri.rot_encoderR_val ); if(local_status_ == S_GET_OFF){ cmd.param2 =- 3; // 左側へ降りる } if(local_status_ == S_RECOVER_ON_GROUND){ cmd.param2 =+ 3; // 右側のラインへ復帰する } }else if(tecType_ == TEC_P){ cmd = tec_p_.Calclate(ri.light_sensor_val, speed_, edge_dir); // if(local_status_ == S_STAGING || local_status_ == P_RECOVER_ON_SEESAW){ // cmd.param2 = (S16)((F32)cmd.param2 * 0.7); // } }else if(tecType_ == TEC_R){ cmd = tec_r_.Calclate(speed_, rotate_, dir_); }else if(tecType_ == TEC_T){ // しっぽの設定はcase内で設定するよ }else{} return stay_mission; }
/* send a report to the vehicle control code over MAVLink */ void ADSB::send_report(void) { if (AP_HAL::millis() < 10000) { // simulated aircraft don't appear until 10s after startup. This avoids a windows // threading issue with non-blocking sockets and the initial wait on uartA return; } if (!mavlink.connected && mav_socket.connect(target_address, target_port)) { ::printf("ADSB connected to %s:%u\n", target_address, (unsigned)target_port); mavlink.connected = true; } if (!mavlink.connected) { return; } // check for incoming MAVLink messages uint8_t buf[100]; ssize_t ret; while ((ret=mav_socket.recv(buf, sizeof(buf), 0)) > 0) { for (uint8_t i=0; i<ret; i++) { mavlink_message_t msg; mavlink_status_t status; if (mavlink_frame_char_buffer(&mavlink.rxmsg, &mavlink.status, buf[i], &msg, &status) == MAVLINK_FRAMING_OK) { switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: { if (!seen_heartbeat) { seen_heartbeat = true; vehicle_component_id = msg.compid; vehicle_system_id = msg.sysid; ::printf("ADSB using srcSystem %u\n", (unsigned)vehicle_system_id); } break; } } } } } if (!seen_heartbeat) { return; } uint32_t now = AP_HAL::millis(); mavlink_message_t msg; uint16_t len; if (now - last_heartbeat_ms >= 1000) { mavlink_heartbeat_t heartbeat; heartbeat.type = MAV_TYPE_ADSB; heartbeat.autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA; heartbeat.base_mode = 0; heartbeat.system_status = 0; heartbeat.mavlink_version = 0; heartbeat.custom_mode = 0; /* save and restore sequence number for chan0, as it is used by generated encode functions */ mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_heartbeat_encode(vehicle_system_id, vehicle_component_id, &msg, &heartbeat); chan0_status->current_tx_seq = saved_seq; mav_socket.send(&msg.magic, len); last_heartbeat_ms = now; } /* send a ADSB_VEHICLE messages */ uint32_t now_us = AP_HAL::micros(); if (now_us - last_report_us >= reporting_period_ms*1000UL) { for (uint8_t i=0; i<num_vehicles; i++) { ADSB_Vehicle &vehicle = vehicles[i]; Location loc = home; location_offset(loc, vehicle.position.x, vehicle.position.y); // re-init when exceeding radius range if (get_distance(home, loc) > _sitl->adsb_radius_m) { vehicle.initialised = false; } mavlink_adsb_vehicle_t adsb_vehicle {}; last_report_us = now_us; adsb_vehicle.ICAO_address = vehicle.ICAO_address; adsb_vehicle.lat = loc.lat; adsb_vehicle.lon = loc.lng; adsb_vehicle.altitude_type = ADSB_ALTITUDE_TYPE_PRESSURE_QNH; adsb_vehicle.altitude = -vehicle.position.z * 1000; adsb_vehicle.heading = wrap_360_cd(100*degrees(atan2f(vehicle.velocity_ef.y, vehicle.velocity_ef.x))); adsb_vehicle.hor_velocity = norm(vehicle.velocity_ef.x, vehicle.velocity_ef.y) * 100; adsb_vehicle.ver_velocity = -vehicle.velocity_ef.z * 100; memcpy(adsb_vehicle.callsign, vehicle.callsign, sizeof(adsb_vehicle.callsign)); adsb_vehicle.emitter_type = ADSB_EMITTER_TYPE_LARGE; adsb_vehicle.tslc = 1; adsb_vehicle.flags = ADSB_FLAGS_VALID_COORDS | ADSB_FLAGS_VALID_ALTITUDE | ADSB_FLAGS_VALID_HEADING | ADSB_FLAGS_VALID_VELOCITY | ADSB_FLAGS_VALID_CALLSIGN | ADSB_FLAGS_SIMULATED; adsb_vehicle.squawk = 0; // NOTE: ADSB_FLAGS_VALID_SQUAWK bit is not set mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; chan0_status->current_tx_seq = mavlink.seq; len = mavlink_msg_adsb_vehicle_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &adsb_vehicle); chan0_status->current_tx_seq = saved_seq; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); } } } // ADSB_transceiever is enabled, send the status report. if (_sitl->adsb_tx && now - last_tx_report_ms > 1000) { last_tx_report_ms = now; mavlink_status_t *chan0_status = mavlink_get_channel_status(MAVLINK_COMM_0); uint8_t saved_seq = chan0_status->current_tx_seq; uint8_t saved_flags = chan0_status->flags; chan0_status->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; chan0_status->current_tx_seq = mavlink.seq; const mavlink_uavionix_adsb_transceiver_health_report_t health_report = {UAVIONIX_ADSB_RF_HEALTH_OK}; len = mavlink_msg_uavionix_adsb_transceiver_health_report_encode(vehicle_system_id, MAV_COMP_ID_ADSB, &msg, &health_report); chan0_status->current_tx_seq = saved_seq; chan0_status->flags = saved_flags; uint8_t msgbuf[len]; len = mavlink_msg_to_send_buffer(msgbuf, &msg); if (len > 0) { mav_socket.send(msgbuf, len); ::printf("ADSBsim send tx health packet\n"); } } }
/* * Calculate selectivity of "var <@ const" operator, ie. estimate the fraction * of ranges that fall within the constant lower and upper bounds. This uses * the histograms of range lower bounds and range lengths, on the assumption * that the range lengths are independent of the lower bounds. * * The caller has already checked that constant lower and upper bounds are * finite. */ static double calc_hist_selectivity_contained(TypeCacheEntry *typcache, RangeBound *lower, RangeBound *upper, RangeBound *hist_lower, int hist_nvalues, Datum *length_hist_values, int length_hist_nvalues) { int i, upper_index; float8 prev_dist; double bin_width; double upper_bin_width; double sum_frac; /* * Begin by finding the bin containing the upper bound, in the lower bound * histogram. Any range with a lower bound > constant upper bound can't * match, ie. there are no matches in bins greater than upper_index. */ upper->inclusive = !upper->inclusive; upper->lower = true; upper_index = rbound_bsearch(typcache, upper, hist_lower, hist_nvalues, false); /* * Calculate upper_bin_width, ie. the fraction of the (upper_index, * upper_index + 1) bin which is greater than upper bound of query range * using linear interpolation of subdiff function. */ if (upper_index >= 0 && upper_index < hist_nvalues - 1) upper_bin_width = get_position(typcache, upper, &hist_lower[upper_index], &hist_lower[upper_index + 1]); else upper_bin_width = 0.0; /* * In the loop, dist and prev_dist are the distance of the "current" bin's * lower and upper bounds from the constant upper bound. * * bin_width represents the width of the current bin. Normally it is 1.0, * meaning a full width bin, but can be less in the corner cases: start * and end of the loop. We start with bin_width = upper_bin_width, because * we begin at the bin containing the upper bound. */ prev_dist = 0.0; bin_width = upper_bin_width; sum_frac = 0.0; for (i = upper_index; i >= 0; i--) { double dist; double length_hist_frac; bool final_bin = false; /* * dist -- distance from upper bound of query range to lower bound of * the current bin in the lower bound histogram. Or to the lower bound * of the constant range, if this is the final bin, containing the * constant lower bound. */ if (range_cmp_bounds(typcache, &hist_lower[i], lower) < 0) { dist = get_distance(typcache, lower, upper); /* * Subtract from bin_width the portion of this bin that we want to * ignore. */ bin_width -= get_position(typcache, lower, &hist_lower[i], &hist_lower[i + 1]); if (bin_width < 0.0) bin_width = 0.0; final_bin = true; } else dist = get_distance(typcache, &hist_lower[i], upper); /* * Estimate the fraction of tuples in this bin that are narrow enough * to not exceed the distance to the upper bound of the query range. */ length_hist_frac = calc_length_hist_frac(length_hist_values, length_hist_nvalues, prev_dist, dist, true); /* * Add the fraction of tuples in this bin, with a suitable length, to * the total. */ sum_frac += length_hist_frac * bin_width / (double) (hist_nvalues - 1); if (final_bin) break; bin_width = 1.0; prev_dist = dist; } return sum_frac; }
/** * @brief Returns the distance between two points. * @param xy1 coordinates of the first point * @param xy2 coordinates of the second point * @return the distance in pixels */ double Geometry::get_distance(const Rectangle& xy1, const Rectangle& xy2) { return get_distance(xy1.get_x(), xy1.get_y(), xy2.get_x(), xy2.get_y()); }
/* a special glide slope calculation for the landing approach During the land approach use a linear glide slope to a point projected through the landing point. We don't use the landing point itself as that leads to discontinuities close to the landing point, which can lead to erratic pitch control */ void Plane::setup_landing_glide_slope(void) { float total_distance = get_distance(prev_WP_loc, next_WP_loc); // If someone mistakenly puts all 0's in their LAND command then total_distance // will be calculated as 0 and cause a divide by 0 error below. Lets avoid that. if (total_distance < 1) { total_distance = 1; } // height we need to sink for this WP float sink_height = (prev_WP_loc.alt - next_WP_loc.alt)*0.01f; // current ground speed float groundspeed = ahrs.groundspeed(); if (groundspeed < 0.5f) { groundspeed = 0.5f; } // calculate time to lose the needed altitude float sink_time = total_distance / groundspeed; if (sink_time < 0.5f) { sink_time = 0.5f; } // find the sink rate needed for the target location float sink_rate = sink_height / sink_time; // the height we aim for is the one to give us the right flare point float aim_height = aparm.land_flare_sec * sink_rate; if (aim_height <= 0) { aim_height = g.land_flare_alt; } // don't allow the aim height to be too far above LAND_FLARE_ALT if (g.land_flare_alt > 0 && aim_height > g.land_flare_alt*2) { aim_height = g.land_flare_alt*2; } // calculate slope to landing point bool is_first_calc = is_zero(auto_state.land_slope); auto_state.land_slope = (sink_height - aim_height) / total_distance; if (is_first_calc) { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(auto_state.land_slope))); } // time before landing that we will flare float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate(); // distance to flare is based on ground speed, adjusted as we // get closer. This takes into account the wind float flare_distance = groundspeed * flare_time; // don't allow the flare before half way along the final leg if (flare_distance > total_distance/2) { flare_distance = total_distance/2; } // project a point 500 meters past the landing point, passing // through the landing point const float land_projection = 500; int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); // now calculate our aim point, which is before the landing // point and above it Location loc = next_WP_loc; location_update(loc, land_bearing_cd*0.01f, -flare_distance); loc.alt += aim_height*100; // calculate point along that slope 500m ahead location_update(loc, land_bearing_cd*0.01f, land_projection); loc.alt -= auto_state.land_slope * land_projection * 100; // setup the offset_cm for set_target_altitude_proportion() target_altitude.offset_cm = loc.alt - prev_WP_loc.alt; // calculate the proportion we are to the target float land_proportion = location_path_proportion(current_loc, prev_WP_loc, loc); // now setup the glide slope for landing set_target_altitude_proportion(loc, 1.0f - land_proportion); // stay within the range of the start and end locations in altitude constrain_target_altitude_location(loc, prev_WP_loc); }
/* update navigation for landing. Called when on landing approach or final flare */ bool Plane::verify_land() { // we don't 'verify' landing in the sense that it never completes, // so we don't verify command completion. Instead we use this to // adjust final landing parameters // when aborting a landing, mimic the verify_takeoff with steering hold. Once // the altitude has been reached, restart the landing sequence if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) { throttle_suppressed = false; auto_state.land_complete = false; auto_state.land_pre_flare = false; nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc)); // see if we have reached abort altitude if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) { next_WP_loc = current_loc; mission.stop(); bool success = restart_landing_sequence(); mission.resume(); if (!success) { // on a restart failure lets RTL or else the plane may fly away with nowhere to go! set_mode(RTL, MODE_REASON_MISSION_END); } // make sure to return false so it leaves the mission index alone } return false; } float height = height_above_target(); // use rangefinder to correct if possible height -= rangefinder_correction(); /* Set land_complete (which starts the flare) under 3 conditions: 1) we are within LAND_FLARE_ALT meters of the landing altitude 2) we are within LAND_FLARE_SEC of the landing point vertically by the calculated sink rate (if LAND_FLARE_SEC != 0) 3) we have gone past the landing point and don't have rangefinder data (to prevent us keeping throttle on after landing if we've had positive baro drift) */ #if RANGEFINDER_ENABLED == ENABLED bool rangefinder_in_range = rangefinder_state.in_range; #else bool rangefinder_in_range = false; #endif // flare check: // 1) below flare alt/sec requires approach stage check because if sec/alt are set too // large, and we're on a hard turn to line up for approach, we'll prematurely flare by // skipping approach phase and the extreme roll limits will make it hard to line up with runway // 2) passed land point and don't have an accurate AGL // 3) probably crashed (ensures motor gets turned off) bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH || flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE); bool below_flare_alt = (height <= g.land_flare_alt); bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec); bool probably_crashed = (g.crash_detection_enable && fabsf(auto_state.sink_rate) < 0.2f && !is_flying()); if ((on_approach_stage && below_flare_alt) || (on_approach_stage && below_flare_sec && (auto_state.wp_proportion > 0.5)) || (!rangefinder_in_range && auto_state.wp_proportion >= 1) || probably_crashed) { if (!auto_state.land_complete) { auto_state.post_landing_stats = true; if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) { gcs_send_text_fmt(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)gps.ground_speed()); } else { gcs_send_text_fmt(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f", (double)height, (double)auto_state.sink_rate, (double)gps.ground_speed(), (double)get_distance(current_loc, next_WP_loc)); } auto_state.land_complete = true; update_flight_stage(); } if (gps.ground_speed() < 3) { // reload any airspeed or groundspeed parameters that may have // been set for landing. We don't do this till ground // speed drops below 3.0 m/s as otherwise we will change // target speeds too early. g.airspeed_cruise_cm.load(); g.min_gndspeed_cm.load(); aparm.throttle_cruise.load(); } } else if (!auto_state.land_complete && !auto_state.land_pre_flare && aparm.land_pre_flare_airspeed > 0) { bool reached_pre_flare_alt = g.land_pre_flare_alt > 0 && (height <= g.land_pre_flare_alt); bool reached_pre_flare_sec = g.land_pre_flare_sec > 0 && (height <= auto_state.sink_rate * g.land_pre_flare_sec); if (reached_pre_flare_alt || reached_pre_flare_sec) { auto_state.land_pre_flare = true; update_flight_stage(); } } /* when landing we keep the L1 navigation waypoint 200m ahead. This prevents sudden turns if we overshoot the landing point */ struct Location land_WP_loc = next_WP_loc; int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); location_update(land_WP_loc, land_bearing_cd*0.01f, get_distance(prev_WP_loc, current_loc) + 200); nav_controller->update_waypoint(prev_WP_loc, land_WP_loc); // once landed and stationary, post some statistics // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm if (auto_state.post_landing_stats && !arming.is_armed()) { auto_state.post_landing_stats = false; gcs_send_text_fmt(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc)); } // check if we should auto-disarm after a confirmed landing disarm_if_autoland_complete(); /* we return false as a landing mission item never completes we stay on this waypoint unless the GCS commands us to change mission item, reset the mission, command a go-around or finish a land_abort procedure. */ return false; }
void LocationFilterDelegate::paint(QPainter *painter, const QStyleOptionViewItem &option, const QModelIndex &origIdx) const { QFont fontBigger = qApp->font(); QFont fontSmaller = qApp->font(); QFontMetrics fmBigger(fontBigger); QStyleOptionViewItemV4 opt = option; const QAbstractProxyModel *proxyModel = dynamic_cast<const QAbstractProxyModel*>(origIdx.model()); QModelIndex index = proxyModel->mapToSource(origIdx); QStyledItemDelegate::initStyleOption(&opt, index); QString diveSiteName = index.data().toString(); QString bottomText; QIcon icon = index.data(Qt::DecorationRole).value<QIcon>(); struct dive_site *ds = get_dive_site_by_uuid( index.model()->data(index.model()->index(index.row(),0)).toInt() ); //Special case: do not show name, but instead, show if (index.row() < 2) { diveSiteName = index.data().toString(); bottomText = index.data(Qt::ToolTipRole).toString(); goto print_part; } if (!ds) return; for (int i = 0; i < 3; i++) { if (prefs.geocoding.category[i] == TC_NONE) continue; int idx = taxonomy_index_for_category(&ds->taxonomy, prefs.geocoding.category[i]); if (idx == -1) continue; if(!bottomText.isEmpty()) bottomText += " / "; bottomText += QString(ds->taxonomy.category[idx].value); } if (bottomText.isEmpty()) { const char *gpsCoords = printGPSCoords(ds->latitude.udeg, ds->longitude.udeg); bottomText = QString(gpsCoords); free( (void*) gpsCoords); } if (dive_site_has_gps_location(ds) && dive_site_has_gps_location(&displayed_dive_site)) { // so we are showing a completion and both the current dive site and the completion // have a GPS fix... so let's show the distance if (ds->latitude.udeg == displayed_dive_site.latitude.udeg && ds->longitude.udeg == displayed_dive_site.longitude.udeg) { bottomText += tr(" (same GPS fix)"); } else { int distanceMeters = get_distance(ds->latitude, ds->longitude, displayed_dive_site.latitude, displayed_dive_site.longitude); QString distance = distance_string(distanceMeters); int nr = nr_of_dives_at_dive_site(ds->uuid, false); bottomText += tr(" (~%1 away").arg(distance); bottomText += tr(", %1 dive(s) here)").arg(nr); } } if (bottomText.isEmpty()) { if (dive_site_has_gps_location(&displayed_dive_site)) bottomText = tr("(no existing GPS data, add GPS fix from this dive)"); else bottomText = tr("(no GPS data)"); } bottomText = tr("Pick site: ") + bottomText; print_part: fontBigger.setPointSize(fontBigger.pointSize() + 1); fontBigger.setBold(true); QPen textPen = QPen(option.state & QStyle::State_Selected ? option.palette.highlightedText().color() : option.palette.text().color(), 1); initStyleOption(&opt, index); opt.text = QString(); opt.icon = QIcon(); painter->setClipRect(option.rect); painter->save(); if (option.state & QStyle::State_Selected) { painter->setPen(QPen(opt.palette.highlight().color().darker())); painter->setBrush(opt.palette.highlight()); const qreal pad = 1.0; const qreal pad2 = pad * 2.0; const qreal rounding = 5.0; painter->drawRoundedRect(option.rect.x() + pad, option.rect.y() + pad, option.rect.width() - pad2, option.rect.height() - pad2, rounding, rounding); } painter->setPen(textPen); painter->setFont(fontBigger); const qreal textPad = 5.0; painter->drawText(option.rect.x() + textPad, option.rect.y() + fmBigger.boundingRect("YH").height(), diveSiteName); double pointSize = fontSmaller.pointSizeF(); fontSmaller.setPointSizeF(0.9 * pointSize); painter->setFont(fontSmaller); painter->drawText(option.rect.x() + textPad, option.rect.y() + fmBigger.boundingRect("YH").height() * 2, bottomText); painter->restore(); if (!icon.isNull()) { painter->save(); painter->drawPixmap( option.rect.x() + option.rect.width() - 24, option.rect.y() + option.rect.height() - 24, icon.pixmap(20,20)); painter->restore(); } }
static int check_collision_with_mouse( INSTANCE * proc1, int colltype ) { REGION bbox1, bbox2 ; int x, y, mx, my ; static GRAPH * bmp = NULL; switch ( colltype ) { case COLLISION_BOX: case COLLISION_CIRCLE: if ( !get_bbox( &bbox2, proc1 ) ) return 0 ; case COLLISION_NORMAL: break; default: return 0; } mx = GLOINT32( mod_grproc, MOUSEX ) ; my = GLOINT32( mod_grproc, MOUSEY ) ; /* Checks the process's bounding box to optimize checking (only for screen-type objects) */ if ( LOCDWORD( mod_grproc, proc1, CTYPE ) == C_SCREEN ) { switch ( colltype ) { case COLLISION_NORMAL: if ( !get_bbox( &bbox2, proc1 ) ) return 0 ; if ( bbox2.x > mx || bbox2.x2 < mx || bbox2.y > my || bbox2.y2 < my ) return 0 ; break; case COLLISION_BOX: if ( bbox2.x <= mx && bbox2.x2 >= mx && bbox2.y <= my && bbox2.y2 >= my ) return 1; return 0; break; case COLLISION_CIRCLE: { int cx1, cy1, dx1, dy1; cx1 = bbox2.x + ( dx1 = ( bbox2.x2 - bbox2.x + 1 ) ) / 2 ; cy1 = bbox2.y + ( dy1 = ( bbox2.y2 - bbox2.y + 1 ) ) / 2 ; if ( get_distance( cx1, cy1, 0, mx, my, 0 ) < ( dx1 + dy1 ) / 4 ) return 1; return 0; break; } } } /* Creates a temporary bitmap (only once) */ if ( colltype == COLLISION_NORMAL ) { /* maybe must force this to 32 bits */ if ( bmp && bmp->format->depth != sys_pixel_format->depth ) { bitmap_destroy( bmp ); bmp = NULL; } if ( !bmp ) bmp = bitmap_new( 0, 2, 1, sys_pixel_format->depth ) ; if ( !bmp ) return 0 ; memset( bmp->data, 0, bmp->pitch * bmp->height ) ; } /* Retrieves process information */ bbox1.x = 0 ; bbox1.x2 = 1 ; bbox1.y = 0 ; bbox1.y2 = 0 ; x = LOCINT32( mod_grproc, proc1, COORDX ) ; y = LOCINT32( mod_grproc, proc1, COORDY ) ; RESOLXY( mod_grproc, proc1, x, y ); /* Scroll-type process: check for each region */ if ( LOCDWORD( mod_grproc, proc1, CTYPE ) == C_SCROLL ) { SCROLL_EXTRA_DATA * data; scrolldata * scroll; int i; if ( GLOEXISTS( mod_grproc, SCROLLS ) ) { int cnumber = LOCDWORD( mod_grproc, proc1, CNUMBER ); if ( !cnumber ) cnumber = 0xffffffff ; for ( i = 0 ; i < 10 ; i++ ) { data = &(( SCROLL_EXTRA_DATA * ) & GLODWORD( mod_grproc, SCROLLS ) )[i] ; scroll = ( scrolldata * ) data->reserved[0]; if ( scroll && scroll->active && ( cnumber & ( 1 << i ) ) ) { REGION * r = scroll->region; switch ( colltype ) { case COLLISION_NORMAL: if ( r->x > mx || r->x2 < mx || r->y > my || r->y2 < my ) continue; draw_at( bmp, x + r->x - mx - scroll->posx0, y + r->y - my - scroll->posy0, &bbox1, proc1 ); switch ( sys_pixel_format->depth ) { case 8: if ( *( uint8_t * )bmp->data ) return 1; break; case 16: if ( *( uint16_t * )bmp->data ) return 1; break; case 32: if ( *( uint32_t * )bmp->data ) return 1; break; } break; case COLLISION_BOX: if ( bbox2.x <= scroll->posx0 + r->x + mx && bbox2.x2 >= scroll->posx0 + r->x + mx && bbox2.y <= scroll->posy0 + r->y + my && bbox2.y2 >= scroll->posy0 + r->y + my ) return 1; break; case COLLISION_CIRCLE: { int cx1, cy1, dx1, dy1; cx1 = bbox2.x + ( dx1 = ( bbox2.x2 - bbox2.x + 1 ) ) / 2 ; cy1 = bbox2.y + ( dy1 = ( bbox2.y2 - bbox2.y + 1 ) ) / 2 ; if ( get_distance( cx1, cy1, 0, r->x + mx + scroll->posx0, r->y + my + scroll->posy0, 0 ) < ( dx1 + dy1 ) / 4 ) return 1; break; } } } } } return 0; } switch ( colltype ) { case COLLISION_NORMAL: /* Collision check (blits into temporary space and checks the resulting pixel) */ draw_at( bmp, x - mx, y - my, &bbox1, proc1 ) ; switch ( sys_pixel_format->depth ) { case 8: if ( *( uint8_t * )bmp->data ) return 1; break; case 16: if ( *( uint16_t * )bmp->data ) return 1; break; case 32: if ( *( uint32_t * )bmp->data ) return 1; break; } break; case COLLISION_BOX: if ( bbox2.x <= mx && bbox2.x2 >= mx && bbox2.y <= my && bbox2.y2 >= my ) return 1; break; case COLLISION_CIRCLE: { int cx1, cy1, dx1, dy1; cx1 = bbox2.x + ( dx1 = ( bbox2.x2 - bbox2.x + 1 ) ) / 2 ; cy1 = bbox2.y + ( dy1 = ( bbox2.y2 - bbox2.y + 1 ) ) / 2 ; if ( get_distance( cx1, cy1, 0, mx, my, 0 ) < ( dx1 + dy1 ) / 4 ) return 1; break; } } return 0 ; }
void BoxSweepClosePairsFinder::do_show(std::ostream &out) const { out << "distance " << get_distance() << std::endl; }
void ModeGuided::update() { switch (_guided_mode) { case Guided_WP: { _distance_to_destination = get_distance(rover.current_loc, _destination); const bool near_wp = _distance_to_destination <= rover.g.waypoint_radius; // check if we've reached the destination if (!_reached_destination && (near_wp || location_passed_point(rover.current_loc, _origin, _destination))) { _reached_destination = true; rover.gcs().send_mission_item_reached_message(0); } // determine if we should keep navigating if (!_reached_destination || (rover.is_boat() && !near_wp)) { // drive towards destination calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed); calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { stop_vehicle(); } break; } case Guided_HeadingAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers calc_steering_to_heading(_desired_yaw_cd, _desired_speed < 0); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true); } else { stop_vehicle(); } break; } case Guided_TurnRateAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.limit.steer_left, g2.motors.limit.steer_right, rover.G_Dt); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true, true); } else { stop_vehicle(); } break; } default: gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); break; } }
/* update navigation for landing. Called when on landing approach or final flare */ bool Plane::verify_land() { // we don't 'verify' landing in the sense that it never completes, // so we don't verify command completion. Instead we use this to // adjust final landing parameters // If a go around has been commanded, we are done landing. This will send // the mission to the next mission item, which presumably is a mission // segment with operations to perform when a landing is called off. // If there are no commands after the land waypoint mission item then // the plane will proceed to loiter about its home point. if (auto_state.commanded_go_around) { return true; } float height = height_above_target(); // use rangefinder to correct if possible height -= rangefinder_correction(); /* Set land_complete (which starts the flare) under 3 conditions: 1) we are within LAND_FLARE_ALT meters of the landing altitude 2) we are within LAND_FLARE_SEC of the landing point vertically by the calculated sink rate (if LAND_FLARE_SEC != 0) 3) we have gone past the landing point and don't have rangefinder data (to prevent us keeping throttle on after landing if we've had positive baro drift) */ #if RANGEFINDER_ENABLED == ENABLED bool rangefinder_in_range = rangefinder_state.in_range; #else bool rangefinder_in_range = false; #endif if (height <= g.land_flare_alt || (aparm.land_flare_sec > 0 && height <= auto_state.sink_rate * aparm.land_flare_sec) || (!rangefinder_in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) || (fabsf(auto_state.sink_rate) < 0.2f && !is_flying())) { if (!auto_state.land_complete) { auto_state.post_landing_stats = true; if (!is_flying() && (millis()-auto_state.last_flying_ms) > 3000) { gcs_send_text_fmt(PSTR("Flare crash detected: speed=%.1f"), (double)gps.ground_speed()); } else { gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), (double)height, (double)auto_state.sink_rate, (double)gps.ground_speed()); } } auto_state.land_complete = true; if (gps.ground_speed() < 3) { // reload any airspeed or groundspeed parameters that may have // been set for landing. We don't do this till ground // speed drops below 3.0 m/s as otherwise we will change // target speeds too early. g.airspeed_cruise_cm.load(); g.min_gndspeed_cm.load(); aparm.throttle_cruise.load(); } } /* when landing we keep the L1 navigation waypoint 200m ahead. This prevents sudden turns if we overshoot the landing point */ struct Location land_WP_loc = next_WP_loc; int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc); location_update(land_WP_loc, land_bearing_cd*0.01f, get_distance(prev_WP_loc, current_loc) + 200); nav_controller->update_waypoint(prev_WP_loc, land_WP_loc); // once landed and stationary, post some statistics // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm if (auto_state.post_landing_stats && !arming.is_armed()) { auto_state.post_landing_stats = false; gcs_send_text_fmt(PSTR("Distance from LAND point=%.2fm"), (double)get_distance(current_loc, next_WP_loc)); } // check if we should auto-disarm after a confirmed landing disarm_if_autoland_complete(); /* we return false as a landing mission item never completes we stay on this waypoint unless the GCS commands us to change mission item or reset the mission, or a go-around is commanded */ return false; }
static inline struct triangle *triangle_new (struct point pts[], int v1, int v2, int v3, double epsilon) { struct triangle *t = calloc(1, sizeof(struct triangle)); int v[3], vv[3]; double r1, r2, r3; vv[0] = v1; vv[1] = v2; vv[2] = v3; /* permute until ordered righ :-) */ for (v[0] = 0; v[0] < 3; v[0]++) { for (v[1] = 0; v[1] < 3; v[1]++) { if (v[1] == v[0]) continue; for (v[2] = 0; v[2] < 3; v[2]++) { if (v[2] == v[1] || v[2] == v[0]) continue; /* NB: r2 latura mica, r1 latura medie, r3 latura mare */ r2 = get_distance(pts, vv[v[0]], vv[v[1]]); r1 = get_distance(pts, vv[v[1]], vv[v[2]]); r3 = get_distance(pts, vv[v[2]], vv[v[0]]); if (r2 < r1 && r1 < r3) goto found; } } } found: g_assert(r2 < r1 && r1 < r3 && r2 < r3); t->v[0] = vv[v[0]]; t->v[1] = vv[v[1]]; t->v[2] = vv[v[2]]; t->R = r3 / r2; /* from the law of cosines */ t->C = ( (pts[t->v[2]].x - pts[t->v[0]].x) * (pts[t->v[1]].x - pts[t->v[0]].x) + (pts[t->v[2]].y - pts[t->v[0]].y) * (pts[t->v[1]].y - pts[t->v[0]].y) ) / (r2 * r3); double cterm = 1.0 / sqr(r3) - t->C / (r3 * r2) + 1.0 / sqr(r2); t->Rtolsq = 2 * sqr(t->R) * sqr(epsilon) * cterm; double sinsq = 1 - sqr(t->C); t->Ctolsq = 2 * sinsq * sqr(epsilon) * cterm + 3 * sqr(t->C) * sqr(sqr(epsilon)) * sqr(cterm); /* if det. is positive, we're clockwise. */ double det = ((pts[t->v[1]].x - pts[t->v[0]].x) * (pts[t->v[2]].y - pts[t->v[0]].y) - (pts[t->v[1]].y - pts[t->v[0]].y) * (pts[t->v[2]].x - pts[t->v[1]].x)); t->clockwise = (det > 0); t->logP = log(r1 + r2 + r3); return t; }
string optical_flow::get_final_direction(Mat m1, Mat m2, Rect old_hand_boundary, Rect new_hand_boundary) { left_count = 0; up_count = 0; down_count = 0; right_count = 0; non_count = 0; TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03); cvtColor(m1, old_gray_img, COLOR_BGR2GRAY);//convert to gray cvtColor(m2, new_gray_img, COLOR_BGR2GRAY); // extract features goodFeaturesToTrack(old_gray_img, old_frame_points, max_count, .01, min_distance, Mat(), block_size, 0, .04); cornerSubPix(old_gray_img, old_frame_points, Size(10, 10), Size(-1, -1), termcrit); // track features in next frame vector<uchar> status; vector<float> err; calcOpticalFlowPyrLK(old_gray_img, new_gray_img, old_frame_points, new_frame_points, status, err, Size(10, 10), 3, termcrit, 0, 0.001); for (unsigned int i = 0; i < new_frame_points.size(); i++) { Point2f old_point = old_frame_points.at(i); Point2f new_point = new_frame_points.at(i); if (!(old_hand_boundary.contains(old_point) && new_hand_boundary.contains(new_point))) continue; float dist = get_distance(old_point, new_point); // cout<<dist<<endl; if (dist < threshold) { directions.push_back(non_direction); non_count++; } else { float dx = new_point.x - old_point.x; float dy = new_point.y - old_point.y; if (abs(dx) > abs(dy)) {//horizontal if (abs(dx) <= thresh_in_on_dir) non_count++; else { if (dx < 0) { directions.push_back(left_direction); left_count++; } else { directions.push_back(right_direction); right_count++; } } } else { //vertical if (abs(dy) <= thresh_in_on_dir) non_count++; else { if (dy < 0) { directions.push_back(up_direction); up_count++; } else { directions.push_back(down_direction); down_count++; } } } } } int dirs_counts[] = { up_count, down_count, left_count, right_count, non_count }; int max_elem = *max_element(dirs_counts, dirs_counts + 5); // cout<<up_count << " "<<down_count<<" "<<left_count<<" " <<right_count<<" "<<non_count<<endl; final_direction = ""; if (up_count == max_elem) final_direction = "up"; else if (down_count == max_elem) final_direction = "down"; else if (left_count == max_elem) final_direction = "left"; else if (right_count == max_elem) final_direction = "right"; else final_direction = "none"; return final_direction; }
void ModeGuided::update() { switch (_guided_mode) { case Guided_WP: { if (!_reached_destination || rover.is_boat()) { // check if we've reached the destination _distance_to_destination = get_distance(rover.current_loc, _destination); if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) { _reached_destination = true; rover.gcs().send_mission_item_reached_message(0); } // drive towards destination calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); } else { stop_vehicle(); } break; } case Guided_HeadingAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers const float yaw_error = wrap_PI(radians((_desired_yaw_cd - ahrs.yaw_sensor) * 0.01f)); const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true); } else { stop_vehicle(); g2.motors.set_steering(0.0f); } break; } case Guided_TurnRateAndSpeed: { // stop vehicle if target not updated within 3 seconds if (have_attitude_target && (millis() - _des_att_time_ms) > 3000) { gcs().send_text(MAV_SEVERITY_WARNING, "target not received last 3secs, stopping"); have_attitude_target = false; } if (have_attitude_target) { // run steering and throttle controllers float steering_out = attitude_control.get_steering_out_rate(radians(_desired_yaw_rate_cds / 100.0f), g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, _desired_speed < 0); g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true); } else { stop_vehicle(); g2.motors.set_steering(0.0f); } break; } default: gcs().send_text(MAV_SEVERITY_WARNING, "Unknown GUIDED mode"); break; } }
/* * Calculate selectivity of "var @> const" operator, ie. estimate the fraction * of ranges that contain the constant lower and upper bounds. This uses * the histograms of range lower bounds and range lengths, on the assumption * that the range lengths are independent of the lower bounds. * * Note, this is "var @> const", ie. estimate the fraction of ranges that * contain the constant lower and upper bounds. */ static double calc_hist_selectivity_contains(TypeCacheEntry *typcache, RangeBound *lower, RangeBound *upper, RangeBound *hist_lower, int hist_nvalues, Datum *length_hist_values, int length_hist_nvalues) { int i, lower_index; double bin_width, lower_bin_width; double sum_frac; float8 prev_dist; /* Find the bin containing the lower bound of query range. */ lower_index = rbound_bsearch(typcache, lower, hist_lower, hist_nvalues, true); /* * Calculate lower_bin_width, ie. the fraction of the of (lower_index, * lower_index + 1) bin which is greater than lower bound of query range * using linear interpolation of subdiff function. */ if (lower_index >= 0 && lower_index < hist_nvalues - 1) lower_bin_width = get_position(typcache, lower, &hist_lower[lower_index], &hist_lower[lower_index + 1]); else lower_bin_width = 0.0; /* * Loop through all the lower bound bins, smaller than the query lower * bound. In the loop, dist and prev_dist are the distance of the * "current" bin's lower and upper bounds from the constant upper bound. * We begin from query lower bound, and walk backwards, so the first bin's * upper bound is the query lower bound, and its distance to the query * upper bound is the length of the query range. * * bin_width represents the width of the current bin. Normally it is 1.0, * meaning a full width bin, except for the first bin, which is only * counted up to the constant lower bound. */ prev_dist = get_distance(typcache, lower, upper); sum_frac = 0.0; bin_width = lower_bin_width; for (i = lower_index; i >= 0; i--) { float8 dist; double length_hist_frac; /* * dist -- distance from upper bound of query range to current value * of lower bound histogram or lower bound of query range (if we've * reach it). */ dist = get_distance(typcache, &hist_lower[i], upper); /* * Get average fraction of length histogram which covers intervals * longer than (or equal to) distance to upper bound of query range. */ length_hist_frac = 1.0 - calc_length_hist_frac(length_hist_values, length_hist_nvalues, prev_dist, dist, false); sum_frac += length_hist_frac * bin_width / (double) (hist_nvalues - 1); bin_width = 1.0; prev_dist = dist; } return sum_frac; }
vector3 Wall::collision_detection(vector3 pos){ //note #10 vector3 gamma = vector3(pos[0] - bbox[0], 0, pos[2] - bbox[2]); vector3 delta = vector3(pos[0] - bbox[3], 0, pos[2] - bbox[5]); GLfloat sindelta = (dir[0]*delta[2]-dir[2]*delta[0])/(delta.length()); GLfloat singamma = (dir[0]*gamma[2]-dir[2]*gamma[0])/(gamma.length()); if(signbit(sindelta)!=signbit(singamma) && vertices[4]<=pos[1] && pos[1]<vertices[7] && get_distance(pos)<1.0) return dir; /*vector3 gamma = vector3(pos[0] - bbox[0], 0, pos[2] - bbox[2]); vector3 delta = vector3(pos[0] - bbox[3], 0, pos[2] - bbox[5]); vector3 alpha = vector3(pos[0] - bbox[0+12], pos[1] - bbox[1+12], pos[2] - bbox[2+12]); vector3 eps = vector3(pos[0] - bbox[9], pos[1] - bbox[10], pos[2] - bbox[11]); vector3 zeta = alpha; GLfloat dl = delta.length(); GLfloat cosalpha = (dir[0]*alpha[0]+dir[2]*alpha[2])/(alpha.length()); GLfloat cosdelta = (dir[0]*delta[0]+dir[2]*delta[2])/(dl); GLfloat sindelta = (dir[0]*delta[2]-dir[2]*delta[0])/(dl); GLfloat singamma = (dir[0]*gamma[2]-dir[2]*gamma[0])/(gamma.length()); //perpendicular of collider vector3 perp = vector3(-dir[2],dir[1],dir[0]); perp = cross(perp,dir); GLfloat sineps = (perp[0]*eps[0]+perp[1]*eps[1])/(eps.length()); GLfloat sinzeta = (perp[0]*zeta[0]+perp[1]*zeta[1])/(zeta.length()); if(signbit(sindelta)!=signbit(singamma) && (signbit(sineps)!=signbit(sinzeta)) && signbit(cosalpha)!=signbit(cosdelta)) return dir;*/ return vector3(0.0,0.0,0.0); }
gboolean dijkstra_to (GList *nodes, Node *source, Node *target, gint width, gint height, gint *distances, Node **previous) { gint nr; GList *unvisited_nodes, *current; for (current = g_list_first (nodes); previous != NULL && current != NULL; current = g_list_next (current)) { Node *node; node = (Node *) current->data; previous[node->j * width + node->i] = NULL; } distances[source->j * width + source->i] = 0; unvisited_nodes = g_list_copy (nodes); nr = 0; while (unvisited_nodes != NULL) { Node *node; GList *current_neighbor, *shorter_dist_node, *cur_node; shorter_dist_node = g_list_first (unvisited_nodes); cur_node = g_list_next (shorter_dist_node); while (cur_node != NULL) { Node *value, *shorter_dist; value = (Node *) cur_node->data; shorter_dist = (Node *) shorter_dist_node->data; if (distances[shorter_dist->j * width + shorter_dist->i] == -1 || (distances[value->j * width + value->i] != -1 && distances[value->j * width + value->i] < distances[shorter_dist->j * width + shorter_dist->i])) { shorter_dist_node = cur_node; } cur_node = g_list_next (cur_node); } node = (Node *) shorter_dist_node->data; if (distances[node->j * width + node->i] == -1) { break; } current_neighbor = g_list_first (node->neighbors); while (current_neighbor) { gint dist; Node *neighbor; neighbor = (Node *) current_neighbor->data; dist = get_distance (node, neighbor) + distances[node->j * width + node->i]; if (distances[neighbor->j * width + neighbor->i] == -1 || dist < distances[neighbor->j * width + neighbor->i]) { distances[neighbor->j * width + neighbor->i] = dist; if (previous != NULL) { previous[neighbor->j * width + neighbor->i] = node; } nr++; } if (target != NULL && neighbor == target) { g_list_free (unvisited_nodes); return TRUE; } current_neighbor = g_list_next (current_neighbor); } unvisited_nodes = g_list_delete_link (unvisited_nodes, shorter_dist_node); } g_list_free (unvisited_nodes); return FALSE; }
int main (int argc, char **argv) { struct arguments arguments; /* Parse our arguments; every option seen by parse_opt will be reflected in arguments. */ argp_parse (&argp, argc, argv, 0, 0, &arguments); // number of nearest neighbors int k; k = 1; //default is 1 if (sscanf (arguments.args[0], "%i", &k)!=1) {} //omp vars int num_threads; num_threads = 4; if (sscanf(arguments.args[1], "%i", &num_threads)!=1) {} //verbose? int verbose; verbose = arguments.verbose; if (verbose>0 && verbose<130){ verbose = 1; } else{ verbose = 0; } //define a bunch of counters! int i, j, m, n, ii, jj, kk; //number of examples to read in int total_examples = 10000; // int total_examples = 19; //max words per question int num_words = 300; //max word length int max_word_len = 20; //max vocab count // int max_vocab = 200000; //data read in poorly int bad_iter = 0; //Used to split into training and testing data (will train on example_num%train) int train = 10; //Debug int debug = 0; printf("k, Verbose, num_threads = %i, %i, %i\n", k, verbose, num_threads); //Allocate space for data being read in with fgets char *csv_line = malloc(sizeof(char)*1500); //store all data //array of structs //struct.question->array of char* //struct.cat->char* //struct.example_num->int struct data *all_data; all_data = malloc(sizeof(struct data)*total_examples); for (ii=0; ii<total_examples; ii++){ all_data[ii].question = malloc(sizeof(char*)*num_words); for (jj=0; jj<num_words; jj++){ // all_data[ii].question[jj] = malloc(sizeof(char)*max_word_len); all_data[ii].question[jj] = calloc(max_word_len, sizeof(char)); } all_data[ii].cat = malloc(sizeof(char)*max_word_len); } //store numeric version of data for algorithms struct numeric_data *num_data; num_data = malloc(sizeof(struct numeric_data)*total_examples); for (ii=0; ii<total_examples; ii++){ num_data[ii].array_of_features = malloc(sizeof(struct feature_count)*num_words); for (jj=0; jj<num_words; jj++){ num_data[ii].array_of_features[jj].feature_num = 0; num_data[ii].array_of_features[jj].count = 0; } } //store struct which keep track of the k nearest neighbors // struct distance_results results; // results.example_num = 0; // results.distances = calloc(k, sizeof(double)); // results.cat = calloc(k, sizeof(int)); // results.example_nums = calloc(k, sizeof(int)); // //struct used to calculate the mode of the k nearest neighbors // struct mode mod; // mod.count = calloc(k, sizeof(int)); // mod.cat = calloc(k, sizeof(int)); // //store vocabulary list (char** points to array of char* of length 20) // char **word_list; // word_list = malloc(sizeof(char*)*max_vocab); //assumes max_vocab total vocab // for (ii=0; ii<max_vocab; ii++){ // // word_list[ii] = malloc(sizeof(char)*max_word_len); //assumes max word length of 20 // word_list[ii] = calloc(max_word_len, sizeof(char)); //assumes max word length of 20 // } //alternate vocab store tree feature_tree *vocab; vocab = NULL; //store category list char **cat_list; cat_list = malloc(sizeof(char*)*40); //assumes 20 max categories for (ii=0; ii<40; ii++){ cat_list[ii] = malloc(sizeof(char)*max_word_len); strncpy(cat_list[ii], "\0", 1); } //Read in csv file FILE *f = fopen("train_pruned2.csv", "r"); if (f == NULL){ printf("Failed to open file \n"); return -1; } //parse question into individual words, create vocabulary list int vocab_count = 0; int category_count = 1; for (i=0; i<total_examples; i++){ // printf("Iteration = %i\n", i); //line in csv to buffer if (fgets(csv_line, 1500, f) == NULL){ printf("Fgets error!\n"); exit(0); } //csv line to 3 individual parts if (i>0) { char *tok; char *tok_copy; //problem with tok getting overwritten in parse_question // char **parsed_question = malloc(sizeof(char*)*num_words); // printf("CSV_LINE = %s\n", csv_line); tok = strtok(csv_line, "|"); if (tok == NULL){ // all_data[i-bad_iter-1].example_num = -1; bad_iter++; // i--; continue; } sscanf(tok, "%i", &all_data[i-bad_iter-1].example_num); tok = strtok(NULL, "|"); if (tok == NULL){ // all_data[i-bad_iter-1].example_num = -1; bad_iter++; // i--; continue; } tok_copy = (char *)tok; tok = strtok(NULL, "|"); if (tok == NULL){ // all_data[i-bad_iter-1].example_num = -1; bad_iter++; // i--; continue; } strncpy(all_data[i-bad_iter-1].cat, tok, 19); all_data[i-bad_iter-1].cat[max_word_len-1] = 0; char *tok2; tok2 = strtok(tok_copy, " \t"); j = 0; if ((tok2 != NULL) && (strlen(tok2)>3)){ strncpy(all_data[i-bad_iter-1].question[0], tok2, 19); all_data[i-bad_iter-1].question[0][max_word_len-1] = 0; //add to tree if not test data // if (all_data[i-bad_iter-1].example_num % train != 0){ insert_word(&vocab, all_data[i-bad_iter-1].question[0]); j += 1; // } } while (tok2 != NULL){ if (j>=num_words){ break; } tok2 = strtok(NULL, " \t"); if ((tok2 != NULL) && (strlen(tok2)>3)){ strncpy(all_data[i-bad_iter-1].question[j], tok2, 19); all_data[i-bad_iter-1].question[j][max_word_len-1] = 0; //add to tree if not test data // if (all_data[i-bad_iter-1].example_num % train != 0){ insert_word(&vocab, all_data[i-bad_iter-1].question[j]); j++; // } } } //end while // all_data[i-bad_iter-1] = instance; // print_data(&all_data[i-bad_iter-1]); ////add to vocabulary (using array, VERY slow with lots of data) // add_to_word_list(all_data[i-bad_iter-1].question, word_list, &vocab_count); //add to category list add_to_cat_list(all_data[i-bad_iter-1].cat, cat_list, &category_count); } //end if } //end for //close file fclose(f); //assign unique number to each feature //first feature is feature 1, feature 0 is for errors etc. unsigned int mm = 1; number_features(vocab, &mm); //Some of the csv rows aren't read in properly with fgets printf("Bad iterations = %i/%i\n", bad_iter, i); printf("Feature count = %i\n", count_features(vocab)); // print_inorder(vocab); // for (ii=0; ii<40; ii++){ // printf("%s", cat_list[ii]); // } ////turn data into numeric features//// for (i=0; i<total_examples; i++){ num_data[i].example_num = all_data[i].example_num; num_data[i].cat = get_cat_index(cat_list, all_data[i].cat); words_to_num(&num_data[i], &all_data[i], &vocab, num_words); // count_features2(&num_data[i]); } // num_data->array_of_features[0].feature_num = 44; // print_num_data(&num_data[0]); // print_num_data(&num_data[1]); total_examples = total_examples-bad_iter-1; int sadfjh; double av_feature_count = 0; for (ii=0; ii<total_examples; ii++){ sadfjh = count_features2(&num_data[ii]); av_feature_count += sadfjh; // printf("%i ", sadfjh); } // printf("\n av_feature_count %f\n", av_feature_count/(total_examples-bad_iter-1)); // print_num_data(&num_data[4464]); // printf("vocab->right = %s \n", vocab->feature); // print_data(&all_data[0]); // print_data(&all_data[29000]); // printf("%s, %u\n", "1829", get_feature_number(&vocab, "1829")); //find the distance between first example and rest double distance; //range each process will cover int range; // printf("%i, %i\n", range, total_examples); // printf("R, Min, Max = %i, %i, %i\n", rank, rank*range, (rank+1)*range); // struct distance_results results; // results.example_num = 0; // results.distances = calloc(k, sizeof(double)); // results.cat = calloc(k, sizeof(int)); // results.example_nums = calloc(k, sizeof(int)); // //struct used to calculate the mode of the k nearest neighbors // struct mode mod; // mod.count = calloc(k, sizeof(int)); // mod.cat = calloc(k, sizeof(int)); //correct/total/answer int c = 0; int total = 0; int answer; omp_set_dynamic(0); //Explicitly disable dynamic teams omp_set_num_threads(num_threads); //Specify thread count #pragma omp parallel \ private(kk, ii, distance, answer) \ reduction(+:c,total) \ shared(num_data) { //store struct which keep track of the k nearest neighbors struct distance_results results; results.example_num = 0; results.distances = calloc(k, sizeof(double)); results.cat = calloc(k, sizeof(int)); results.example_nums = calloc(k, sizeof(int)); //struct used to calculate the mode of the k nearest neighbors struct mode mod; mod.count = calloc(k, sizeof(int)); mod.cat = calloc(k, sizeof(int)); #pragma omp for for (kk=0; kk<total_examples; kk++){ // printf("Thread = %i, Iter = %i, c = %i, total=%i\n", omp_get_thread_num(), kk, c, total); //only test on test data if (num_data[kk].example_num%train != 0){ continue; } if (num_data[kk].cat == 0){ continue; } results.correct_answer = num_data[kk].cat; results.example_num = num_data[kk].example_num; for (ii=0; ii<k; ii++){ results.distances[ii] = 0; results.cat[ii] = 0; mod.count[ii] = 0; mod.cat[ii] = 0; } // print_num_data(&num_data[kk]); //calc distance to neighbors for (ii=0; ii<total_examples-1; ii++){ //don't calc distance to self if (kk != ii){ //Eliminate bad data (examples with few words tend to have low distances //reguardless of whether they are more similar... if (num_data[ii].total_features >= 40){ distance = get_distance(&num_data[kk], &num_data[ii], num_words); // if (distance < 2){ // continue; // } // printf("%f ", distance); if (num_data[ii].example_num > 0){ add_distance_to_results(&results, distance, k, num_data[ii].cat, num_data[ii].example_num); } } } } answer = calc_nearest_neighbor(&results, &mod, k); if (answer == results.correct_answer){ c += 1; } // printf("\n"); // for (ii=0; ii<k; ii++){ // printf("Distance, cat, example_num1, example_num2 = %2.2f, %i, %i, %i\n", // results.distances[ii], results.cat[ii], results.example_num, results.example_nums[ii]); // } // else{ // } total += 1; if (verbose>0 && debug>0){ printf("Thread = %i, Correct/Total = %i/%i Answer/Correct = %i/%i\n", omp_get_thread_num(), c, total, answer, results.correct_answer); } } //Thread results #pragma omp barrier if (omp_get_thread_num() == 0){ printf("/// Thread Results ///\n"); } #pragma omp barrier printf("Thread = %i, Correct/Total = %i/%i\n", omp_get_thread_num(), c, total); //free distance result free(results.distances); free(results.cat); //free mode struct free(mod.count); free(mod.cat); } printf("/// Final Results ///\n"); printf("Correct/Total = %i/%i\n", c, total); // printf("verbose = %i", verbose); ////free malloc calls//// //free feature tree free_feature_tree(vocab); //free numeric data for (ii=0; ii<total_examples; ii++){ free(num_data[ii].array_of_features); } free(num_data); // //free vocab list // for (ii=0; ii<max_vocab; ii++){ // free(word_list[ii]); // } // free(word_list); //free category list for (ii=0; ii<40; ii++){ free(cat_list[ii]); } free(cat_list); //free all_data list for (ii=0; ii<total_examples; ii++){ for (jj=0; jj<num_words; jj++){ free(all_data[ii].question[jj]); } free(all_data[ii].question); free(all_data[ii].cat); } free(all_data); //free var used to rean in csv free(csv_line); }
bool QuadraticClosePairsFinder::get_are_close(kernel::Model *m, kernel::ParticleIndex a, kernel::ParticleIndex b) const { return internal::get_are_close(m, access_pair_filters(), a, b, get_distance()); }
double get_distance_in(double lat1, double lng1, double lat2, double lng2) { return 0.8 * (get_distance(lat1, lng1, lat2, lng2)); }
///[PDA 인증]////////////////// int db_opw_list(OPW_HEADER *hd, OPW_LIST_REQ *req, OPW_RESULT *rd, OPW_LIST_INFO **out) { int ret = E_NO_ERR ; char sql[SQL_SIZE] = {'\0'} ; char where_oiltype[100] = {'\0'} ; char where_search_opt[100] = {'\0'} ; char where_pole[300] = {'\0'} ; char where_sort_opt[100] = {'\0'} ; int sido_flag = 0 ; int gugun_flag = 0 ; int dong_flag = 0 ; int i, n, cnt = 0 ; int valid_cnt = 0 ; int k = 0 ; MYSQL *conn = NULL ; MYSQL_RES *res_set = NULL ; MYSQL_ROW row = NULL ; OPW_LIST_INFO *data = NULL ; int fetch_count = 0 ; OPW_LIST_INFO *buf = NULL ; // ret = mydbc_init(&conn); // if(ret != DB_SUCCESS) { // printLog(HEAD, "ERR: mydbc_init[%d]\n", ret); // ret = E_NOT_CONN_DB ; // goto error_exit ; // } conn = mysql_init(NULL) ; if(mysql_real_connect(conn, Config.DB_HOST, Config.DB_USER, Config.DB_PASSWORD, Config.DB_NAME, atoi(Config.DB_PORT), NULL, 0) == NULL) { printLog(HEAD, "ERR: mysql_real_connect\n") ; ret = E_NOT_CONN_DB ; goto error_exit ; } memset(sql, 0, sizeof(sql)) ; switch(hd->oil_type) { case 'A' : //: 모든 종류 sprintf(where_oiltype, "and (g_price>100 or k_price>100 or d_price>100 or l_price>100) ") ; break ; case 'G' : //: 휘발유 // sprintf(where_oiltype, "where (jcomp=0 and g_price > 0) ") ; sprintf(where_oiltype, "and (g_price>100) ") ; break ; case 'K' : //: 경유 // sprintf(where_oiltype, "where (jcomp=0 and k_price > 0) ") ; sprintf(where_oiltype, "and (k_price>100) ") ; break ; case 'D' : //: 등유 // sprintf(where_oiltype, "where (jcomp=0 and d_price > 0) ") ; sprintf(where_oiltype, "and (d_price>100) ") ; break ; case 'L' : //: LPG // sprintf(where_oiltype, "where (jcomp=1 and l_price > 0) ") ; sprintf(where_oiltype, "and (l_price >100) ") ; break ; default : break ; } switch(hd->search_opt[0]) { case 'A' : //: 좌표로 검색 sprintf(where_search_opt, "and (map_x > '%d' and map_x < '%d' and map_y > '%d' and map_y < '%d') ", (req->x - req->radius), (req->x + req->radius), (req->y - req->radius), (req->y + req->radius)) ; break ; case 'I' : //: 주소로 검색 if(strlen(req->sido)>0) sido_flag = 1 ; if(strlen(req->gugun)>0) gugun_flag = 1 ; if(strlen(req->dong)>0) dong_flag = 1 ; if((sido_flag == 1) && (gugun_flag == 1) && (dong_flag == 1)) sprintf(where_search_opt, "and (sido='%.16s' and gugun='%.32s' and dong like '%%%.52s%%') ", req->sido, req->gugun, req->dong) ; else if((sido_flag == 1) && (gugun_flag == 1) && (dong_flag == 0)) sprintf(where_search_opt, "and (sido='%.16s' and gugun='%.32s') ", req->sido, req->gugun) ; else if((sido_flag == 1) && (gugun_flag == 0) && (dong_flag == 0)) sprintf(where_search_opt, "and (sido='%.16s') ", req->sido) ; break ; case 'U' : //: 국도(상)로 검색 sprintf(where_search_opt, "and (type_way=%d and updown='L') ", req->nr_num+100) ; break ; case 'D' : //: 국도(하)로 검색 sprintf(where_search_opt, "and (type_way=%d and updown = 'R') ", req->nr_num+100) ; break ; default : break ; } if(req->pole != 0) { //: pole=7: SK-gas pole=8: LG-gas // if((req->pole == 7) || (req->pole == 8)) sprintf(where_pole, "and (pole=7 or pole=8) ") ; if((req->pole == 7) || (req->pole == 8)) sprintf(where_pole, "and (jcomp=1 or jcomp=2) ") ; else if(req->pole <= 6) { if(req->pole == 1) sprintf(where_pole, "and (pole=1 or pole=14) ") ; else sprintf(where_pole, "and (pole=%d) ", req->pole) ; } else { short pole = req->pole ; short flag = 0 ; short a4=0, a5=0, a6=0, a7=0, a8=0, a9=0, a10=0, a11=0 ; char temp_pole[300] = {'\0'} ; pole = pole >> 4 ; a4 = pole & 0x01 ; //SK pole = pole >> 1 ; a5 = pole & 0x01 ; //LG pole = pole >> 1 ; a6 = pole & 0x01 ; //H-OIL pole = pole >> 1 ; a7 = pole & 0x01 ; //S-OIL pole = pole >> 1 ; a8 = pole & 0x01 ; //T-OIL pole = pole >> 1 ; a9 = pole & 0x01 ; //무폴 pole = pole >> 1 ; a10 = pole & 0x01 ; //SK-GAS pole = pole >> 1 ; a11 = pole & 0x01 ; //LG-GAS printLog(HEAD, "POLE[%d]: a11[%d]a10[%d]a9[%d]a8[%d]a7[%d]a6[%d]a5[%d]a4[%d]\n", pole, a11, a10, a9, a8, a7, a6, a5, a4) ; if(a4) { strcat(temp_pole, "pole=1 ") ; flag = 1 ; } if(a5) { if(flag) strcat(temp_pole, "or pole=2 ") ; else { strcat(temp_pole, "pole=2 ") ; flag = 1 ; } } if(a6) { if(flag) strcat(temp_pole, "or pole=3 ") ; else { strcat(temp_pole, "pole=3 ") ; flag = 1 ; } } if(a7) { if(flag) strcat(temp_pole, "or pole=4 ") ; else { strcat(temp_pole, "pole=4 ") ; flag = 1 ; } } if(a8) { if(flag) strcat(temp_pole, "or pole=5 ") ; else { strcat(temp_pole, "pole=5 ") ; flag = 1 ; } } if(a9) { if(flag) strcat(temp_pole, "or pole=6 ") ; else { strcat(temp_pole, "pole=6 ") ; flag = 1 ; } } if(a10 || a11) { if(flag) strcat(temp_pole, "or (jcomp=1 or jcomp=2) ") ; else { strcat(temp_pole, "(jcomp=1 or jcomp=2) ") ; flag = 1 ; } } sprintf(where_pole, "and (%s) ", temp_pole) ; } } switch(req->sort_opt) { case 1 : //: 휘발유 가격으로 오름차순 sprintf(where_sort_opt, "and (g_price > 0 ) order by g_price asc") ; break ; case 2 : //: 경유 가격으로 오름차순 sprintf(where_sort_opt, "and (k_price > 0) order by k_price asc") ; break ; case 3 : //: 등유 가격으로 오름차순 sprintf(where_sort_opt, "and (d_price > 0) order by d_price asc") ; break ; case 4 : //: LPG 가격으로 오름차순 sprintf(where_sort_opt, "and (l_price > 0) order by l_price asc") ; break ; case 6 : //: 휘발유>경유>등유>LPG 오름차순 sprintf(where_sort_opt, "order by g_price, k_price, d_price, l_price asc") ; break ; default : break ; } sprintf(sql, "select j_id, jname, pole, g_price, k_price, d_price, l_price, " "date_format(update_time, '%%Y%%m%%d'), date_format(update_time, '%%H%%i%%s'), " "map_x, map_y from TSmain where (update_time > date_add(now(), interval -1 month)) %s %s %s %s", where_oiltype, where_search_opt, where_pole, where_sort_opt) ; printLog(HEAD, "sql(%s)\n", sql) ; // ret = mydbc_execute_get_result(&conn, sql, &cnt, &res_set); ret = mysql_query(conn, sql) ; if(ret == DB_SUCCESS) { res_set = mysql_store_result(conn) ; if(res_set == NULL) ret = E_BAD_SQL ; else cnt = (int) mysql_num_rows(res_set) ; } if(ret != DB_SUCCESS) { if(data != NULL) free(data), data=NULL ; ret = E_BAD_SQL ; goto error_exit ; } if(cnt == 0) { printLog(HEAD, "ERR: (%d) NO DATA ABOUT TSmain\n", hd->mdn) ; ret = E_NO_DATA ; goto error_exit ; } if((buf = (OPW_LIST_INFO *) malloc (sizeof(OPW_LIST_INFO) * cnt)) == NULL) { printLog(HEAD, "ERR: Fatal Memory Alloc Fail\n") ; ret = E_MEM_ALLOC ; goto error_exit ; } memset(buf, 0, sizeof(OPW_LIST_INFO) * cnt) ; for(i = 0 ; i < cnt ; i++) { if((row = mydbc_next_row(&res_set)) == NULL) break; n = -1; if(row[++n] != NULL) buf[i].id = atoi(row[n]) ; if(row[++n] != NULL) strncpy(buf[i].name, row[n], D_NAME_SIZE) ; if(row[++n] != NULL) buf[i].pole = atoi(row[n]) ; if(buf[i].pole == 14) buf[i].pole = 1 ; //: SK인천정유 주유소(14)->SK(1) if(buf[i].pole == 15) buf[i].pole = 7 ; //: SK인천 GAS(15)->SK GAS(7) if(row[++n] != NULL) { buf[i].g_price = atoi(row[n]) ; if((req->sort_opt == 6) && (buf[i].g_price == 0)) buf[i].g_price = ZERO_MAX_CONV ; } if(row[++n] != NULL) { buf[i].k_price = atoi(row[n]) ; if((req->sort_opt == 6) && (buf[i].k_price == 0)) buf[i].k_price = ZERO_MAX_CONV ; } if(row[++n] != NULL) { buf[i].d_price = atoi(row[n]) ; if((req->sort_opt == 6) && (buf[i].d_price == 0)) buf[i].d_price = ZERO_MAX_CONV ; } if(row[++n] != NULL) { buf[i].l_price = atoi(row[n]) ; if((req->sort_opt == 6) && (buf[i].l_price == 0)) buf[i].l_price = ZERO_MAX_CONV ; } if(row[++n] != NULL) buf[i].update_date = atoi(row[n]) ; if(row[++n] != NULL) buf[i].update_time = atoi(row[n]) ; if(row[++n] != NULL) buf[i].x = atoi(row[n]) ; if(row[++n] != NULL) buf[i].y = atoi(row[n]) ; //: 좌표로 검색인 경우 if(hd->search_opt[0]=='A') { buf[i].distance = get_distance(req->x, req->y, buf[i].x, buf[i].y) ; } //: 2007-05-28 valid data구함 if((buf[i].x != 0) && (buf[i].y != 0)) valid_cnt++ ; } if(req->sort_opt == 6) { qsort(buf, cnt, sizeof(OPW_LIST_INFO), sort_comp) ; for(i = 0 ; i <cnt ; i++) { if(buf[i].g_price == ZERO_MAX_CONV) buf[i].g_price = 0 ; if(buf[i].k_price == ZERO_MAX_CONV) buf[i].k_price = 0 ; if(buf[i].d_price == ZERO_MAX_CONV) buf[i].d_price = 0 ; if(buf[i].l_price == ZERO_MAX_CONV) buf[i].l_price = 0 ; printLog(HEAD, "[%d] id(%d)name(%.52s)pole(%d)g(%d)k(%d)d(%d)l(%d)date(%d)time(%d)x(%d)y(%d)distance(%d)\n", i, buf[i].id, buf[i].name, buf[i].pole, buf[i].g_price, buf[i].k_price, buf[i].d_price, buf[i].l_price, buf[i].update_date, buf[i].update_time, buf[i].x, buf[i].y, buf[i].distance) ; } } //: 좌표로 검색인 경우이고 거리로 오름차순일때 if((hd->search_opt[0]=='A') && (req->sort_opt == 5)) { qsort(buf, cnt, sizeof(OPW_LIST_INFO), (int (*)(const void *, const void *)) cmp_string_up) ; } //: 최종 검색 데이타 printLog(HEAD, "(%d): cnt(%d) valid_cnt[%d] req->start_pos(%d) req->search_num(%d)\n", hd->mdn, cnt, valid_cnt, req->start_pos, req->search_num) ; //: 2007-05-28 valid한 데이타로 fetch count구함 // if(cnt <= (req->start_pos)) { if(valid_cnt <= (req->start_pos)) { printLog(HEAD, "ERR: (%d) NO DATA(cnt:%d-valid_cnt:%d) more than start(%d)\n", hd->mdn, cnt, valid_cnt, req->start_pos) ; ret = E_NO_DATA ; goto error_exit ; } // if((cnt - (req->start_pos)) < req->search_num) fetch_count = cnt - (req->start_pos) ; if((valid_cnt - (req->start_pos)) < req->search_num) fetch_count = valid_cnt - (req->start_pos) ; else fetch_count = req->search_num ; printLog(HEAD, "DEBUG: fetch_count(%d)\n", fetch_count) ; if((data = (OPW_LIST_INFO *) malloc (sizeof(OPW_LIST_INFO) * fetch_count)) == NULL) { printLog(HEAD, "ERR: Fatal Memory Alloc Fail\n") ; ret = E_MEM_ALLOC ; goto error_exit ; } memset(data, 0, sizeof(OPW_LIST_INFO) * fetch_count) ; // for(i= 0, n=0 ; (i < fetch_count) && (n < cnt) ; n++) { for(i= 0, n=0 ; n < cnt ; n++) { // 2007-05-28 예외 사항 추가 // x가 0 또는 y=0이면 패스 if((buf[n].x == 0) || (buf[n].y == 0)) { k++ ; continue ; } if(n < (req->start_pos + k)) continue ; else if(n >= (req->start_pos+fetch_count+k)) continue ; memcpy(&data[i], &buf[n], sizeof(OPW_LIST_INFO)) ; printLog(HEAD, "[%d] id(%d)name(%.52s)pole(%d)g(%d)k(%d)d(%d)l(%d)date(%d)time(%d)x(%d)y(%d)distance(%d)\n", i, data[i].id, data[i].name, data[i].pole, data[i].g_price, data[i].k_price, data[i].d_price, data[i].l_price, data[i].update_date, data[i].update_time, data[i].x, data[i].y, data[i].distance) ; data[i].id = htonl(data[i].id) ; data[i].pole = htonl(data[i].pole) ; if(data[i].g_price < 100) data[i].g_price = 0 ; data[i].g_price = htons(data[i].g_price) ; if(data[i].k_price < 100) data[i].k_price = 0 ; data[i].k_price = htons(data[i].k_price) ; if(data[i].d_price < 100) data[i].d_price = 0 ; data[i].d_price = htons(data[i].d_price) ; if(data[i].l_price < 100) data[i].l_price = 0 ; data[i].l_price = htons(data[i].l_price) ; data[i].update_date = htonl(data[i].update_date) ; data[i].update_time = htonl(data[i].update_time) ; data[i].x = htonl(data[i].x) ; data[i].y = htonl(data[i].y) ; data[i].distance = htonl(data[i].distance) ; i++ ; } *out = data; error_exit: // mydbc_free_result(&res_set) ; mysql_free_result(res_set) ; if(buf != NULL) free(buf) ; buf = NULL ; rd->res_code = ret ; // rd->total_num = cnt ; rd->total_num = valid_cnt ; rd->search_num = fetch_count ; mysql_close(conn) ; // ret = mydbc_end(&conn); // if(ret != DB_SUCCESS) { // printLog(HEAD, "ERR: mydbc_end[%d]\n", ret); // } return ret ; }
double get_distance_out(double lat1, double lng1, double lat2, double lng2) { return 1.2 * (get_distance(lat1, lng1, lat2, lng2)); }
int seg_build_streamlines(SEGMENT *streams, SEGMENT *dirs, SEGMENT *elevation, int number_of_streams) { int r, c, i; int d, next_d; int prev_r, prev_c; int stream_num = 1, cell_num = 0; int contrib_cell; STREAM *SA; int border_dir; int streams_cell, dirs_cell; int dirs_prev_cell; float elevation_cell, elevation_prev_cell; stream_attributes = (STREAM *) G_malloc(number_of_streams * sizeof(STREAM)); G_message(_("Finding inits...")); SA = stream_attributes; /* finding inits */ for (r = 0; r < nrows; ++r) for (c = 0; c < ncols; ++c) { Segment_get(streams, &streams_cell, r, c); if (streams_cell) if (seg_trib_nums(r, c, streams, dirs) != 1) { /* adding inits */ if (stream_num > number_of_streams) G_fatal_error(_("Error finding inits. Stream and direction maps probably do not match")); SA[stream_num].stream = stream_num; SA[stream_num].init = INDEX(r, c); stream_num++; } } /* building streamline */ for (i = 1; i < stream_num; ++i) { r = (int)SA[i].init / ncols; c = (int)SA[i].init % ncols; Segment_get(streams, &streams_cell, r, c); SA[i].order = streams_cell; SA[i].number_of_cells = 0; do { SA[i].number_of_cells++; Segment_get(dirs, &dirs_cell, r, c); d = abs(dirs_cell); if (NOT_IN_REGION(d) || d == 0) break; r = NR(d); c = NC(d); Segment_get(streams, &streams_cell, r, c); } while (streams_cell == SA[i].order); SA[i].number_of_cells += 2; /* add two extra points for point before init and after outlet */ } for (i = 1; i < number_of_streams; ++i) { SA[i].points = (unsigned long int *) G_malloc((SA[i].number_of_cells) * sizeof(unsigned long int)); SA[i].elevation = (float *) G_malloc((SA[i].number_of_cells) * sizeof(float)); SA[i].distance = (double *) G_malloc((SA[i].number_of_cells) * sizeof(double)); r = (int)SA[i].init / ncols; c = (int)SA[i].init % ncols; contrib_cell = seg_find_contributing_cell(r, c, dirs, elevation); prev_r = NR(contrib_cell); prev_c = NC(contrib_cell); /* add one point contributing to init to calculate parameters */ /* what to do if there is no contributing points? */ Segment_get(dirs, &dirs_cell, r, c); Segment_get(dirs, &dirs_prev_cell, prev_r, prev_c); Segment_get(elevation, &elevation_prev_cell, prev_r, prev_c); Segment_get(elevation, &elevation_cell, r, c); SA[i].points[0] = (contrib_cell == 0) ? -1 : INDEX(prev_r, prev_c); SA[i].elevation[0] = (contrib_cell == 0) ? -99999 : elevation_prev_cell; d = (contrib_cell == 0) ? dirs_cell : dirs_prev_cell; SA[i].distance[0] = (contrib_cell == 0) ? get_distance(r, c, d) : get_distance(prev_r, prev_c, d); SA[i].points[1] = INDEX(r, c); SA[i].elevation[1] = elevation_cell; d = abs(dirs_cell); SA[i].distance[1] = get_distance(r, c, d); cell_num = 2; do { Segment_get(dirs, &dirs_cell, r, c); d = abs(dirs_cell); if (NOT_IN_REGION(d) || d == 0) { SA[i].points[cell_num] = -1; SA[i].distance[cell_num] = SA[i].distance[cell_num - 1]; SA[i].elevation[cell_num] = 2 * SA[i].elevation[cell_num - 1] - SA[i].elevation[cell_num - 2]; border_dir = convert_border_dir(r, c, dirs_cell); SA[i].last_cell_dir = border_dir; break; } r = NR(d); c = NC(d); Segment_get(dirs, &dirs_cell, r, c); SA[i].last_cell_dir = dirs_cell; SA[i].points[cell_num] = INDEX(r, c); Segment_get(elevation, &SA[i].elevation[cell_num], r, c); next_d = (abs(dirs_cell) == 0) ? d : abs(dirs_cell); SA[i].distance[cell_num] = get_distance(r, c, next_d); cell_num++; if (cell_num > SA[i].number_of_cells) G_fatal_error(_("To much points in stream line")); Segment_get(streams, &streams_cell, r, c); } while (streams_cell == SA[i].order); if (SA[i].elevation[0] == -99999) SA[i].elevation[0] = 2 * SA[i].elevation[1] - SA[i].elevation[2]; } return 0; }
double track_segment::get_avg_speed() { return (get_distance() / get_duration()); }
// return distance in centimeters to between two locations uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2) { return get_distance(loc1, loc2) * 100; }
/* update navigation for landing */ bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range) { switch (stage) { case DEEPSTALL_STAGE_FLY_TO_LANDING: if (get_distance(current_loc, landing_point) > fabsf(2 * landing.aparm.loiter_radius)) { landing.nav_controller->update_waypoint(current_loc, landing_point); return false; } stage = DEEPSTALL_STAGE_ESTIMATE_WIND; loiter_sum_cd = 0; // reset the loiter counter FALLTHROUGH; case DEEPSTALL_STAGE_ESTIMATE_WIND: { landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1); if (!landing.nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) { // wait until the altitude is correct before considering a breakout return false; } // only count loiter progress when within the target altitude int32_t target_bearing = landing.nav_controller->target_bearing_cd(); int32_t delta = wrap_180_cd(target_bearing - last_target_bearing); delta *= (landing_point.flags.loiter_ccw ? -1 : 1); if (delta > 0) { // only accumulate turns in the correct direction loiter_sum_cd += delta; } last_target_bearing = target_bearing; if (loiter_sum_cd < 36000) { // wait until we've done at least one complete loiter at the correct altitude return false; } stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT; loiter_sum_cd = 0; // reset the loiter counter FALLTHROUGH; } case DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT: // rebuild the approach path if we have done less then a full circle to allow it to be // more into the wind as the estimator continues to refine itself, and allow better // compensation on windy days. This is limited to a single full circle though, as on // a no wind day you could be in this loop forever otherwise. if (loiter_sum_cd < 36000) { build_approach_path(false); } if (!verify_breakout(current_loc, arc_entry, height)) { int32_t target_bearing = landing.nav_controller->target_bearing_cd(); int32_t delta = wrap_180_cd(target_bearing - last_target_bearing); if (delta > 0) { // only accumulate turns in the correct direction loiter_sum_cd += delta; } last_target_bearing = target_bearing; landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1); return false; } stage = DEEPSTALL_STAGE_FLY_TO_ARC; memcpy(&breakout_location, ¤t_loc, sizeof(Location)); FALLTHROUGH; case DEEPSTALL_STAGE_FLY_TO_ARC: if (get_distance(current_loc, arc_entry) > 2 * landing.aparm.loiter_radius) { landing.nav_controller->update_waypoint(breakout_location, arc_entry); return false; } stage = DEEPSTALL_STAGE_ARC; FALLTHROUGH; case DEEPSTALL_STAGE_ARC: { Vector2f groundspeed = landing.ahrs.groundspeed_vector(); if (!landing.nav_controller->reached_loiter_target() || (fabsf(wrap_180(target_heading_deg - degrees(atan2f(-groundspeed.y, -groundspeed.x) + M_PI))) >= 10.0f)) { landing.nav_controller->update_loiter(arc, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1); return false; } stage = DEEPSTALL_STAGE_APPROACH; } FALLTHROUGH; case DEEPSTALL_STAGE_APPROACH: { Location entry_point; landing.nav_controller->update_waypoint(arc_exit, extended_approach); float relative_alt_D; landing.ahrs.get_relative_position_D_home(relative_alt_D); const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, false); memcpy(&entry_point, &landing_point, sizeof(Location)); location_update(entry_point, target_heading_deg + 180.0, travel_distance); if (!location_passed_point(current_loc, arc_exit, entry_point)) { if (location_passed_point(current_loc, arc_exit, extended_approach)) { // this should never happen, but prevent against an indefinite fly away stage = DEEPSTALL_STAGE_FLY_TO_LANDING; } return false; } predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, true); stage = DEEPSTALL_STAGE_LAND; stall_entry_time = AP_HAL::millis(); const SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator); if (elevator != nullptr) { // take the last used elevator angle as the starting deflection // don't worry about bailing here if the elevator channel can't be found // that will be handled within override_servos initial_elevator_pwm = elevator->get_output_pwm(); } } FALLTHROUGH; case DEEPSTALL_STAGE_LAND: // while in deepstall the only thing verify needs to keep the extended approach point sufficently far away landing.nav_controller->update_waypoint(current_loc, extended_approach); landing.disarm_if_autoland_complete_fn(); return false; default: return true; } }