CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/AP_Periph/gps.cpp
Views: 1798
1
#include "AP_Periph.h"

2


3
#ifdef HAL_PERIPH_ENABLE_GPS

4


5
/*

6
  GPS support

7
 */

8


9
#include <dronecan_msgs.h>

10
#include <AP_GPS/RTCM3_Parser.h>

11


12
#define DEBUG_PRINTS 0

13


14
#if DEBUG_PRINTS

15
 # define Debug(fmt, args ...)  do {can_printf(fmt "\n", ## args);} while(0)

16
#else

17
 # define Debug(fmt, args ...)

18
#endif

19


20
/*

21
  handle gnss::RTCMStream

22
 */

23
void AP_Periph_FW::handle_RTCMStream(CanardInstance* canard_instance, CanardRxTransfer* transfer)

24
{

25
    uavcan_equipment_gnss_RTCMStream req;

26
    if (uavcan_equipment_gnss_RTCMStream_decode(transfer, &req)) {

27
        return;

28
    }

29
    gps.handle_gps_rtcm_fragment(0, req.data.data, req.data.len);

30
}

31


32
/*

33
    handle gnss::MovingBaselineData

34
*/

35
#if GPS_MOVING_BASELINE

36
void AP_Periph_FW::handle_MovingBaselineData(CanardInstance* canard_instance, CanardRxTransfer* transfer)

37
{

38
    ardupilot_gnss_MovingBaselineData msg;

39
    if (ardupilot_gnss_MovingBaselineData_decode(transfer, &msg)) {

40
        return;

41
    }

42
    gps.inject_MBL_data(msg.data.data, msg.data.len);

43
    Debug("MovingBaselineData: len=%u\n", msg.data.len);

44
}

45
#endif // GPS_MOVING_BASELINE

46


47
/*

48
  convert large values to NaN for float16

49
 */

50
static void check_float16_range(float *v, uint8_t len)

51
{

52
    for (uint8_t i=0; i<len; i++) {

53
        const float f16max = 65519;

54
        if (isinf(v[i]) || v[i] <= -f16max || v[i] >= f16max) {

55
            v[i] = nanf("");

56
        }

57
    }

58
}

59


60
/*

61
  update CAN GPS

62
 */

63
void AP_Periph_FW::can_gps_update(void)

64
{

65
    if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {

66
        return;

67
    }

68
    gps.update();

69
    send_moving_baseline_msg();

70
    send_relposheading_msg();

71
    if (last_gps_update_ms == gps.last_message_time_ms()) {

72
        return;

73
    }

74
    last_gps_update_ms = gps.last_message_time_ms();

75


76
    {

77
        /*

78
          send Fix2 packet

79
        */

80
        uavcan_equipment_gnss_Fix2 pkt {};

81
        const Location &loc = gps.location();

82
        const Vector3f &vel = gps.velocity();

83
        if (gps.status() < AP_GPS::GPS_OK_FIX_2D && !saw_gps_lock_once) {

84
            pkt.timestamp.usec = AP_HAL::micros64();

85
            pkt.gnss_timestamp.usec = 0;

86
        } else {

87
            saw_gps_lock_once = true;

88
            pkt.timestamp.usec = gps.time_epoch_usec();

89
            pkt.gnss_timestamp.usec = gps.last_message_epoch_usec();

90
        }

91
        if (pkt.gnss_timestamp.usec == 0) {

92
            pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_NONE;

93
        } else {

94
            pkt.gnss_time_standard = UAVCAN_EQUIPMENT_GNSS_FIX_GNSS_TIME_STANDARD_UTC;

95
        }

96
        pkt.longitude_deg_1e8 = uint64_t(loc.lng) * 10ULL;

97
        pkt.latitude_deg_1e8 = uint64_t(loc.lat) * 10ULL;

98
        pkt.height_ellipsoid_mm = loc.alt * 10;

99
        pkt.height_msl_mm = loc.alt * 10;

100
        float undulation;

101
        if (gps.get_undulation(undulation)) {

102
            pkt.height_ellipsoid_mm -= undulation*1000;

103
        }

104
        for (uint8_t i=0; i<3; i++) {

105
            pkt.ned_velocity[i] = vel[i];

106
        }

107
        pkt.sats_used = gps.num_sats();

108
        switch (gps.status()) {

109
        case AP_GPS::GPS_Status::NO_GPS:

110
        case AP_GPS::GPS_Status::NO_FIX:

111
            pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_NO_FIX;

112
            pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;

113
            pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;

114
            break;

115
        case AP_GPS::GPS_Status::GPS_OK_FIX_2D:

116
            pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_2D_FIX;

117
            pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;

118
            pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;

119
            break;

120
        case AP_GPS::GPS_Status::GPS_OK_FIX_3D:

121
            pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;

122
            pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_SINGLE;

123
            pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_OTHER;

124
            break;

125
        case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:

126
            pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;

127
            pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_DGPS;

128
            pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_DGPS_SBAS;

129
            break;

130
        case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:

131
            pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;

132
            pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;

133
            pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FLOAT;

134
            break;

135
        case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:

136
            pkt.status = UAVCAN_EQUIPMENT_GNSS_FIX2_STATUS_3D_FIX;

137
            pkt.mode = UAVCAN_EQUIPMENT_GNSS_FIX2_MODE_RTK;

138
            pkt.sub_mode = UAVCAN_EQUIPMENT_GNSS_FIX2_SUB_MODE_RTK_FIXED;

139
            break;

140
        }

141


142
        pkt.covariance.len = 6;

143


144
        float hacc;

145
        if (gps.horizontal_accuracy(hacc)) {

146
            pkt.covariance.data[0] = pkt.covariance.data[1] = sq(hacc);

147
        }

148
    

149
        float vacc;

150
        if (gps.vertical_accuracy(vacc)) {

151
            pkt.covariance.data[2] = sq(vacc);

152
        }

153


154
        float sacc;

155
        if (gps.speed_accuracy(sacc)) {

156
            float vc3 = sq(sacc);

157
            pkt.covariance.data[3] = pkt.covariance.data[4] = pkt.covariance.data[5] = vc3;

158
        }

159


160
        check_float16_range(pkt.covariance.data, pkt.covariance.len);

161


162
        uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_FIX2_MAX_SIZE];

163
        uint16_t total_size = uavcan_equipment_gnss_Fix2_encode(&pkt, buffer, !canfdout());

164


165
        canard_broadcast(UAVCAN_EQUIPMENT_GNSS_FIX2_SIGNATURE,

166
                         UAVCAN_EQUIPMENT_GNSS_FIX2_ID,

167
                         CANARD_TRANSFER_PRIORITY_LOW,

168
                         &buffer[0],

169
                         total_size);

170
    }

171
    

172
    /*

173
      send aux packet

174
     */

175
    {

176
        uavcan_equipment_gnss_Auxiliary aux {};

177
        aux.hdop = gps.get_hdop() * 0.01;

178
        aux.vdop = gps.get_vdop() * 0.01;

179


180
        uint8_t buffer[UAVCAN_EQUIPMENT_GNSS_AUXILIARY_MAX_SIZE];

181
        uint16_t total_size = uavcan_equipment_gnss_Auxiliary_encode(&aux, buffer, !canfdout());

182
        canard_broadcast(UAVCAN_EQUIPMENT_GNSS_AUXILIARY_SIGNATURE,

183
                        UAVCAN_EQUIPMENT_GNSS_AUXILIARY_ID,

184
                        CANARD_TRANSFER_PRIORITY_LOW,

185
                        &buffer[0],

186
                        total_size);

187
    }

188


189
    // send the gnss status packet

190
    {

191
        ardupilot_gnss_Status status {};

192


193
        status.healthy = gps.is_healthy();

194
        if (gps.logging_present() && gps.logging_enabled() && !gps.logging_failed()) {

195
            status.status |= ARDUPILOT_GNSS_STATUS_STATUS_LOGGING;

196
        }

197
        uint8_t idx; // unused

198
        if (status.healthy && !gps.first_unconfigured_gps(idx)) {

199
            status.status |= ARDUPILOT_GNSS_STATUS_STATUS_ARMABLE;

200
        }

201


202
        uint32_t error_codes;

203
        if (gps.get_error_codes(error_codes)) {

204
            status.error_codes = error_codes;

205
        }

206


207
        uint8_t buffer[ARDUPILOT_GNSS_STATUS_MAX_SIZE];

208
        const uint16_t total_size = ardupilot_gnss_Status_encode(&status, buffer, !canfdout());

209
        canard_broadcast(ARDUPILOT_GNSS_STATUS_SIGNATURE,

210
                        ARDUPILOT_GNSS_STATUS_ID,

211
                        CANARD_TRANSFER_PRIORITY_LOW,

212
                        &buffer[0],

213
                        total_size);

214


215
    }

216


217
    // send Heading message if we are not sending RelPosHeading messages and have yaw

218
    if (gps.have_gps_yaw() && last_relposheading_ms == 0) {

219
        float yaw_deg, yaw_acc_deg;

220
        uint32_t yaw_time_ms;

221
        if (gps.gps_yaw_deg(yaw_deg, yaw_acc_deg, yaw_time_ms) && yaw_time_ms != last_gps_yaw_ms) {

222
            last_gps_yaw_ms = yaw_time_ms;

223
            

224
            ardupilot_gnss_Heading heading {};

225
            heading.heading_valid = true;

226
            heading.heading_accuracy_valid = is_positive(yaw_acc_deg);

227
            heading.heading_rad = radians(yaw_deg);

228
            heading.heading_accuracy_rad = radians(yaw_acc_deg);

229
            uint8_t buffer[ARDUPILOT_GNSS_HEADING_MAX_SIZE];

230
            const uint16_t total_size = ardupilot_gnss_Heading_encode(&heading, buffer, !canfdout());

231
            canard_broadcast(ARDUPILOT_GNSS_HEADING_SIGNATURE,

232
                             ARDUPILOT_GNSS_HEADING_ID,

233
                             CANARD_TRANSFER_PRIORITY_LOW,

234
                             &buffer[0],

235
                             total_size);

236
        }

237
    }

238
}

239


240


241
void AP_Periph_FW::send_moving_baseline_msg()

242
{

243
#if GPS_MOVING_BASELINE

244
    const uint8_t *data = nullptr;

245
    uint16_t len = 0;

246
    if (!gps.get_RTCMV3(data, len)) {

247
        return;

248
    }

249
    if (len == 0 || data == nullptr) {

250
        return;

251
    }

252


253
    /*

254
      send the packet from Moving Base to be used RelPosHeading calc by GPS module

255
      for long RTCMv3 packets we may need to split it up

256
    */

257


258
    uint8_t iface_mask = 0;

259
#if HAL_NUM_CAN_IFACES >= 2 && CANARD_MULTI_IFACE

260
    if (gps_mb_can_port != -1 && (gps_mb_can_port < HAL_NUM_CAN_IFACES)) {

261
        iface_mask = 1U<<gps_mb_can_port;

262
    }

263
#endif

264


265
    // get the data from the moving base and send as MovingBaselineData message

266
    while (len > 0) {

267
        ardupilot_gnss_MovingBaselineData mbldata {};

268


269
        const uint16_t n = MIN(len, sizeof(mbldata.data.data));

270


271
        mbldata.data.len = n;

272
        memcpy(mbldata.data.data, data, n);

273


274
        uint8_t buffer[ARDUPILOT_GNSS_MOVINGBASELINEDATA_MAX_SIZE];

275
        const uint16_t total_size = ardupilot_gnss_MovingBaselineData_encode(&mbldata, buffer, !canfdout());

276


277
        canard_broadcast(ARDUPILOT_GNSS_MOVINGBASELINEDATA_SIGNATURE,

278
                         ARDUPILOT_GNSS_MOVINGBASELINEDATA_ID,

279
                         CANARD_TRANSFER_PRIORITY_LOW,

280
                         &buffer[0],

281
                         total_size,

282
                         iface_mask);

283
        len -= n;

284
        data += n;

285
    }

286


287
    gps.clear_RTCMV3();

288
#endif // GPS_MOVING_BASELINE

289
}

290


291
void AP_Periph_FW::send_relposheading_msg() {

292
#if GPS_MOVING_BASELINE

293
    float reported_heading;

294
    float relative_distance;

295
    float relative_down_pos;

296
    float reported_heading_acc;

297
    uint32_t curr_timestamp = 0;

298
    if (!gps.get_RelPosHeading(curr_timestamp, reported_heading, relative_distance, relative_down_pos, reported_heading_acc) ||

299
        last_relposheading_ms == curr_timestamp) {

300
        return;

301
    }

302
    last_relposheading_ms = curr_timestamp;

303
    ardupilot_gnss_RelPosHeading relpos {};

304
    relpos.timestamp.usec = uint64_t(curr_timestamp)*1000LLU;

305
    relpos.reported_heading_deg = reported_heading;

306
    relpos.relative_distance_m = relative_distance;

307
    relpos.relative_down_pos_m = relative_down_pos;

308
    relpos.reported_heading_acc_deg = reported_heading_acc;

309
    relpos.reported_heading_acc_available = !is_zero(relpos.reported_heading_acc_deg);

310
    uint8_t buffer[ARDUPILOT_GNSS_RELPOSHEADING_MAX_SIZE];

311
    const uint16_t total_size = ardupilot_gnss_RelPosHeading_encode(&relpos, buffer, !canfdout());

312
    canard_broadcast(ARDUPILOT_GNSS_RELPOSHEADING_SIGNATURE,

313
                    ARDUPILOT_GNSS_RELPOSHEADING_ID,

314
                    CANARD_TRANSFER_PRIORITY_LOW,

315
                    &buffer[0],

316
                    total_size);

317
#endif // GPS_MOVING_BASELINE

318
}

319


320
#endif // HAL_PERIPH_ENABLE_GPS

321


322