CoCalc -- AC_Avoidance

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AC_Avoidance/AC_Avoidance_Logging.cpp
⁴¹⁸² views
1
#include <AP_Logger/AP_Logger_config.h>
2

3
#if HAL_LOGGING_ENABLED
4

5
#include <AP_AHRS/AP_AHRS.h>
6
#include "AC_Avoid.h"
7
#include "AP_OADijkstra.h"
8
#include "AP_OABendyRuler.h"
9
#include <AP_Logger/AP_Logger.h>
10
#include <AP_AHRS/AP_AHRS.h>
11

12
#if AP_OAPATHPLANNER_BENDYRULER_ENABLED
13
void AP_OABendyRuler::Write_OABendyRuler(const uint8_t type, const bool active, const float target_yaw, const float target_pitch, const bool resist_chg, const float margin, const Location &final_dest, const Location &oa_dest) const
14
{
15
    auto &logger = AP::logger();
16

17
    float oa_dest_alt;
18
    if (!oa_dest.get_alt_m(Location::AltFrame::ABOVE_ORIGIN, oa_dest_alt)) {
19
        oa_dest_alt = logger.quiet_nanf();
20
    }
21
    float final_alt;
22
    if (!final_dest.get_alt_m(Location::AltFrame::ABOVE_ORIGIN, final_alt)) {
23
        final_alt = logger.quiet_nanf();
24
    }
25

26
    const struct log_OABendyRuler pkt{
27
        LOG_PACKET_HEADER_INIT(LOG_OA_BENDYRULER_MSG),
28
        time_us     : AP_HAL::micros64(),
29
        type        : type,
30
        active      : active,
31
        target_yaw  : (uint16_t)wrap_360(target_yaw),
32
        yaw         : (uint16_t)wrap_360(AP::ahrs().get_yaw_deg()),
33
        target_pitch: (uint16_t)target_pitch,
34
        resist_chg  : resist_chg,
35
        margin      : margin,
36
        final_lat   : final_dest.lat,
37
        final_lng   : final_dest.lng,
38
        final_alt   : final_alt,
39
        oa_lat      : oa_dest.lat,
40
        oa_lng      : oa_dest.lng,
41
        oa_alt      : oa_dest_alt
42
    };
43
    logger.WriteBlock(&pkt, sizeof(pkt));
44
}
45
#endif  // AP_OAPATHPLANNER_BENDYRULER_ENABLED
46

47
#if AP_OAPATHPLANNER_DIJKSTRA_ENABLED
48
void AP_OADijkstra::Write_OADijkstra(const uint8_t state, const uint8_t error_id, const uint8_t curr_point, const uint8_t tot_points, const Location &final_dest, const Location &oa_dest) const
49
{
50
    const struct log_OADijkstra pkt{
51
        LOG_PACKET_HEADER_INIT(LOG_OA_DIJKSTRA_MSG),
52
        time_us     : AP_HAL::micros64(),
53
        state       : state,
54
        error_id    : error_id,
55
        curr_point  : curr_point,
56
        tot_points  : tot_points,
57
        final_lat   : final_dest.lat,
58
        final_lng   : final_dest.lng,
59
        oa_lat      : oa_dest.lat,
60
        oa_lng      : oa_dest.lng
61
    };
62
    AP::logger().WriteBlock(&pkt, sizeof(pkt));
63
}
64
#endif
65

66
#if AP_OAPATHPLANNER_ENABLED
67
void AP_OADijkstra::Write_Visgraph_point(const uint8_t version, const uint8_t point_num, const int32_t Lat, const int32_t Lon) const
68
{
69
    const struct log_OD_Visgraph pkt{
70
        LOG_PACKET_HEADER_INIT(LOG_OD_VISGRAPH_MSG),
71
        time_us     : AP_HAL::micros64(),
72
        version     : version,
73
        point_num   : point_num,
74
        Lat         : Lat,
75
        Lon         : Lon,
76
    };
77
    AP::logger().WriteBlock(&pkt, sizeof(pkt));
78
}
79
#endif
80

81
#if AP_AVOIDANCE_ENABLED
82
void AC_Avoid::Write_SimpleAvoidance(const uint8_t state, const Vector3f& desired_vel, const Vector3f& modified_vel, const bool back_up) const
83
{
84
    const struct log_SimpleAvoid pkt{
85
        LOG_PACKET_HEADER_INIT(LOG_SIMPLE_AVOID_MSG),
86
        time_us         : AP_HAL::micros64(),
87
        state           : state,
88
        desired_vel_x   : desired_vel.x * 0.01f,
89
        desired_vel_y   : desired_vel.y * 0.01f,
90
        desired_vel_z   : desired_vel.z * 0.01f,
91
        modified_vel_x  : modified_vel.x * 0.01f,
92
        modified_vel_y  : modified_vel.y * 0.01f,
93
        modified_vel_z  : modified_vel.z * 0.01f,
94
        backing_up      : back_up,
95
    };
96
    AP::logger().WriteBlock(&pkt, sizeof(pkt));
97
}
98
#endif
99

100
#endif  // HAL_LOGGING_ENABLED
101

102
Product

Resources

Company
