Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : //
17 : // DroneCAN GPS driver
18 : //
19 : #pragma once
20 :
21 : #include "AP_GPS_config.h"
22 :
23 : #if AP_GPS_DRONECAN_ENABLED
24 :
25 : #include <AP_Common/AP_Common.h>
26 : #include <AP_HAL/AP_HAL.h>
27 : #include "AP_GPS.h"
28 : #include "GPS_Backend.h"
29 : #include "RTCM3_Parser.h"
30 : #include <AP_DroneCAN/AP_DroneCAN.h>
31 :
32 : class AP_GPS_DroneCAN : public AP_GPS_Backend {
33 : public:
34 : AP_GPS_DroneCAN(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role);
35 : ~AP_GPS_DroneCAN();
36 :
37 : bool read() override;
38 :
39 : bool is_healthy(void) const override;
40 :
41 : bool logging_healthy(void) const override;
42 :
43 : bool is_configured(void) const override;
44 :
45 0 : const char *name() const override { return _name; }
46 :
47 : static bool subscribe_msgs(AP_DroneCAN* ap_dronecan);
48 : static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state);
49 :
50 : static void handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg);
51 :
52 : static void handle_aux_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Auxiliary& msg);
53 : static void handle_heading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Heading& msg);
54 : static void handle_status_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_Status& msg);
55 : #if GPS_MOVING_BASELINE
56 : static void handle_moving_baseline_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_MovingBaselineData& msg);
57 : static void handle_relposheading_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_gnss_RelPosHeading& msg);
58 : #endif
59 : static bool inter_instance_pre_arm_checks(char failure_msg[], uint16_t failure_msg_len);
60 : void inject_data(const uint8_t *data, uint16_t len) override;
61 :
62 0 : bool get_error_codes(uint32_t &error_codes) const override { error_codes = error_code; return seen_status; };
63 :
64 : #if GPS_MOVING_BASELINE
65 : bool get_RTCMV3(const uint8_t *&data, uint16_t &len) override;
66 : void clear_RTCMV3() override;
67 : #endif
68 :
69 : #if AP_DRONECAN_SEND_GPS
70 : static bool instance_exists(const AP_DroneCAN* ap_dronecan);
71 : #endif
72 :
73 : private:
74 :
75 : bool param_configured = true;
76 : enum config_step {
77 : STEP_SET_TYPE = 0,
78 : STEP_SET_MB_CAN_TX,
79 : STEP_SAVE_AND_REBOOT,
80 : STEP_FINISHED
81 : };
82 : uint8_t cfg_step;
83 : bool requires_save_and_reboot;
84 :
85 : // returns true once configuration has finished
86 : bool do_config(void);
87 :
88 : void handle_fix2_msg(const uavcan_equipment_gnss_Fix2& msg, uint64_t timestamp_usec);
89 : void handle_aux_msg(const uavcan_equipment_gnss_Auxiliary& msg);
90 : void handle_heading_msg(const ardupilot_gnss_Heading& msg);
91 : void handle_status_msg(const ardupilot_gnss_Status& msg);
92 : void handle_velocity(const float vx, const float vy, const float vz);
93 :
94 : #if GPS_MOVING_BASELINE
95 : void handle_moving_baseline_msg(const ardupilot_gnss_MovingBaselineData& msg, uint8_t node_id);
96 : void handle_relposheading_msg(const ardupilot_gnss_RelPosHeading& msg, uint8_t node_id);
97 : #endif
98 :
99 : static bool take_registry();
100 : static void give_registry();
101 : static AP_GPS_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
102 :
103 : bool _new_data;
104 : AP_GPS::GPS_State interim_state;
105 :
106 : HAL_Semaphore sem;
107 :
108 : uint8_t _detected_module;
109 : bool seen_message;
110 : bool seen_fix2;
111 : bool seen_aux;
112 : bool seen_status;
113 : bool seen_relposheading;
114 : bool seen_valid_height_ellipsoid;
115 :
116 : bool healthy;
117 : uint32_t status_flags;
118 : uint32_t error_code;
119 : char _name[16];
120 :
121 : // Module Detection Registry
122 : static struct DetectedModules {
123 : AP_DroneCAN* ap_dronecan;
124 : uint8_t node_id;
125 : uint8_t instance;
126 : uint32_t last_inject_ms;
127 : AP_GPS_DroneCAN* driver;
128 : } _detected_modules[GPS_MAX_RECEIVERS];
129 :
130 : static HAL_Semaphore _sem_registry;
131 :
132 : #if GPS_MOVING_BASELINE
133 : // RTCM3 parser for when in moving baseline base mode
134 : RTCM3_Parser *rtcm3_parser;
135 : uint32_t last_base_warning_ms;
136 : #endif
137 : // the role set from GPS_TYPE
138 : AP_GPS::GPS_Role role;
139 :
140 : FUNCTOR_DECLARE(param_int_cb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
141 : FUNCTOR_DECLARE(param_float_cb, bool, AP_DroneCAN*, const uint8_t, const char*, float &);
142 : FUNCTOR_DECLARE(param_save_cb, void, AP_DroneCAN*, const uint8_t, bool);
143 :
144 : bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value);
145 : bool handle_param_get_set_response_float(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, float &value);
146 : void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success);
147 : void send_rtcm(void);
148 :
149 : // GNSS RTCM injection
150 : struct {
151 : uint32_t last_send_ms;
152 : ByteBuffer *buf;
153 : } _rtcm_stream;
154 :
155 : // returns true if the supplied GPS_Type is a DroneCAN GPS type
156 76915 : static bool is_dronecan_gps_type(AP_GPS::GPS_Type type) {
157 : return (
158 76902 : type == AP_GPS::GPS_TYPE_UAVCAN ||
159 153817 : type == AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE ||
160 : type == AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER
161 76915 : );
162 : }
163 : };
164 :
165 : #endif // AP_GPS_DRONECAN_ENABLED
|
