<commit_before>/*
// Copyright (c) 2016 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
*/

#include <algorithm>
#include <arpa/inet.h>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstdio>
#include <iostream>
#include <mavlink.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <vector>

#include "mavlink_vehicles.hh"

#if PRINT_MAVLINK
#define print_mavlink(...) printf(__VA_ARGS__)
#else
#define print_mavlink(...) ;
#endif

#if VERBOSE
#define print_verbose(...) printf("[mav_vehicle] " __VA_ARGS__)
#else
#define print_verbose(...) ;
#endif

namespace defaults
{
const uint8_t target_system_id = 1;
const uint8_t target_component_id = 1;
const uint8_t system_id = 20;
const uint8_t component_id = 0;
const float takeoff_init_alt_m = 1.5;
const float lookat_rot_speed_degps = 90.0;
const size_t send_buffer_len = 2041;
const uint16_t remote_max_response_time_ms = 10000;
const double waypoint_acceptance_radius_m = 0.01;
const double arrival_max_dist_m = 0.5;
const double rotation_arrival_max_dif_deg = 10.0;
const double autorotate_max_targ_angle = 20.0;
const double is_stopped_max_speed_mps = 0.3;
}

namespace request_intervals_ms
{
const uint16_t request_mission_item = 3000;
const uint16_t home_position = 3000;
const uint16_t arm_disarm = 1000;
const uint16_t heartbeat = 1000;
const uint16_t set_mode = 1000;
const uint16_t takeoff = 1000;
const uint16_t rotate = 200;
}

bool is_timedout(std::chrono::time_point<std::chrono::system_clock> timestamp,
                 uint16_t timeout_ms)
{
    using namespace std::chrono;
    return duration_cast<milliseconds>(system_clock::now() - timestamp)
               .count() > timeout_ms;
}

bool is_same_socket(sockaddr_storage &r1, sockaddr_storage &r2)
{
    return (((struct sockaddr_in *)&r1)->sin_addr.s_addr ==
                ((struct sockaddr_in *)&r2)->sin_addr.s_addr &&
            ((struct sockaddr_in *)&r1)->sin_port ==
                ((struct sockaddr_in *)&r2)->sin_port);
}

namespace mavlink_vehicles
{

namespace math
{

inline double rad2deg(double x)
{
    return (180.0 / M_PI) * x;
}

inline double deg2rad(double x)
{
    return (M_PI / 180.0) * x;
}

local_pos global_to_local_ned(global_pos_int point, global_pos_int reference)
{
    // Scaling factor to convert from degrees (fixed-point-1e7) to meters at
    // equator: (M_PI/(180*1e7))*EARTH_RADIUS_AT_EQUATOR
    const double deg_to_meters_scaling_factor = 0.011131884502145034f;

    // Compensate shrinking in earth radius as you move north or
    // shouth from the equator.
    double radius_scaling_factor = cosf(math::deg2rad(reference.lat * 1.0e-7f));

    // Subtract and scale
    local_pos local;
    local.x = (point.lat - reference.lat) * deg_to_meters_scaling_factor;
    local.y = (point.lon - reference.lon) * deg_to_meters_scaling_factor *
              radius_scaling_factor;

    // Subtract and convert from millimeters (int) to meters (double)
    local.z = (point.alt - reference.alt) * 1.0e-3f;

    // Convert from ENU to NED
    std::swap(local.x, local.y);
    local.z = -local.z;

    return local;
}

global_pos_int local_ned_to_global(local_pos point, global_pos_int reference)
{
    // Scaling factor to convert from meters to degrees (fixed-point-1e7) at
    // equator: ((180*1e7)/M_PI)/EARTH_RADIUS_AT_EQUATOR
    const double meters_to_deg_scaling_factor = 89.83204953368922f;

    // Compensate shrinking in earth radius as you move north or
    // south from the equator.
    double radius_scaling_factor = cosf(math::deg2rad(reference.lat * 1.0e-7f));

    // Convert from NED to ENU
    std::swap(point.x, point.y);
    point.z = -point.z;

    // Scale and add
    global_pos_int global;
    global.lat = point.x * meters_to_deg_scaling_factor + reference.lat;
    global.lon =
        point.y * meters_to_deg_scaling_factor / radius_scaling_factor +
        reference.lon;

    // Convert from meters (double) to millimeters (int) and sum with reference
    global.alt = point.z * 1000.0 + reference.alt;

    return global;
}

double ground_dist(global_pos_int p1, global_pos_int p2)
{
    local_pos res = global_to_local_ned(p1, p2);
    return sqrt(res.x * res.x + res.y * res.y);
}

double ground_dist(local_pos p1, local_pos p2)
{
    local_pos res(p2.x - p1.x, p2.y - p1.y, 0);
    return sqrt(res.x * res.x + res.y * res.y);
}

double dist(global_pos_int p1, global_pos_int p2)
{
    local_pos res = global_to_local_ned(p1, p2);
    return sqrt(res.x * res.x + res.y * res.y + res.z * res.z);
}

double get_waypoint_rel_angle(global_pos_int wp_pos, global_pos_int ref_pos,
                              attitude ref_att)
{
    local_pos wp_rel = global_to_local_ned(wp_pos, ref_pos);

    // Calculate relative angle
    double wp_angle = math::rad2deg(atan2(wp_rel.y, wp_rel.x));

    // While yaw follow NED convention, wp_angle is calculated according to ENU
    // conventions. We need to follow the same convention before subracting them
    // up to calculate the relative angle.
    wp_angle = -wp_angle + 90.0;

    double wp_rel_angle = rad2deg(ref_att.yaw) - wp_angle;

    if (wp_rel_angle > 180.0) {
        wp_rel_angle = wp_rel_angle - 360.0;
    } else if (wp_rel_angle < -180.0) {
        wp_rel_angle = wp_rel_angle + 360.0;
    }

    return wp_rel_angle;
}
}

class msghandler
{
  public:
    static void handle(mav_vehicle &mav, const mavlink_message_t *msg);
};
<commit_msg>Fix global-local convertions (ENU/NED)<commit_after>/*
// Copyright (c) 2016 Intel Corporation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
*/

#include <algorithm>
#include <arpa/inet.h>
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <cstdio>
#include <iostream>
#include <mavlink.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <vector>

#include "mavlink_vehicles.hh"

#if PRINT_MAVLINK
#define print_mavlink(...) printf(__VA_ARGS__)
#else
#define print_mavlink(...) ;
#endif

#if VERBOSE
#define print_verbose(...) printf("[mav_vehicle] " __VA_ARGS__)
#else
#define print_verbose(...) ;
#endif

namespace defaults
{
const uint8_t target_system_id = 1;
const uint8_t target_component_id = 1;
const uint8_t system_id = 20;
const uint8_t component_id = 0;
const float takeoff_init_alt_m = 1.5;
const float lookat_rot_speed_degps = 90.0;
const size_t send_buffer_len = 2041;
const uint16_t remote_max_response_time_ms = 10000;
const double waypoint_acceptance_radius_m = 0.01;
const double arrival_max_dist_m = 0.5;
const double rotation_arrival_max_dif_deg = 10.0;
const double autorotate_max_targ_angle = 20.0;
const double is_stopped_max_speed_mps = 0.3;
}

namespace request_intervals_ms
{
const uint16_t request_mission_item = 3000;
const uint16_t home_position = 3000;
const uint16_t arm_disarm = 1000;
const uint16_t heartbeat = 1000;
const uint16_t set_mode = 1000;
const uint16_t takeoff = 1000;
const uint16_t rotate = 200;
}

bool is_timedout(std::chrono::time_point<std::chrono::system_clock> timestamp,
                 uint16_t timeout_ms)
{
    using namespace std::chrono;
    return duration_cast<milliseconds>(system_clock::now() - timestamp)
               .count() > timeout_ms;
}

bool is_same_socket(sockaddr_storage &r1, sockaddr_storage &r2)
{
    return (((struct sockaddr_in *)&r1)->sin_addr.s_addr ==
                ((struct sockaddr_in *)&r2)->sin_addr.s_addr &&
            ((struct sockaddr_in *)&r1)->sin_port ==
                ((struct sockaddr_in *)&r2)->sin_port);
}

namespace mavlink_vehicles
{

namespace math
{

inline double rad2deg(double x)
{
    return (180.0 / M_PI) * x;
}

inline double deg2rad(double x)
{
    return (M_PI / 180.0) * x;
}

local_pos global_to_local_ned(global_pos_int point, global_pos_int reference)
{
    // Scaling factor to convert from degrees (fixed-point-1e7) to meters at
    // equator: (M_PI/(180*1e7))*EARTH_RADIUS_AT_EQUATOR
    const double deg_to_meters_scaling_factor = 0.011131884502145034f;

    // Compensate shrinking in earth radius as you move north or
    // shouth from the equator.
    double radius_scaling_factor = cosf(math::deg2rad(reference.lat * 1.0e-7f));

    // Subtract and scale
    local_pos local;
    local.x = (point.lat - reference.lat) * deg_to_meters_scaling_factor;
    local.y = (point.lon - reference.lon) * deg_to_meters_scaling_factor *
              radius_scaling_factor;

    // Subtract and convert from millimeters (int) to meters (double)
    local.z = (point.alt - reference.alt) * 1.0e-3f;

    // Set Altitude-Down (NED convention)
    local.z = -local.z;

    return local;
}

global_pos_int local_ned_to_global(local_pos point, global_pos_int reference)
{
    // Scaling factor to convert from meters to degrees (fixed-point-1e7) at
    // equator: ((180*1e7)/M_PI)/EARTH_RADIUS_AT_EQUATOR
    const double meters_to_deg_scaling_factor = 89.83204953368922f;

    // Compensate shrinking in earth radius as you move north or
    // south from the equator.
    double radius_scaling_factor = cosf(math::deg2rad(reference.lat * 1.0e-7f));

    // Set Altitude-UP (ENU convention)
    point.z = -point.z;

    // Scale and add
    global_pos_int global;
    global.lat = point.x * meters_to_deg_scaling_factor + reference.lat;
    global.lon =
        point.y * meters_to_deg_scaling_factor / radius_scaling_factor +
        reference.lon;

    // Convert from meters (double) to millimeters (int) and sum with reference
    global.alt = point.z * 1000.0 + reference.alt;

    return global;
}

double ground_dist(global_pos_int p1, global_pos_int p2)
{
    local_pos res = global_to_local_ned(p1, p2);
    return sqrt(res.x * res.x + res.y * res.y);
}

double ground_dist(local_pos p1, local_pos p2)
{
    local_pos res(p2.x - p1.x, p2.y - p1.y, 0);
    return sqrt(res.x * res.x + res.y * res.y);
}

double dist(global_pos_int p1, global_pos_int p2)
{
    local_pos res = global_to_local_ned(p1, p2);
    return sqrt(res.x * res.x + res.y * res.y + res.z * res.z);
}

double get_waypoint_rel_angle(global_pos_int wp_pos, global_pos_int ref_pos,
                              attitude ref_att)
{
    local_pos wp_rel = global_to_local_ned(wp_pos, ref_pos);

    // Calculate relative angle
    double wp_angle = math::rad2deg(atan2(wp_rel.y, wp_rel.x));

    double wp_rel_angle = rad2deg(ref_att.yaw) - wp_angle;

    if (wp_rel_angle > 180.0) {
        wp_rel_angle = wp_rel_angle - 360.0;
    } else if (wp_rel_angle < -180.0) {
        wp_rel_angle = wp_rel_angle + 360.0;
    }

    return wp_rel_angle;
}
}

class msghandler
{
  public:
    static void handle(mav_vehicle &mav, const mavlink_message_t *msg);
};

{'fraction_non_alphanumeric': 0.1069069069069069, 'fraction_numerical': 0.035263835263835266, 'mean_word_length': 3.562035225048924, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 43, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '349285', 'n_tokens_mistral': 4405, 'n_tokens_neox': 4094, 'n_words': 1054}