|
|
|
|
@ -296,6 +296,7 @@ bool AP_GPS_NMEA::_term_complete()
@@ -296,6 +296,7 @@ bool AP_GPS_NMEA::_term_complete()
|
|
|
|
|
|
|
|
|
|
#if HAL_WITH_UAVCAN |
|
|
|
|
uint8_t can_num_drivers = AP::can().get_num_drivers(); |
|
|
|
|
|
|
|
|
|
for (uint8_t i = 0; i < can_num_drivers; i++) |
|
|
|
|
{ |
|
|
|
|
AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i); |
|
|
|
|
@ -321,16 +322,20 @@ bool AP_GPS_NMEA::_term_complete()
@@ -321,16 +322,20 @@ bool AP_GPS_NMEA::_term_complete()
|
|
|
|
|
|
|
|
|
|
if (logger != nullptr) |
|
|
|
|
{ |
|
|
|
|
// uint32_t tk1 = AP_HAL::micros();
|
|
|
|
|
Vector3f cam_pos_ned = camera_pos_rotation(); |
|
|
|
|
// uint32_t tk2 = AP_HAL::micros();
|
|
|
|
|
Vector3d llh = Vector3d(gps_mark.lat, gps_mark.lon, gps_mark.hgt); |
|
|
|
|
Vector3f sigma = Vector3f(gps_mark.lat_sigma, gps_mark.lon_sigma, gps_mark.hgt_sigma); |
|
|
|
|
AP::logger().Write_RTK_Mark_Pos( |
|
|
|
|
gps_mark.week, |
|
|
|
|
gps_mark.second, |
|
|
|
|
llh, |
|
|
|
|
gps_mark.undulation, |
|
|
|
|
sigma, |
|
|
|
|
gps_mark.diff_age, |
|
|
|
|
gps_mark.sol_age); |
|
|
|
|
cam_pos_ned, |
|
|
|
|
state.status); |
|
|
|
|
// gcs().send_text(MAV_SEVERITY_INFO, "mrk:%d, %d, %d, %d", gps_mark.week, gps_mark.second, (int32_t)(gps_mark.lat * (int32_t)10000000UL), (int32_t)(gps_mark.lon * (int32_t)10000000UL));
|
|
|
|
|
// gcs().send_text(MAV_SEVERITY_INFO,"rot:%dus,%.3f,%.3f,%.3f",(uint16_t)(tk2 - tk1),cam_pos_ned.x,cam_pos_ned.y,cam_pos_ned.z);
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
@ -675,6 +680,30 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
@@ -675,6 +680,30 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data)
|
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
bool AP_GPS_NMEA::get_offset_param() |
|
|
|
|
{ |
|
|
|
|
const Vector3f &off = gps._antenna_offset[1].get(); |
|
|
|
|
_cam_offset.x = off.x; |
|
|
|
|
_cam_offset.y = off.y; |
|
|
|
|
_cam_offset.z = off.z; |
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO,"ant offset:%.3f,%.3f,%.3f",_cam_offset.x,_cam_offset.y,_cam_offset.z); |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
Vector3f AP_GPS_NMEA::camera_pos_rotation() |
|
|
|
|
{ |
|
|
|
|
Matrix3f Tbn; // dcm rotation matrix to rotate body frame to north
|
|
|
|
|
Vector3f target_vec_unit_body; |
|
|
|
|
Vector3f cam_pos_ned = {-0.02,-0.225,0.17}; |
|
|
|
|
if(!get_param){ |
|
|
|
|
get_param = get_offset_param(); |
|
|
|
|
return cam_pos_ned; |
|
|
|
|
} |
|
|
|
|
const AP_AHRS &ahrs = AP::ahrs(); |
|
|
|
|
Tbn = ahrs.get_rotation_body_to_ned(); |
|
|
|
|
cam_pos_ned = Tbn * (_cam_offset); |
|
|
|
|
return cam_pos_ned; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Calculate and return the CRC for usA binary buffer
|
|
|
|
|
// unsigned long CalculateCRC32(u_char *szBuf, int iSize)
|
|
|
|
|
|
