|
|
@ -1131,7 +1131,7 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg) |
|
|
|
vision_position.az = pos.az; |
|
|
|
vision_position.az = pos.az; |
|
|
|
|
|
|
|
|
|
|
|
// Low risk change for now. TODO : full covariance matrix
|
|
|
|
// Low risk change for now. TODO : full covariance matrix
|
|
|
|
vision_position.eph = sqrtf(fmaxf(pos.covariance[0],pos.covariance[9])); |
|
|
|
vision_position.eph = sqrtf(fmaxf(pos.covariance[0], pos.covariance[9])); |
|
|
|
vision_position.epv = sqrtf(pos.covariance[17]); |
|
|
|
vision_position.epv = sqrtf(pos.covariance[17]); |
|
|
|
|
|
|
|
|
|
|
|
vision_position.xy_global = globallocalconverter_initialized(); |
|
|
|
vision_position.xy_global = globallocalconverter_initialized(); |
|
|
@ -1173,8 +1173,10 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) |
|
|
|
vision_attitude.q[3] = q(3); |
|
|
|
vision_attitude.q[3] = q(3); |
|
|
|
|
|
|
|
|
|
|
|
int instance_id = 0; |
|
|
|
int instance_id = 0; |
|
|
|
orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &instance_id, ORB_PRIO_DEFAULT); |
|
|
|
orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &instance_id, |
|
|
|
orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &instance_id, ORB_PRIO_DEFAULT); |
|
|
|
ORB_PRIO_DEFAULT); |
|
|
|
|
|
|
|
orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &instance_id, |
|
|
|
|
|
|
|
ORB_PRIO_DEFAULT); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void |
|
|
|
void |
|
|
|