From a89b69b0eadab3e24d374aba71deda3a056f446d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 11 Mar 2020 23:57:43 -0400 Subject: [PATCH] vehicle_global_position: remove velocity fields (duplicates of local vx, vy, vz) * attitude_estimator_q: get velocity from local position (if available) --- msg/vehicle_global_position.msg | 4 -- .../telemetry/frsky_telemetry/frsky_data.cpp | 7 ++- .../telemetry/frsky_telemetry/sPort_data.cpp | 8 +-- .../attitude_estimator_q_main.cpp | 35 +++++++----- src/modules/ekf2/ekf2_main.cpp | 4 -- .../FixedwingAttitudeControl.cpp | 57 +++++++++++++------ .../FixedwingAttitudeControl.hpp | 6 +- .../FixedwingPositionControl.cpp | 4 +- .../BlockLocalPositionEstimator.cpp | 3 - src/modules/mavlink/mavlink_high_latency2.cpp | 16 +++--- src/modules/mavlink/mavlink_high_latency2.h | 5 +- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 3 - src/modules/navigator/follow_target.cpp | 2 +- src/modules/navigator/mission_block.cpp | 10 ++-- src/modules/navigator/navigator_main.cpp | 2 +- .../RoverPositionControl.cpp | 2 +- src/modules/sih/sih.cpp | 3 - src/modules/simulator/simulator_mavlink.cpp | 4 -- 19 files changed, 98 insertions(+), 79 deletions(-) diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg index e000549683..6ddf7e5475 100644 --- a/msg/vehicle_global_position.msg +++ b/msg/vehicle_global_position.msg @@ -16,10 +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 vel_n # North velocity in NED earth-fixed frame, (metres/sec) -float32 vel_e # East velocity in NED earth-fixed frame, (metres/sec) -float32 vel_d # Down velocity in NED earth-fixed frame, (metres/sec) - float32 yaw # Euler yaw angle relative to NED earth-fixed frame, -PI..+PI, (radians) float32 eph # Standard deviation of horizontal position error, (metres) diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp b/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp index 6659b8b2f3..891c617fd7 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -68,6 +69,7 @@ struct frsky_subscription_data_s { uORB::SubscriptionData battery_status_sub{ORB_ID(battery_status)}; uORB::SubscriptionData sensor_combined_sub{ORB_ID(sensor_combined)}; uORB::SubscriptionData vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + uORB::SubscriptionData vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::SubscriptionData vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; @@ -149,6 +151,7 @@ void frsky_update_topics() subscription_data->battery_status_sub.update(); subscription_data->sensor_combined_sub.update(); subscription_data->vehicle_air_data_sub.update(); + subscription_data->vehicle_local_position_sub.update(); subscription_data->vehicle_global_position_sub.update(); subscription_data->vehicle_gps_position_sub.update(); subscription_data->vehicle_status_sub.update(); @@ -199,6 +202,7 @@ static float frsky_format_gps(float dec) void frsky_send_frame2(int uart) { const vehicle_global_position_s &gpos = subscription_data->vehicle_global_position_sub.get(); + const vehicle_local_position_s &lpos = subscription_data->vehicle_local_position_sub.get(); const battery_status_s &battery_status = subscription_data->battery_status_sub.get(); const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get(); @@ -218,8 +222,7 @@ void frsky_send_frame2(int uart) lat_ns = (gpos.lat < 0) ? 'S' : 'N'; lon = frsky_format_gps(fabsf(gpos.lon)); lon_ew = (gpos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(gpos.vel_n * gpos.vel_n + gpos.vel_e * gpos.vel_e) - * 25.0f / 46.0f; + speed = sqrtf(lpos.vx * lpos.vx + lpos.vy * lpos.vy) * 25.0f / 46.0f; alt = gpos.alt; } diff --git a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp index a0247991b8..db149f9c32 100644 --- a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp @@ -204,10 +204,10 @@ void sPort_send_ALT(int uart) // verified scaling for "calculated" option void sPort_send_SPD(int uart) { - const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get(); + const vehicle_local_position_s &local_pos = s_port_subscription_data->vehicle_local_position_sub.get(); /* send data for A2 */ - float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); + float speed = sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy); uint32_t ispeed = (int)(10 * speed); sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); } @@ -297,10 +297,10 @@ void sPort_send_GPS_TIME(int uart) void sPort_send_GPS_SPD(int uart) { - const vehicle_global_position_s &global_pos = s_port_subscription_data->vehicle_global_position_sub.get(); + const vehicle_local_position_s &local_pos = s_port_subscription_data->vehicle_local_position_sub.get(); /* send 100 * knots */ - float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); + float speed = sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy); uint32_t ispeed = (int)(1944 * speed); sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); } diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 4af7b511f0..aea02511ff 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -58,7 +58,8 @@ #include #include #include -#include +#include +#include #include #include @@ -109,7 +110,8 @@ private: uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vision_odom_sub{ORB_ID(vehicle_visual_odometry)}; uORB::Subscription _mocap_odom_sub{ORB_ID(vehicle_mocap_odometry)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; @@ -307,29 +309,36 @@ AttitudeEstimatorQ::Run() } } - if (_global_pos_sub.updated()) { - vehicle_global_position_s gpos; + if (_gps_sub.updated()) { + vehicle_gps_position_s gps; - if (_global_pos_sub.copy(&gpos)) { - if (_param_att_mag_decl_a.get() && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1_s) { + if (_gps_sub.copy(&gps)) { + if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) { /* set magnetic declination automatically */ - update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon))); + update_mag_declination(math::radians(get_mag_declination(gps.lat, gps.lon))); } + } + } + + if (_local_position_sub.updated()) { + vehicle_local_position_s lpos; + + if (_local_position_sub.copy(&lpos)) { - if (_param_att_acc_comp.get() && gpos.timestamp != 0 && hrt_absolute_time() < gpos.timestamp + 20000 && gpos.eph < 5.0f - && _inited) { + if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms) + && lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) { /* position data is actual */ - Vector3f vel(gpos.vel_n, gpos.vel_e, gpos.vel_d); + const Vector3f vel(lpos.vx, lpos.vy, lpos.vz); /* velocity updated */ - if (_vel_prev_t != 0 && gpos.timestamp != _vel_prev_t) { - float vel_dt = (gpos.timestamp - _vel_prev_t) / 1e6f; + if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) { + float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f; /* calculate acceleration in body frame */ _pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt); } - _vel_prev_t = gpos.timestamp; + _vel_prev_t = lpos.timestamp; _vel_prev = vel; } else { diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index efe92e4a13..1a173a175e 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1432,10 +1432,6 @@ void Ekf2::Run() // global altitude has opposite sign of local down position global_pos.delta_alt = -lpos.delta_z; - global_pos.vel_n = lpos.vx; // Ground north velocity, m/s - global_pos.vel_e = lpos.vy; // Ground east velocity, m/s - global_pos.vel_d = lpos.vz; // Ground downside velocity, m/s - global_pos.yaw = lpos.yaw; // Yaw in radians -PI..+PI. _ekf.get_ekf_gpos_accuracy(&global_pos.eph, &global_pos.epv); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 50162d50c1..38846902c7 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -361,13 +361,19 @@ void FixedwingAttitudeControl::Run() vehicle_control_mode_poll(); vehicle_manual_poll(); - _global_pos_sub.update(&_global_pos); vehicle_land_detected_poll(); // the position controller will not emit attitude setpoints in some modes // we need to make sure that this flag is reset _att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled; + bool wheel_control = false; + + // TODO: manual wheel_control on ground? + if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) { + wheel_control = true; + } + /* lock integrator until control is started */ bool lock_integrator = !_vcontrol_mode.flag_control_rates_enabled || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && ! _vehicle_status.in_transition_mode); @@ -393,14 +399,6 @@ void FixedwingAttitudeControl::Run() const float airspeed = get_airspeed_and_update_scaling(); - /* Use min airspeed to calculate ground speed scaling region. - * Don't scale below gspd_scaling_trim - */ - float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n + - _global_pos.vel_e * _global_pos.vel_e); - float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f); - float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); - /* reset integrals where needed */ if (_att_sp.roll_reset_integral) { _roll_ctrl.reset_integrator(); @@ -444,8 +442,20 @@ void FixedwingAttitudeControl::Run() control_input.airspeed = airspeed; control_input.scaler = _airspeed_scaling; control_input.lock_integrator = lock_integrator; - control_input.groundspeed = groundspeed; - control_input.groundspeed_scaler = groundspeed_scaler; + + if (wheel_control) { + _local_pos_sub.update(&_local_pos); + + /* Use min airspeed to calculate ground speed scaling region. + * Don't scale below gspd_scaling_trim + */ + float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy); + float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f); + float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed); + + control_input.groundspeed = groundspeed; + control_input.groundspeed_scaler = groundspeed_scaler; + } /* reset body angular rate limits on mode change */ if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { @@ -497,8 +507,16 @@ void FixedwingAttitudeControl::Run() if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { _roll_ctrl.control_attitude(control_input); _pitch_ctrl.control_attitude(control_input); - _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude - _wheel_ctrl.control_attitude(control_input); + + if (wheel_control) { + _wheel_ctrl.control_attitude(control_input); + _yaw_ctrl.reset_integrator(); + + } else { + // runs last, because is depending on output of roll and pitch attitude + _yaw_ctrl.control_attitude(control_input); + _wheel_ctrl.reset_integrator(); + } /* Update input data for rate controllers */ control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); @@ -522,7 +540,7 @@ void FixedwingAttitudeControl::Run() float yaw_u = 0.0f; - if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) { + if (wheel_control) { yaw_u = _wheel_ctrl.control_bodyrate(control_input); } else { @@ -595,12 +613,17 @@ void FixedwingAttitudeControl::Run() _rates_sp.thrust_body[0] : 0.0f; } - rate_ctrl_status_s rate_ctrl_status; + rate_ctrl_status_s rate_ctrl_status{}; rate_ctrl_status.timestamp = hrt_absolute_time(); rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); - rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); - rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); + + if (wheel_control) { + rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); + + } else { + rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); + } _rate_ctrl_status_pub.publish(rate_ctrl_status); } diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index b825ab0815..6d9d693077 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -62,7 +62,7 @@ #include #include #include -#include +#include #include #include #include @@ -97,7 +97,7 @@ private: uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */ uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ - uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */ uORB::Subscription _manual_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */ uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */ uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */ @@ -120,7 +120,7 @@ private: vehicle_attitude_s _att {}; /**< vehicle attitude setpoint */ vehicle_attitude_setpoint_s _att_sp {}; /**< vehicle attitude setpoint */ vehicle_control_mode_s _vcontrol_mode {}; /**< vehicle control mode */ - vehicle_global_position_s _global_pos {}; /**< global position */ + vehicle_local_position_s _local_pos {}; /**< local position */ vehicle_rates_setpoint_s _rates_sp {}; /* attitude rates setpoint */ vehicle_status_s _vehicle_status {}; /**< vehicle status */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ca02445d49..0f074e32f9 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1369,7 +1369,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector _land_noreturn_vertical = true; } else { - if (_global_pos.vel_d > 0.1f) { + if (_local_pos.vz > 0.1f) { _flare_pitch_sp = radians(_param_fw_lnd_fl_pmin.get()) * constrain((_flare_height - (_global_pos.alt - terrain_alt)) / _flare_height, 0.0f, 1.0f); } @@ -1525,7 +1525,7 @@ FixedwingPositionControl::Run() _vehicle_rates_sub.update(); Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); - Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e); + Vector2f ground_speed(_local_pos.vx, _local_pos.vy); //Convert Local setpoints to global setpoints if (_control_mode.flag_control_offboard_enabled) { diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 1fa9a9c7f3..b16d35f126 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -803,9 +803,6 @@ void BlockLocalPositionEstimator::publishGlobalPos() _pub_gpos.get().lat = lat; _pub_gpos.get().lon = lon; _pub_gpos.get().alt = alt; - _pub_gpos.get().vel_n = xLP(X_vx); - _pub_gpos.get().vel_e = xLP(X_vy); - _pub_gpos.get().vel_d = xLP(X_vz); _pub_gpos.get().yaw = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); _pub_gpos.get().eph = eph; _pub_gpos.get().epv = epv; diff --git a/src/modules/mavlink/mavlink_high_latency2.cpp b/src/modules/mavlink/mavlink_high_latency2.cpp index 7723f653b9..abe12b1321 100644 --- a/src/modules/mavlink/mavlink_high_latency2.cpp +++ b/src/modules/mavlink/mavlink_high_latency2.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -76,6 +77,8 @@ MavlinkStreamHighLatency2::MavlinkStreamHighLatency2(Mavlink *mavlink) : Mavlink _pos_ctrl_status_time(0), _geofence_sub(_mavlink->add_orb_subscription(ORB_ID(geofence_result))), _geofence_time(0), + _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), + _local_pos_time(0), _global_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position))), _global_pos_time(0), _gps_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position))), @@ -503,7 +506,7 @@ void MavlinkStreamHighLatency2::update_data() update_battery_status(); - update_global_position(); + update_local_position(); update_gps(); @@ -543,14 +546,13 @@ void MavlinkStreamHighLatency2::update_battery_status() } } -void MavlinkStreamHighLatency2::update_global_position() +void MavlinkStreamHighLatency2::update_local_position() { - vehicle_global_position_s global_pos; + vehicle_local_position_s local_pos; - if (_global_pos_sub->update(&global_pos)) { - _climb_rate.add_value(fabsf(global_pos.vel_d), _update_rate_filtered); - _groundspeed.add_value(sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e), - _update_rate_filtered); + if (_local_pos_sub->update(&local_pos)) { + _climb_rate.add_value(fabsf(local_pos.vz), _update_rate_filtered); + _groundspeed.add_value(sqrtf(local_pos.vx * local_pos.vx + local_pos.vy * local_pos.vy), _update_rate_filtered); } } diff --git a/src/modules/mavlink/mavlink_high_latency2.h b/src/modules/mavlink/mavlink_high_latency2.h index 539b200231..da3ecb4b3b 100644 --- a/src/modules/mavlink/mavlink_high_latency2.h +++ b/src/modules/mavlink/mavlink_high_latency2.h @@ -113,6 +113,9 @@ private: MavlinkOrbSubscription *_geofence_sub; uint64_t _geofence_time; + MavlinkOrbSubscription *_local_pos_sub; + uint64_t _local_pos_time; + MavlinkOrbSubscription *_global_pos_sub; uint64_t _global_pos_time; @@ -193,7 +196,7 @@ protected: void update_battery_status(); - void update_global_position(); + void update_local_position(); void update_gps(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index c6d4258836..6665e2b19c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -2067,7 +2067,7 @@ public: private: MavlinkOrbSubscription *_local_pos_sub; uint64_t _local_pos_time = 0; - vehicle_local_position_s _local_position = {}; + vehicle_local_position_s _local_position = {}; MavlinkOrbSubscription *_global_pos_sub; uint64_t _global_pos_time = 0; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d98e3c5ae8..62ffd47404 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2492,9 +2492,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat / ((double)1e7); hil_global_pos.lon = hil_state.lon / ((double)1e7); hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vel_n = hil_state.vx / 100.0f; - hil_global_pos.vel_e = hil_state.vy / 100.0f; - hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.eph = 2.0f; hil_global_pos.epv = 4.0f; diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp index 81cb783fd7..18c0f8fa28 100644 --- a/src/modules/navigator/follow_target.cpp +++ b/src/modules/navigator/follow_target.cpp @@ -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_global_position()->yaw)) < math::radians(3.0F)) { + if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->yaw)) < math::radians(3.0F)) { _yaw_rate = NAN; } } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 8b1883624e..2527c941a7 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -336,10 +336,10 @@ 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_global_position()->yaw : + _navigator->get_local_position()->yaw : atan2f( - _navigator->get_global_position()->vel_e, - _navigator->get_global_position()->vel_n + _navigator->get_local_position()->vy, + _navigator->get_local_position()->vx ); float yaw_err = wrap_pi(_mission_item.yaw - cog); @@ -642,7 +642,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_global_position()->yaw; + item->yaw = _navigator->get_local_position()->yaw; item->altitude = abs_altitude; item->altitude_is_relative = false; @@ -711,7 +711,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_global_position()->yaw; + item->yaw = _navigator->get_local_position()->yaw; item->autocontinue = true; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c212844ea7..0427bfa41a 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -305,7 +305,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_global_position()->yaw; + rep->previous.yaw = get_local_position()->yaw; rep->previous.lat = get_global_position()->lat; rep->previous.lon = get_global_position()->lon; rep->previous.alt = get_global_position()->alt; diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index a016bf69b5..4aff905498 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -433,7 +433,7 @@ RoverPositionControl::run() // update the reset counters in any case _pos_reset_counter = _global_pos.lat_lon_reset_counter; - matrix::Vector3f ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); + matrix::Vector3f ground_speed(_local_pos.vx, _local_pos.vy, _local_pos.vz); matrix::Vector2f current_position((float)_global_pos.lat, (float)_global_pos.lon); matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz); diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index db0a9322e2..e12dc48304 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -391,9 +391,6 @@ void Sih::publish_sih() _gpos_gt.lat = _gps_lat_noiseless; _gpos_gt.lon = _gps_lon_noiseless; _gpos_gt.alt = _gps_alt_noiseless; - _gpos_gt.vel_n = _v_I(0); - _gpos_gt.vel_e = _v_I(1); - _gpos_gt.vel_d = _v_I(2); _gpos_gt_pub.publish(_gpos_gt); } diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index be3dfbe3f2..267c67c281 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -421,10 +421,6 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg hil_gpos.lon = hil_state.lon / 1E7;//1E7 hil_gpos.alt = hil_state.alt / 1E3;//1E3 - hil_gpos.vel_n = hil_state.vx / 100.0f; - hil_gpos.vel_e = hil_state.vy / 100.0f; - hil_gpos.vel_d = hil_state.vz / 100.0f; - // always publish ground truth attitude message _gpos_ground_truth_pub.publish(hil_gpos); }