dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

vehicle_local_position: rename yaw -> heading and add reset logic 

- vehicle_global_position yaw removed (redundant)
This commit is contained in:
Daniel Agar 2020-08-07 14:06:38 -04:00 committed by Mathieu Bresciani
parent 33fb9d0c62
commit 97fc1db768
24 changed files with 75 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_global_position.msg
View File

@ -16,8 +16,6 @@ float32 delta_alt # Reset delta for altitude
uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates
uint8 alt_reset_counter # Counter for reset events on altitude
float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians)
float32 eph # Standard deviation of horizontal position error, (metres)
float32 epv # Standard deviation of vertical position error, (metres)

5
msg/vehicle_local_position.msg
View File

@ -36,8 +36,9 @@ float32 ax # North velocity derivative in NED earth-fixed frame, (metres/
float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2)
float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2)
# Heading
float32 yaw # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians)
float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians)
float32 delta_heading
uint8 heading_reset_counter
# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame
bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon)

2
src/drivers/telemetry/frsky_telemetry/frsky_data.cpp
View File

@ -212,7 +212,7 @@ void frsky_send_frame2(int uart)
int sec = 0;
if (gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000) {
course = gpos.yaw / M_PI_F * 180.0f;
course = lpos.heading / M_PI_F * 180.0f;
if (course < 0.f) { // course is in range [0, 360], 0=north, CW
course += 360.f;

2
src/drivers/telemetry/frsky_telemetry/sPort_data.cpp
View File

@ -265,7 +265,7 @@ void sPort_send_GPS_CRS(int uart)
/* send course */
/* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */
int32_t iYaw = s_port_subscription_data->vehicle_local_position_sub.get().yaw * 18000.0f / M_PI_F;
int32_t iYaw = s_port_subscription_data->vehicle_local_position_sub.get().heading * 18000.0f / M_PI_F;
if (iYaw < 0) { iYaw += 36000; }

16
src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
View File

@ -32,7 +32,6 @@ bool FlightTask::updateInitialize()
_time_stamp_last = _time_stamp_current;
_sub_vehicle_local_position.update();
_sub_attitude.update();
_sub_home_position.update();
_evaluateVehicleLocalPosition();
@ -69,10 +68,9 @@ void FlightTask::_checkEkfResetCounters()
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
}
if (_sub_attitude.get().quat_reset_counter != _reset_counters.quat) {
float delta_psi = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().delta_q_reset)).psi();
_ekfResetHandlerHeading(delta_psi);
_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
if (_sub_vehicle_local_position.get().heading_reset_counter != _reset_counters.heading) {
_ekfResetHandlerHeading(_sub_vehicle_local_position.get().delta_heading);
_reset_counters.heading = _sub_vehicle_local_position.get().heading_reset_counter;
}
}
@ -117,14 +115,12 @@ void FlightTask::_evaluateVehicleLocalPosition()
_yaw = NAN;
_dist_to_bottom = NAN;
if ((_time_stamp_current - _sub_attitude.get().timestamp) < _timeout) {
// yaw
_yaw = matrix::Eulerf(matrix::Quatf(_sub_attitude.get().q)).psi();
}
// Only use vehicle-local-position topic fields if the topic is received within a certain timestamp
if ((_time_stamp_current - _sub_vehicle_local_position.get().timestamp) < _timeout) {
// yaw
_yaw = _sub_vehicle_local_position.get().heading;
// position
if (_sub_vehicle_local_position.get().xy_valid) {
_position(0) = _sub_vehicle_local_position.get().x;

3
src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp
View File

@ -60,7 +60,7 @@ struct ekf_reset_counters_s {
uint8_t vxy;
uint8_t z;
uint8_t vz;
uint8_t quat;
uint8_t heading;
};
class FlightTask : public ModuleParams
@ -189,7 +189,6 @@ public:
protected:
uORB::SubscriptionData<vehicle_local_position_s> _sub_vehicle_local_position{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_attitude_s> _sub_attitude{ORB_ID(vehicle_attitude)};
uORB::SubscriptionData<home_position_s> _sub_home_position{ORB_ID(home_position)};
/** Reset all setpoints to NAN */

32
src/modules/commander/Commander.cpp
View File

@ -1199,7 +1199,8 @@ Commander::set_home_position()
home.y = lpos.y;
home.z = lpos.z;
home.yaw = lpos.yaw;
home.yaw = lpos.heading;
_heading_reset_counter = lpos.heading_reset_counter;
home.manual_home = false;
@ -1248,20 +1249,6 @@ Commander::updateHomePositionYaw(float yaw)
_home_pub.update(home);
}
void
Commander::checkEkfResetCounters()
{
if (_attitude_sub.get().quat_reset_counter != _quat_reset_counter) {
const float delta_psi = matrix::Eulerf(matrix::Quatf(_attitude_sub.get().delta_q_reset)).psi();
if (!_home_pub.get().manual_home) {
updateHomePositionYaw(matrix::wrap_pi(_home_pub.get().yaw + delta_psi));
}
_quat_reset_counter = _attitude_sub.get().quat_reset_counter;
}
}
void
Commander::run()
{
@ -1629,7 +1616,7 @@ Commander::run()
}
}
estimator_check();
estimator_check(status_flags);
/* Update land detector */
if (_land_detector_sub.updated()) {
@ -2279,8 +2266,6 @@ Commander::run()
/* Get current timestamp */
const hrt_abstime now = hrt_absolute_time();
checkEkfResetCounters();
// automatically set or update home position
if (!_home_pub.get().manual_home) {
const vehicle_local_position_s &local_position = _local_position_sub.get();
@ -4048,18 +4033,25 @@ void Commander::battery_status_check()
}
}
void Commander::estimator_check()
void Commander::estimator_check(const vehicle_status_flags_s &vstatus_flags)
{
// Check if quality checking of position accuracy and consistency is to be performed
const bool run_quality_checks = !status_flags.circuit_breaker_engaged_posfailure_check;
_local_position_sub.update();
_global_position_sub.update();
_attitude_sub.update();
const vehicle_local_position_s &lpos = _local_position_sub.get();
const vehicle_global_position_s &gpos = _global_position_sub.get();
if (lpos.heading_reset_counter != _heading_reset_counter) {
if (vstatus_flags.condition_home_position_valid) {
updateHomePositionYaw(_home_pub.get().yaw + lpos.delta_heading);
}
_heading_reset_counter = lpos.heading_reset_counter;
}
const bool mag_fault_prev = (_estimator_status_sub.get().control_mode_flags & (1 << estimator_status_s::CS_MAG_FAULT));
if (_estimator_status_sub.update()) {

6
src/modules/commander/Commander.hpp
View File

@ -137,7 +137,7 @@ private:
void esc_status_check(const esc_status_s &esc_status);
void estimator_check();
void estimator_check(const vehicle_status_flags_s &status_flags);
bool handle_command(vehicle_status_s *status, const vehicle_command_s &cmd, actuator_armed_s *armed,
uORB::PublicationQueued<vehicle_command_ack_s> &command_ack_pub);
@ -156,7 +156,6 @@ private:
bool set_home_position();
bool set_home_position_alt_only();
void updateHomePositionYaw(float yaw);
void checkEkfResetCounters();
void update_control_mode();
@ -362,7 +361,7 @@ private:
hrt_abstime _timestamp_engine_healthy{0}; ///< absolute time when engine was healty
uint32_t _counter{0};
uint8_t _quat_reset_counter{0};
uint8_t _heading_reset_counter{0};
bool _status_changed{true};
bool _arm_tune_played{false};
@ -423,7 +422,6 @@ private:
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_local_position_s> _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_attitude_s> _attitude_sub{ORB_ID(vehicle_attitude)};
// Publications
uORB::Publication<actuator_armed_s> _armed_pub{ORB_ID(actuator_armed)};

8
src/modules/ekf2/ekf2_main.cpp
View File

@ -1305,7 +1305,11 @@ void Ekf2::Run()
// The rotation of the tangent plane vs. geographical north
const matrix::Quatf q = _ekf.getQuaternion();
lpos.yaw = matrix::Eulerf(q).psi();
matrix::Quatf delta_q_reset;
_ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter);
lpos.heading = matrix::Eulerf(q).psi();
lpos.delta_heading = matrix::Eulerf(delta_q_reset).psi();
// Vehicle odometry quaternion
q.copyTo(odom.q);
@ -1479,8 +1483,6 @@ void Ekf2::Run()
// global altitude has opposite sign of local down position
global_pos.delta_alt = -lpos.delta_z;
global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI.
_ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv);
global_pos.terrain_alt_valid = lpos.dist_bottom_valid;

3
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -595,7 +595,7 @@ void BlockLocalPositionEstimator::publishLocalPos()
_pub_lpos.get().z = xLP(X_z); // down
}
_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
_pub_lpos.get().heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
_pub_lpos.get().vx = xLP(X_vx); // north
_pub_lpos.get().vy = xLP(X_vy); // east
@ -793,7 +793,6 @@ void BlockLocalPositionEstimator::publishGlobalPos()
_pub_gpos.get().lat = lat;
_pub_gpos.get().lon = lon;
_pub_gpos.get().alt = alt;
_pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi();
_pub_gpos.get().eph = eph;
_pub_gpos.get().epv = epv;
_pub_gpos.get().terrain_alt = _altOrigin - xLP(X_tz);

5
src/modules/mavlink/mavlink_high_latency2.cpp
View File

@ -294,8 +294,9 @@ bool MavlinkStreamHighLatency2::write_geofence_result(mavlink_high_latency2_t *m
bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *msg)
{
vehicle_global_position_s global_pos;
vehicle_local_position_s local_pos;
if (_global_pos_sub.update(&global_pos)) {
if (_global_pos_sub.update(&global_pos) && _local_pos_sub.update(&local_pos)) {
msg->latitude = global_pos.lat * 1e7;
msg->longitude = global_pos.lon * 1e7;
@ -310,7 +311,7 @@ bool MavlinkStreamHighLatency2::write_global_position(mavlink_high_latency2_t *m
msg->altitude = altitude;
msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(global_pos.yaw)) * 0.5f);
msg->heading = static_cast<uint8_t>(math::degrees(wrap_2pi(local_pos.heading)) * 0.5f);
return true;
}

6
src/modules/mavlink/mavlink_messages.cpp
View File

@ -1534,7 +1534,7 @@ protected:
mavlink_vfr_hud_t msg{};
msg.airspeed = airspeed_validated.equivalent_airspeed_m_s;
msg.groundspeed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy);
msg.heading = math::degrees(wrap_2pi(lpos.yaw));
msg.heading = math::degrees(wrap_2pi(lpos.heading));
if (armed.armed) {
actuator_controls_s act0{};
@ -2469,7 +2469,7 @@ protected:
msg.vy = lpos.vy * 100.0f;
msg.vz = lpos.vz * 100.0f;
msg.hdg = math::degrees(wrap_2pi(lpos.yaw)) * 100.0f;
msg.hdg = math::degrees(wrap_2pi(lpos.heading)) * 100.0f;
mavlink_msg_global_position_int_send_struct(_mavlink->get_channel(), &msg);
@ -4870,7 +4870,7 @@ protected:
vehicle_local_position_s lpos{};
_lpos_sub.copy(&lpos);
msg.yaw_absolute = math::degrees(matrix::wrap_2pi(lpos.yaw + mount_orientation.attitude_euler_angle[2]));
msg.yaw_absolute = math::degrees(matrix::wrap_2pi(lpos.heading + mount_orientation.attitude_euler_angle[2]));
mavlink_msg_mount_orientation_send_struct(_mavlink->get_channel(), &msg);

2
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -2600,7 +2600,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_local_pos.vz = hil_state.vz / 100.0f;
matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)};
hil_local_pos.yaw = euler.psi();
hil_local_pos.heading = euler.psi();
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
hil_local_pos.vxy_max = INFINITY;

14
src/modules/mc_pos_control/PositionControl/ControlMath.cpp
View File

@ -77,7 +77,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
body_z.normalize();
// vector of desired yaw direction in XY plane, rotated by PI/2
Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
const Vector3f y_C{-sinf(yaw_sp), cosf(yaw_sp), 0.f};
// desired body_x axis, orthogonal to body_z
Vector3f body_x = y_C % body_z;
@ -97,7 +97,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
body_x.normalize();
// desired body_y axis
Vector3f body_y = body_z % body_x;
const Vector3f body_y = body_z % body_x;
Dcmf R_sp;
@ -109,14 +109,14 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
}
// copy quaternion setpoint to attitude setpoint topic
Quatf q_sp = R_sp;
const Quatf q_sp{R_sp};
q_sp.copyTo(att_sp.q_d);
// calculate euler angles, for logging only, must not be used for control
Eulerf euler = R_sp;
att_sp.roll_body = euler(0);
att_sp.pitch_body = euler(1);
att_sp.yaw_body = euler(2);
const Eulerf euler{R_sp};
att_sp.roll_body = euler.phi();
att_sp.pitch_body = euler.theta();
att_sp.yaw_body = euler.psi();
}
Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

13
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -121,7 +121,6 @@ private:
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _att_sub{ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position */
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
@ -423,14 +422,6 @@ MulticopterPositionControl::poll_subscriptions()
_control_mode_sub.update(&_control_mode);
_home_pos_sub.update(&_home_pos);
if (_att_sub.updated()) {
vehicle_attitude_s att;
if (_att_sub.copy(&att) && PX4_ISFINITE(att.q[0])) {
_states.yaw = Eulerf(Quatf(att.q)).psi();
}
}
if (_param_mpc_use_hte.get()) {
hover_thrust_estimate_s hte;
@ -514,6 +505,10 @@ MulticopterPositionControl::set_vehicle_states(const float &vel_sp_z)
// reset derivative to prevent acceleration spikes when regaining velocity
_vel_z_deriv.reset();
}
if (PX4_ISFINITE(_local_pos.heading)) {
_states.yaw = _local_pos.heading;
}
}
int

4
src/modules/navigator/follow_target.cpp
View File

@ -196,7 +196,7 @@ void FollowTarget::on_active()
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0f));
_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
} else {
_yaw_angle = _yaw_rate = NAN;
@ -229,7 +229,7 @@ void FollowTarget::on_active()
// 3 degrees of facing target
if (PX4_ISFINITE(_yaw_rate)) {
if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->yaw)) < math::radians(3.0F)) {
if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) {
_yaw_rate = NAN;
}
}

2
src/modules/navigator/loiter.cpp
View File

@ -131,7 +131,7 @@ Loiter::reposition()
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.yaw = _navigator->get_global_position()->yaw;
pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading;
pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;

6
src/modules/navigator/mission.cpp
View File

@ -711,7 +711,7 @@ Mission::set_mission_items()
_mission_item.lat = _navigator->get_global_position()->lat;
_mission_item.lon = _navigator->get_global_position()->lon;
/* hold heading for takeoff items */
_mission_item.yaw = _navigator->get_global_position()->yaw;
_mission_item.yaw = _navigator->get_local_position()->heading;
_mission_item.altitude = takeoff_alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
@ -808,7 +808,7 @@ Mission::set_mission_items()
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
} else {
_mission_item.yaw = _navigator->get_global_position()->yaw;
_mission_item.yaw = _navigator->get_local_position()->heading;
/* set position setpoint to target during the transition */
generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
@ -1343,7 +1343,7 @@ Mission::heading_sp_update()
} else {
if (!pos_sp_triplet->current.yaw_valid) {
_mission_item.yaw = _navigator->get_local_position()->yaw;
_mission_item.yaw = _navigator->get_local_position()->heading;
pos_sp_triplet->current.yaw = _mission_item.yaw;
pos_sp_triplet->current.yaw_valid = true;
}

8
src/modules/navigator/mission_block.cpp
View File

@ -375,7 +375,7 @@ MissionBlock::is_mission_item_reached()
/* check course if defined only for rotary wing except takeoff */
float cog = (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ?
_navigator->get_local_position()->yaw :
_navigator->get_local_position()->heading :
atan2f(
_navigator->get_local_position()->vy,
_navigator->get_local_position()->vx
@ -690,7 +690,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude,
/* use current position */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->yaw = _navigator->get_local_position()->yaw;
item->yaw = _navigator->get_local_position()->heading;
item->altitude = abs_altitude;
item->altitude_is_relative = false;
@ -720,7 +720,7 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
if (at_current_location) {
item->lat = (double)NAN; //descend at current position
item->lon = (double)NAN; //descend at current position
item->yaw = _navigator->get_local_position()->yaw;
item->yaw = _navigator->get_local_position()->heading;
} else {
/* use home position */
@ -759,7 +759,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
{
item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
item->params[0] = (float) new_mode;
item->yaw = _navigator->get_local_position()->yaw;
item->yaw = _navigator->get_local_position()->heading;
item->autocontinue = true;
}

8
src/modules/navigator/navigator_main.cpp
View File

@ -262,7 +262,7 @@ Navigator::run()
position_setpoint_triplet_s *curr = get_position_setpoint_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = get_global_position()->yaw;
rep->previous.yaw = get_local_position()->heading;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
@ -345,7 +345,7 @@ Navigator::run()
position_setpoint_triplet_s *rep = get_takeoff_triplet();
// store current position as previous position and goal as next
rep->previous.yaw = get_local_position()->yaw;
rep->previous.yaw = get_local_position()->heading;
rep->previous.lat = get_global_position()->lat;
rep->previous.lon = get_global_position()->lon;
rep->previous.alt = get_global_position()->alt;
@ -361,7 +361,7 @@ Navigator::run()
rep->previous.timestamp = hrt_absolute_time();
} else {
rep->current.yaw = get_local_position()->yaw;
rep->current.yaw = get_local_position()->heading;
rep->previous.valid = false;
}
@ -505,7 +505,7 @@ Navigator::run()
&& get_vstatus()->nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
position_setpoint_triplet_s *rep = get_reposition_triplet();
rep->current.yaw = get_global_position()->yaw;
rep->current.yaw = get_local_position()->heading;
rep->current.lat = get_global_position()->lat;
rep->current.lon = get_global_position()->lon;
rep->current.alt = get_global_position()->alt;

2
src/modules/navigator/rtl.cpp
View File

@ -283,7 +283,7 @@ void RTL::set_rtl_item()
_mission_item.lon = gpos.lon;
_mission_item.altitude = _rtl_alt;
_mission_item.altitude_is_relative = false;
_mission_item.yaw = _navigator->get_local_position()->yaw;
_mission_item.yaw = _navigator->get_local_position()->heading;
_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
_mission_item.time_inside = 0.0f;
_mission_item.autocontinue = true;

2
src/modules/simulator/simulator_mavlink.cpp
View File

@ -460,7 +460,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
hil_lpos.vy = hil_state.vy / 100.0f;
hil_lpos.vz = hil_state.vz / 100.0f;
matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
hil_lpos.yaw = euler.psi();
hil_lpos.heading = euler.psi();
hil_lpos.xy_global = true;
hil_lpos.z_global = true;
hil_lpos.ref_lat = _hil_ref_lat;

8
src/modules/vmount/output.cpp
View File

@ -132,14 +132,20 @@ void OutputBase::_handle_position_update(bool force_update)
}
vehicle_global_position_s vehicle_global_position{};
vehicle_local_position_s vehicle_local_position{};
if (force_update) {
_vehicle_global_position_sub.copy(&vehicle_global_position);
_vehicle_local_position_sub.copy(&vehicle_local_position);
} else {
if (!_vehicle_global_position_sub.update(&vehicle_global_position)) {
return;
}
if (!_vehicle_local_position_sub.update(&vehicle_local_position)) {
return;
}
}
const double &vlat = vehicle_global_position.lat;
@ -159,7 +165,7 @@ void OutputBase::_handle_position_update(bool force_update)
_angle_setpoints[1] = _calculate_pitch(lon, lat, alt, vehicle_global_position);
}
_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_global_position.yaw;
_angle_setpoints[2] = get_bearing_to_next_waypoint(vlat, vlon, lat, lon) - vehicle_local_position.heading;
// add offsets from VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET
_angle_setpoints[1] += _cur_control_data->type_data.lonlat.pitch_angle_offset;

2
src/modules/vmount/output.h
View File

@ -47,6 +47,7 @@
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
namespace vmount
{
@ -124,6 +125,7 @@ protected:
private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
};