mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
AP_UAVCAN: reduce whitespace indentation
This commit is contained in:
Tom Pittenger
Tom Pittenger
parent
c49d4aef50
commit
1add05d9a1
@ -108,191 +108,187 @@ static uavcan::equipment::ahrs::Solution _att_state;
|
||||
|
||||
void AP_UAVCAN::UAVCAN_AHRS_update(const AP_AHRS_NavEKF &ahrs)
|
||||
{
|
||||
bool sem_ret = _fix_out_sem->take(1);
|
||||
if (_fix_out_sem->take(1) &&
|
||||
(fix_out_array[_uavcan_i] != nullptr || fix2_out_array[_uavcan_i] != nullptr)) {
|
||||
const AP_GPS &cgps = AP::gps();
|
||||
Location loc;
|
||||
Vector3f vel_NED;
|
||||
|
||||
if (sem_ret) {
|
||||
if (fix_out_array[_uavcan_i] != nullptr || fix2_out_array[_uavcan_i] != nullptr) {
|
||||
const AP_GPS &cgps = AP::gps();
|
||||
Location loc;
|
||||
Vector3f vel_NED;
|
||||
if (ahrs.healthy() && ahrs.get_location(loc)
|
||||
&& ahrs.get_velocity_NED(vel_NED)) {
|
||||
float velVar, posVar, hgtVar;
|
||||
Vector3f magVar;
|
||||
float tasVar;
|
||||
Vector2f offset;
|
||||
ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
|
||||
if (ahrs.healthy() && ahrs.get_location(loc)
|
||||
&& ahrs.get_velocity_NED(vel_NED)) {
|
||||
float velVar, posVar, hgtVar;
|
||||
Vector3f magVar;
|
||||
float tasVar;
|
||||
Vector2f offset;
|
||||
ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset);
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.position_covariance.resize(3);
|
||||
_fix_state.position_covariance[0] = posVar;
|
||||
_fix_state.position_covariance[1] = posVar;
|
||||
_fix_state.position_covariance[2] = hgtVar;
|
||||
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.position_covariance.resize(3);
|
||||
_fix_state.position_covariance[0] = posVar;
|
||||
_fix_state.position_covariance[1] = posVar;
|
||||
_fix_state.position_covariance[2] = hgtVar;
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.velocity_covariance.resize(3);
|
||||
_fix_state.velocity_covariance[0] = velVar;
|
||||
_fix_state.velocity_covariance[1] = velVar;
|
||||
_fix_state.velocity_covariance[2] = velVar;
|
||||
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.velocity_covariance.resize(3);
|
||||
_fix_state.velocity_covariance[0] = velVar;
|
||||
_fix_state.velocity_covariance[1] = velVar;
|
||||
_fix_state.velocity_covariance[2] = velVar;
|
||||
// Manual resize to diagonal form
|
||||
_fix2_state.covariance.resize(6);
|
||||
_fix2_state.covariance[0] = posVar;
|
||||
_fix2_state.covariance[1] = posVar;
|
||||
_fix2_state.covariance[2] = hgtVar;
|
||||
_fix2_state.covariance[3] = velVar;
|
||||
_fix2_state.covariance[4] = velVar;
|
||||
_fix2_state.covariance[5] = velVar;
|
||||
} else {
|
||||
loc = cgps.location();
|
||||
vel_NED = cgps.velocity();
|
||||
|
||||
// Manual resize to diagonal form
|
||||
_fix2_state.covariance.resize(6);
|
||||
_fix2_state.covariance[0] = posVar;
|
||||
_fix2_state.covariance[1] = posVar;
|
||||
_fix2_state.covariance[2] = hgtVar;
|
||||
_fix2_state.covariance[3] = velVar;
|
||||
_fix2_state.covariance[4] = velVar;
|
||||
_fix2_state.covariance[5] = velVar;
|
||||
} else {
|
||||
loc = cgps.location();
|
||||
vel_NED = cgps.velocity();
|
||||
float vel_acc, hacc, vert_acc;
|
||||
cgps.horizontal_accuracy(hacc);
|
||||
cgps.vertical_accuracy(vert_acc);
|
||||
cgps.speed_accuracy(vel_acc);
|
||||
|
||||
float vel_acc, hacc, vert_acc;
|
||||
cgps.horizontal_accuracy(hacc);
|
||||
cgps.vertical_accuracy(vert_acc);
|
||||
cgps.speed_accuracy(vel_acc);
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.position_covariance.resize(3);
|
||||
_fix_state.position_covariance[0] = hacc * hacc;
|
||||
_fix_state.position_covariance[1] = _fix_state.position_covariance[0];
|
||||
_fix_state.position_covariance[2] = vert_acc * vert_acc;
|
||||
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.position_covariance.resize(3);
|
||||
_fix_state.position_covariance[0] = hacc * hacc;
|
||||
_fix_state.position_covariance[1] = _fix_state.position_covariance[0];
|
||||
_fix_state.position_covariance[2] = vert_acc * vert_acc;
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.velocity_covariance.resize(3);
|
||||
_fix_state.velocity_covariance[0] = vel_acc * vel_acc;
|
||||
_fix_state.velocity_covariance[1] = _fix_state.velocity_covariance[0];
|
||||
_fix_state.velocity_covariance[2] = _fix_state.velocity_covariance[0];
|
||||
|
||||
// Manual resize to diagonal form
|
||||
_fix_state.velocity_covariance.resize(3);
|
||||
_fix_state.velocity_covariance[0] = vel_acc * vel_acc;
|
||||
_fix_state.velocity_covariance[1] = _fix_state.velocity_covariance[0];
|
||||
_fix_state.velocity_covariance[2] = _fix_state.velocity_covariance[0];
|
||||
|
||||
// Manual resize to diagonal form
|
||||
_fix2_state.covariance.resize(6);
|
||||
_fix2_state.covariance[0] = _fix_state.position_covariance[0];
|
||||
_fix2_state.covariance[1] = _fix_state.position_covariance[1];
|
||||
_fix2_state.covariance[2] = _fix_state.position_covariance[2];
|
||||
_fix2_state.covariance[3] = _fix_state.velocity_covariance[0];
|
||||
_fix2_state.covariance[4] = _fix_state.velocity_covariance[1];
|
||||
_fix2_state.covariance[5] = _fix_state.velocity_covariance[2];
|
||||
}
|
||||
|
||||
_fix_state.height_msl_mm = loc.alt * 10;
|
||||
_fix_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10;
|
||||
_fix_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10;
|
||||
|
||||
_fix2_state.height_msl_mm = loc.alt * 10;
|
||||
_fix2_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10;
|
||||
_fix2_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10;
|
||||
|
||||
// Not saved in AP
|
||||
//_fix_state.height_ellipsoid_mm
|
||||
|
||||
_fix_state.ned_velocity[0] = vel_NED.x;
|
||||
_fix_state.ned_velocity[1] = vel_NED.y;
|
||||
_fix_state.ned_velocity[2] = vel_NED.z;
|
||||
|
||||
_fix2_state.ned_velocity[0] = vel_NED.x;
|
||||
_fix2_state.ned_velocity[1] = vel_NED.y;
|
||||
_fix2_state.ned_velocity[2] = vel_NED.z;
|
||||
|
||||
_fix_state.sats_used = cgps.num_sats();
|
||||
_fix_state.pdop = cgps.get_hdop();
|
||||
|
||||
_fix2_state.sats_used = _fix_state.sats_used;
|
||||
_fix2_state.pdop = _fix_state.pdop;
|
||||
|
||||
switch(cgps.status()) {
|
||||
case AP_GPS::GPS_Status::NO_GPS:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::NO_FIX:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_2D_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_3D_FIX;
|
||||
break;
|
||||
}
|
||||
|
||||
switch(cgps.status()) {
|
||||
case AP_GPS::GPS_Status::NO_GPS:
|
||||
case AP_GPS::GPS_Status::NO_FIX:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED;
|
||||
break;
|
||||
}
|
||||
|
||||
_fix_state.gnss_time_standard = uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC;
|
||||
_fix2_state.gnss_time_standard = _fix_state.gnss_time_standard;
|
||||
uavcan::Timestamp ts;
|
||||
ts.usec = 1000ULL * ((cgps.time_week() * AP_MSEC_PER_WEEK + cgps.time_week_ms()) + UNIX_OFFSET_MSEC);
|
||||
_fix_state.gnss_timestamp = ts;
|
||||
_fix2_state.gnss_timestamp = ts;
|
||||
|
||||
_fix_state.num_leap_seconds = 0;
|
||||
_fix2_state.num_leap_seconds = 0;
|
||||
ts.usec = 0;
|
||||
_fix_state.timestamp = ts;
|
||||
_fix2_state.timestamp = ts;
|
||||
// Manual resize to diagonal form
|
||||
_fix2_state.covariance.resize(6);
|
||||
_fix2_state.covariance[0] = _fix_state.position_covariance[0];
|
||||
_fix2_state.covariance[1] = _fix_state.position_covariance[1];
|
||||
_fix2_state.covariance[2] = _fix_state.position_covariance[2];
|
||||
_fix2_state.covariance[3] = _fix_state.velocity_covariance[0];
|
||||
_fix2_state.covariance[4] = _fix_state.velocity_covariance[1];
|
||||
_fix2_state.covariance[5] = _fix_state.velocity_covariance[2];
|
||||
}
|
||||
|
||||
if (attitude_out_array[_uavcan_i] != nullptr) {
|
||||
uavcan::Timestamp ts;
|
||||
ts.usec = AP_HAL::micros64();
|
||||
_att_state.timestamp = ts;
|
||||
_fix_state.height_msl_mm = loc.alt * 10;
|
||||
_fix_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10;
|
||||
_fix_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10;
|
||||
|
||||
Quaternion qt;
|
||||
Matrix3f rot = ahrs.get_rotation_body_to_ned();
|
||||
qt.from_rotation_matrix(rot);
|
||||
_att_state.orientation_xyzw[0] = qt.q1;
|
||||
_att_state.orientation_xyzw[1] = qt.q2;
|
||||
_att_state.orientation_xyzw[2] = qt.q3;
|
||||
_att_state.orientation_xyzw[3] = qt.q4;
|
||||
_fix2_state.height_msl_mm = loc.alt * 10;
|
||||
_fix2_state.latitude_deg_1e8 = ((uint64_t) loc.lat) * 10;
|
||||
_fix2_state.longitude_deg_1e8 = ((uint64_t) loc.lng) * 10;
|
||||
|
||||
// TODO: extract from EKF
|
||||
//_att_state.orientation_covariance
|
||||
// Not saved in AP
|
||||
//_fix_state.height_ellipsoid_mm
|
||||
|
||||
Vector3f av = ahrs.get_gyro();
|
||||
_att_state.angular_velocity[0] = av.x;
|
||||
_att_state.angular_velocity[1] = av.y;
|
||||
_att_state.angular_velocity[2] = av.z;
|
||||
_fix_state.ned_velocity[0] = vel_NED.x;
|
||||
_fix_state.ned_velocity[1] = vel_NED.y;
|
||||
_fix_state.ned_velocity[2] = vel_NED.z;
|
||||
|
||||
// TODO: extract from EKF
|
||||
//_att_state.angular_velocity_covariance
|
||||
_fix2_state.ned_velocity[0] = vel_NED.x;
|
||||
_fix2_state.ned_velocity[1] = vel_NED.y;
|
||||
_fix2_state.ned_velocity[2] = vel_NED.z;
|
||||
|
||||
Vector3f la = ahrs.get_accel_ef();
|
||||
_att_state.linear_acceleration[0] = la.x;
|
||||
_att_state.linear_acceleration[1] = la.y;
|
||||
_att_state.linear_acceleration[2] = la.z;
|
||||
_fix_state.sats_used = cgps.num_sats();
|
||||
_fix_state.pdop = cgps.get_hdop();
|
||||
|
||||
// TODO: extract from EKF
|
||||
//_att_state.linear_acceleration_covariance
|
||||
_fix2_state.sats_used = _fix_state.sats_used;
|
||||
_fix2_state.pdop = _fix_state.pdop;
|
||||
|
||||
switch(cgps.status()) {
|
||||
case AP_GPS::GPS_Status::NO_GPS:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::NO_FIX:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_NO_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_2D_FIX;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||
_fix_state.status = uavcan::equipment::gnss::Fix::STATUS_3D_FIX;
|
||||
break;
|
||||
}
|
||||
|
||||
switch(cgps.status()) {
|
||||
case AP_GPS::GPS_Status::NO_GPS:
|
||||
case AP_GPS::GPS_Status::NO_FIX:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_2D:
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_SINGLE;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_DGPS;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_DGPS_OTHER;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FLOAT;
|
||||
break;
|
||||
case AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED:
|
||||
_fix2_state.mode = uavcan::equipment::gnss::Fix2::MODE_RTK;
|
||||
_fix2_state.sub_mode = uavcan::equipment::gnss::Fix2::SUB_MODE_RTK_FIXED;
|
||||
break;
|
||||
}
|
||||
|
||||
_fix_state.gnss_time_standard = uavcan::equipment::gnss::Fix::GNSS_TIME_STANDARD_UTC;
|
||||
_fix2_state.gnss_time_standard = _fix_state.gnss_time_standard;
|
||||
uavcan::Timestamp ts;
|
||||
ts.usec = 1000ULL * ((cgps.time_week() * AP_MSEC_PER_WEEK + cgps.time_week_ms()) + UNIX_OFFSET_MSEC);
|
||||
_fix_state.gnss_timestamp = ts;
|
||||
_fix2_state.gnss_timestamp = ts;
|
||||
|
||||
_fix_state.num_leap_seconds = 0;
|
||||
_fix2_state.num_leap_seconds = 0;
|
||||
ts.usec = 0;
|
||||
_fix_state.timestamp = ts;
|
||||
_fix2_state.timestamp = ts;
|
||||
|
||||
_fix_out_sem->give();
|
||||
} else {
|
||||
if (sem_ret) {
|
||||
_fix_out_sem->give();
|
||||
}
|
||||
}
|
||||
|
||||
if (_att_out_sem->take(1) && (attitude_out_array[_uavcan_i] != nullptr)) {
|
||||
uavcan::Timestamp ts;
|
||||
ts.usec = AP_HAL::micros64();
|
||||
_att_state.timestamp = ts;
|
||||
|
||||
Quaternion qt;
|
||||
Matrix3f rot = ahrs.get_rotation_body_to_ned();
|
||||
qt.from_rotation_matrix(rot);
|
||||
_att_state.orientation_xyzw[0] = qt.q1;
|
||||
_att_state.orientation_xyzw[1] = qt.q2;
|
||||
_att_state.orientation_xyzw[2] = qt.q3;
|
||||
_att_state.orientation_xyzw[3] = qt.q4;
|
||||
|
||||
// TODO: extract from EKF
|
||||
//_att_state.orientation_covariance
|
||||
|
||||
Vector3f av = ahrs.get_gyro();
|
||||
_att_state.angular_velocity[0] = av.x;
|
||||
_att_state.angular_velocity[1] = av.y;
|
||||
_att_state.angular_velocity[2] = av.z;
|
||||
|
||||
// TODO: extract from EKF
|
||||
//_att_state.angular_velocity_covariance
|
||||
|
||||
Vector3f la = ahrs.get_accel_ef();
|
||||
_att_state.linear_acceleration[0] = la.x;
|
||||
_att_state.linear_acceleration[1] = la.y;
|
||||
_att_state.linear_acceleration[2] = la.z;
|
||||
|
||||
// TODO: extract from EKF
|
||||
//_att_state.linear_acceleration_covariance
|
||||
|
||||
_att_out_sem->give();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void gnss_fix_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>& msg, uint8_t mgr)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user