/*
* 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;
}
}
