From 30c65e57bef1f66095729593f029a15fcc18eeae Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 4 Jun 2020 15:54:29 +1000 Subject: [PATCH] SITL: remove use of Vector3 as function --- libraries/SITL/SIM_Calibration.cpp | 4 ++-- libraries/SITL/SIM_FlightAxis.cpp | 8 +++++--- libraries/SITL/SIM_Motor.cpp | 2 +- libraries/SITL/SIM_XPlane.cpp | 2 +- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/SITL/SIM_Calibration.cpp b/libraries/SITL/SIM_Calibration.cpp index 7c10ea21c9..1480cf5a44 100644 --- a/libraries/SITL/SIM_Calibration.cpp +++ b/libraries/SITL/SIM_Calibration.cpp @@ -45,7 +45,7 @@ void SITL::Calibration::update(const struct sitl_input &input) _angular_velocity_control(input, rot_accel); } - accel_body(0, 0, 0); + accel_body.zero(); update_dynamics(rot_accel); update_position(); time_advance(); @@ -172,6 +172,6 @@ void SITL::Calibration::_calibration_poses(Vector3f& rot_accel) r2.from_axis_angle(axis, rot_angle); dcm = r2 * dcm; - accel_body(0, 0, -GRAVITY_MSS); + accel_body = {0, 0, -GRAVITY_MSS}; accel_body = dcm.transposed() * accel_body; } diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 91f2bca42b..1a447b0058 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/libraries/SITL/SIM_FlightAxis.cpp @@ -428,9 +428,11 @@ void FlightAxis::update(const struct sitl_input &input) state.m_aircraftPositionX_MTR, -state.m_altitudeASL_MTR - home.alt*0.01); - accel_body(state.m_accelerationBodyAX_MPS2, - state.m_accelerationBodyAY_MPS2, - state.m_accelerationBodyAZ_MPS2); + accel_body = { + float(state.m_accelerationBodyAX_MPS2), + float(state.m_accelerationBodyAY_MPS2), + float(state.m_accelerationBodyAZ_MPS2) + }; // accel on the ground is nasty in realflight, and prevents helicopter disarm if (!is_zero(state.m_isTouchingGround)) { diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index bda0e0dbd8..31025d4028 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -39,7 +39,7 @@ void Motor::calculate_forces(const struct sitl_input &input, Vector3f rotor_torque(0, 0, yaw_factor * motor_speed * yaw_scale); // get thrust for untilted motor - thrust(0, 0, -motor_speed); + thrust = {0, 0, -motor_speed}; // define the arm position relative to center of mass Vector3f arm(arm_scale * cosf(radians(angle)), arm_scale * sinf(radians(angle)), 0); diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index 87cc6be037..1b07cbedeb 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -311,7 +311,7 @@ bool XPlane::receive_data(void) printf("X-Plane home reset dist=%f alt=%.1f/%.1f\n", loc.get_distance(location), loc.alt*0.01f, location.alt*0.01f); // reset home location - position_zero(-pos.x, -pos.y, -pos.z); + position_zero = {-pos.x, -pos.y, -pos.z}; home.lat = loc.lat; home.lng = loc.lng; home.alt = loc.alt;