@@ -149,11 +149,17 @@ void Wiimote::SendAck(OutputReportID rpt_id, ErrorCode error_code)

void Wiimote::HandleExtensionSwap()
{
if (WIIMOTE_BALANCE_BOARD == m_index)
{
// Prevent M+ or anything else silly from being attached to a balance board.
// In the future if we support an emulated balance board we can force the BB "extension" here.
return;
}

ExtensionNumber desired_extension_number =
static_cast<ExtensionNumber>(m_attachments->GetSelectedAttachment());

// const bool desired_motion_plus = m_motion_plus_setting->GetValue();
const bool desired_motion_plus = false;
const bool desired_motion_plus = m_motion_plus_setting.GetValue();

// FYI: AttachExtension also connects devices to the i2c bus

@@ -283,7 +289,7 @@ void Wiimote::HandleWriteData(const OutputReportWriteData& wd)
if (address >= 0x0FCA && address < 0x12C0)
{
// TODO: Only write parts of the Mii block.
// TODO: Use fifferent files for different wiimote numbers.
// TODO: Use different files for different wiimote numbers.
std::ofstream file;
File::OpenFStream(file, File::GetUserPath(D_SESSION_WIIROOT_IDX) + "/mii.bin",
std::ios::binary | std::ios::out);
@@ -578,12 +584,16 @@ void Wiimote::DoState(PointerWrap& p)
(m_is_motion_plus_attached ? m_motion_plus.GetExtPort() : m_extension_port)
.AttachExtension(GetActiveExtension());

m_motion_plus.DoState(p);
GetActiveExtension()->DoState(p);
if (m_is_motion_plus_attached)
m_motion_plus.DoState(p);

if (m_active_extension != ExtensionNumber::NONE)
GetActiveExtension()->DoState(p);

// Dynamics
p.Do(m_swing_state);
p.Do(m_tilt_state);
p.Do(m_cursor_state);
p.Do(m_shake_state);

p.DoMarker("Wiimote");
@@ -91,8 +91,7 @@ void Nunchuk::Update()
EmulateTilt(&m_tilt_state, m_tilt, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateShake(&m_shake_state, m_shake, 1.f / ::Wiimote::UPDATE_FREQ);

const auto transformation =
GetRotationalMatrix(-m_tilt_state.angle) * GetRotationalMatrix(-m_swing_state.angle);
const auto transformation = GetRotationalMatrix(-m_tilt_state.angle - m_swing_state.angle);

Common::Vec3 accel = transformation * (m_swing_state.acceleration +
Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)));

Large diffs are not rendered by default.

@@ -7,11 +7,14 @@
#include <array>

#include "Common/CommonTypes.h"
#include "Core/HW/WiimoteEmu/Dynamics.h"
#include "Core/HW/WiimoteEmu/ExtensionPort.h"
#include "Core/HW/WiimoteEmu/I2CBus.h"

namespace WiimoteEmu
{
struct AngularVelocity;

struct MotionPlus : public Extension
{
public:
@@ -23,7 +26,37 @@ struct MotionPlus : public Extension

ExtensionPort& GetExtPort();

// Vec3 is interpreted as radians/s about the x,y,z axes following the "right-hand rule".
void PrepareInput(const Common::Vec3& angular_velocity);

private:
enum class ChallengeState : u8
{
// Note: This is not a value seen on a real M+.
// Used to emulate activation state during which the M+ is not responsive.
Activating = 0x00,

PreparingX = 0x02,
ParameterXReady = 0x0e,
PreparingY = 0x14,
ParameterYReady = 0x1a,
};

enum class PassthroughMode : u8
{
Disabled = 0x04,
Nunchuk = 0x05,
Classic = 0x07,
};

enum class ActivationStatus
{
Inactive,
Activating,
Deactivating,
Active,
};

#pragma pack(push, 1)
struct DataFormat
{
@@ -49,83 +82,95 @@ struct MotionPlus : public Extension

struct Register
{
u8 controller_data[21];
std::array<u8, 21> controller_data;
u8 unknown_0x15[11];

// address 0x20
u8 calibration_data[0x20];
std::array<u8, 0x20> calibration_data;

u8 unknown_0x40[0x10];
// address 0x40
// Data is read from the extension on the passthrough port.
std::array<u8, 0x10> passthrough_ext_calib;

// address 0x50
u8 cert_data[0x40];
std::array<u8, 0x40> challenge_data;

u8 unknown_0x90[0x60];

// address 0xF0
u8 initialized;
// Writes initialize the M+ to it's default (non-activated) state.
// Used to deactivate the M+ and activate an attached extension.
u8 init_trigger;

// address 0xF1
u8 cert_enable;

// Conduit 2 writes 1 byte to 0xf2 on calibration screen
u8 unknown_0xf2[5];

// address 0xf7
// Wii Sports Resort reads regularly
// Value starts at 0x00 and goes up after activation (not initialization)
// Immediately returns 0x02, even still after 15 and 30 seconds
// After the first data read the value seems to progress to 0x4,0x8,0xc,0xe
// More typical seems to be 2,8,c,e
// A value of 0xe triggers the game to read 64 bytes from 0x50
// The game claims M+ is disconnected after this read of unsatisfactory data
u8 cert_ready;

u8 unknown_0xf8[2];
// Value is either 0 or 1.
u8 challenge_type;

// address 0xF2
// Games write 0x00 here to start and stop calibration.
u8 calibration_trigger;

// address 0xF3
u8 unknown_0xf3[3];

// address 0xF6
// Value is taken from the extension on the passthrough port.
u8 passthrough_ext_id_4;

// address 0xF7
// Games read this value to know when the data at 0x50 is ready.
// Value is 0x02 upon activation. (via a write to 0xfe)
// Real M+ changes this value to 0x4, 0x8, 0xc, and finally 0xe.
// Games then trigger a 2nd stage via a write to 0xf1.
// Real M+ changes this value to 0x14, 0x18, and finally 0x1a.
// Note: We don't progress like this. We jump to the final value as soon as possible.
ChallengeState challenge_state;

// address 0xF8
// Values are taken from the extension on the passthrough port.
u8 passthrough_ext_id_0;
u8 passthrough_ext_id_5;

// address 0xFA
u8 ext_identifier[6];
std::array<u8, 6> ext_identifier;
};
#pragma pack(pop)

static_assert(sizeof(DataFormat) == 6, "Wrong size");
static_assert(0x100 == sizeof(Register));
static_assert(0x100 == sizeof(Register), "Wrong size");

static const u8 INACTIVE_DEVICE_ADDR = 0x53;
static const u8 ACTIVE_DEVICE_ADDR = 0x52;
static constexpr u8 INACTIVE_DEVICE_ADDR = 0x53;
static constexpr u8 ACTIVE_DEVICE_ADDR = 0x52;

enum class PassthroughMode : u8
{
Disabled = 0x04,
Nunchuk = 0x05,
Classic = 0x07,
};
static constexpr u8 PASSTHROUGH_MODE_OFFSET = 0xfe;

bool IsActive() const;
static constexpr int CALIBRATION_BITS = 16;

PassthroughMode GetPassthroughMode() const;
static constexpr u16 CALIBRATION_ZERO = 1 << (CALIBRATION_BITS - 1);
// Values are similar to that of a typical real M+.
static constexpr u16 CALIBRATION_SCALE_OFFSET = 0x4400;
static constexpr u16 CALIBRATION_FAST_SCALE_DEGREES = 0x4b0;
static constexpr u16 CALIBRATION_SLOW_SCALE_DEGREES = 0x10e;

// TODO: when activated it seems the motion plus reactivates the extension
// It sends 0x55 to 0xf0
// It also writes 0x00 to slave:0x52 addr:0xfa for some reason
// And starts a write to 0xfa but never writes bytes..
// It tries to read data at 0x00 for 3 times (failing)
// then it reads the 16 bytes of calibration at 0x20 and stops
void Activate();
void Deactivate();
void OnPassthroughModeWrite();

// TODO: if an extension is attached after activation, it also does this.
ActivationStatus GetActivationStatus() const;
PassthroughMode GetPassthroughMode() const;

int BusRead(u8 slave_addr, u8 addr, int count, u8* data_out) override;
int BusWrite(u8 slave_addr, u8 addr, int count, const u8* data_in) override;

bool ReadDeviceDetectPin() const override;
bool IsButtonPressed() const override;

// TODO: rename m_
Register m_reg_data = {};

Register reg_data = {};
// Used for timing of activation, deactivation, and preparation of challenge values.
u8 m_progress_timer = {};

// The port on the end of the motion plus:
I2CBus i2c_bus;
ExtensionPort m_extension_port{&i2c_bus};
I2CBus m_i2c_bus;
ExtensionPort m_extension_port{&m_i2c_bus};
};
} // namespace WiimoteEmu
@@ -93,6 +93,7 @@ void Wiimote::Reset()
m_eeprom.accel_calibration_1 = accel_calibration;
m_eeprom.accel_calibration_2 = accel_calibration;

// TODO: Is this needed?
// Data of unknown purpose:
constexpr std::array<u8, 24> EEPROM_DATA_16D0 = {0x00, 0x00, 0x00, 0xFF, 0x11, 0xEE, 0x00, 0x00,
0x33, 0xCC, 0x44, 0xBB, 0x00, 0x00, 0x66, 0x99,
@@ -106,29 +107,29 @@ void Wiimote::Reset()
m_i2c_bus.AddSlave(&m_speaker_logic);
m_i2c_bus.AddSlave(&m_camera_logic);

// Reset extension connections:
// Reset extension connections to NONE:
m_is_motion_plus_attached = false;
m_active_extension = ExtensionNumber::NONE;
m_extension_port.AttachExtension(GetNoneExtension());
m_motion_plus.GetExtPort().AttachExtension(GetNoneExtension());

// Switch to desired M+ status and extension (if any).
// M+ and EXT are reset on attachment.
HandleExtensionSwap();

// Reset sub-devices:
// Reset sub-devices.
m_speaker_logic.Reset();
m_camera_logic.Reset();
m_motion_plus.Reset();
GetActiveExtension()->Reset();

m_status = {};
// TODO: This will suppress a status report on connect when an extension is already attached.
// I am not 100% sure if this is proper.
// This will suppress a status report on connect when an extension is already attached.
// TODO: I am not 100% sure if this is proper.
m_status.extension = m_extension_port.IsDeviceConnected();

// Dynamics:
m_swing_state = {};
m_tilt_state = {};
m_cursor_state = {};
m_shake_state = {};
}

@@ -165,6 +166,8 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index)
m_attachments->AddAttachment(std::make_unique<WiimoteEmu::Drums>());
m_attachments->AddAttachment(std::make_unique<WiimoteEmu::Turntable>());

m_attachments->AddSetting(&m_motion_plus_setting, {_trans("Attach MotionPlus")}, true);

// rumble
groups.emplace_back(m_rumble = new ControllerEmu::ControlGroup(_trans("Rumble")));
m_rumble->controls.emplace_back(
@@ -193,8 +196,6 @@ Wiimote::Wiimote(const unsigned int index) : m_index(index)
_trans("%")},
95, 0, 100);

// m_options->AddSetting(&m_motion_plus_setting, {_trans("Attach MotionPlus")}, true);

// Note: "Upright" and "Sideways" options can be enabled at the same time which produces an
// orientation where the wiimote points towards the left with the buttons towards you.
m_options->AddSetting(&m_upright_setting,
@@ -310,6 +311,7 @@ void Wiimote::UpdateButtonsStatus()
m_dpad->GetState(&m_status.buttons.hex, IsSideways() ? dpad_sideways_bitmasks : dpad_bitmasks);
}

// This is called every ::Wiimote::UPDATE_FREQ (200hz)
void Wiimote::Update()
{
// Check if connected.
@@ -322,6 +324,7 @@ void Wiimote::Update()
// Data is later accessed in IsSideways and IsUpright
m_hotkeys->GetState();

// Update our motion simulations.
StepDynamics();

// Update buttons in the status struct which is sent in 99% of input reports.
@@ -334,10 +337,22 @@ void Wiimote::Update()
// If a new extension is requested in the GUI the change will happen here.
HandleExtensionSwap();

// Allow extension to perform any regular duties it may need.
// (e.g. Nunchuk motion simulation step)
// Input is prepared here too.
// TODO: Separate input preparation from Update.
GetActiveExtension()->Update();

if (m_is_motion_plus_attached)
{
// M+ has some internal state that must processed.
m_motion_plus.Update();
}

// Returns true if a report was sent.
if (ProcessExtensionPortEvent())
{
// Extension port event occured.
// Extension port event occurred.
// Don't send any other reports.
return;
}
@@ -403,6 +418,8 @@ void Wiimote::SendDataReport()
// IR Camera:
if (rpt_builder.HasIR())
{
// Note: Camera logic currently contains no changing state so we can just update it here.
// If that changes this should be moved to Wiimote::Update();
m_camera_logic.Update(GetTransformation());

// The real wiimote reads camera data from the i2c bus starting at offset 0x37:
@@ -416,9 +433,16 @@ void Wiimote::SendDataReport()
// Extension port:
if (rpt_builder.HasExt())
{
// Update extension first as motion-plus may read from it.
GetActiveExtension()->Update();
m_motion_plus.Update();
// Prepare extension input first as motion-plus may read from it.
// This currently happens in Wiimote::Update();
// TODO: Separate extension input data preparation from Update.
// GetActiveExtension()->PrepareInput();

if (m_is_motion_plus_attached)
{
// TODO: Make input preparation triggered by bus read.
m_motion_plus.PrepareInput(GetAngularVelocity());
}

u8* ext_data = rpt_builder.GetExtDataPtr();
const u8 ext_size = rpt_builder.GetExtDataSize();
@@ -658,44 +682,59 @@ void Wiimote::StepDynamics()
{
EmulateSwing(&m_swing_state, m_swing, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateTilt(&m_tilt_state, m_tilt, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateCursor(&m_cursor_state, m_ir, 1.f / ::Wiimote::UPDATE_FREQ);
EmulateShake(&m_shake_state, m_shake, 1.f / ::Wiimote::UPDATE_FREQ);

// TODO: Move cursor state out of ControllerEmu::Cursor
// const auto cursor_mtx = EmulateCursorMovement(m_ir);
}

Common::Vec3 Wiimote::GetAcceleration()
{
// Includes effects of:
// IR, Tilt, Swing, Orientation, Shake

auto orientation = Common::Matrix33::Identity();

if (IsSideways())
orientation *= Common::Matrix33::RotateZ(float(MathUtil::TAU / -4));

if (IsUpright())
orientation *= Common::Matrix33::RotateX(float(MathUtil::TAU / 4));
// TODO: Cursor forward/backward movement should produce acceleration.

Common::Vec3 accel =
orientation *
GetOrientation() *
GetTransformation().Transform(
m_swing_state.acceleration + Common::Vec3(0, 0, float(GRAVITY_ACCELERATION)), 0);

// Our shake effects have never been affected by orientation. Should they be?
accel += m_shake_state.acceleration;

// Simulate centripetal acceleration caused by an offset of the accelerometer sensor.
// Estimate of sensor position based on an image of the wii remote board:
constexpr float ACCELEROMETER_Y_OFFSET = 0.1f;

const auto angular_velocity = GetAngularVelocity();
const auto centripetal_accel =
// TODO: Is this the proper way to combine the x and z angular velocities?
std::pow(std::abs(angular_velocity.x) + std::abs(angular_velocity.z), 2) *
ACCELEROMETER_Y_OFFSET;

accel.y += centripetal_accel;

return accel;
}

Common::Vec3 Wiimote::GetAngularVelocity()
{
return GetOrientation() * (m_tilt_state.angular_velocity + m_swing_state.angular_velocity +
m_cursor_state.angular_velocity);
}

Common::Matrix44 Wiimote::GetTransformation() const
{
// Includes positional and rotational effects of:
// IR, Swing, Tilt, Shake
// Cursor, Swing, Tilt, Shake

// TODO: think about and clean up matrix order, make nunchuk match.
return Common::Matrix44::Translate(-m_shake_state.position) *
Common::Matrix44::FromMatrix33(GetRotationalMatrix(-m_tilt_state.angle) *
GetRotationalMatrix(-m_swing_state.angle)) *
EmulateCursorMovement(m_ir) * Common::Matrix44::Translate(-m_swing_state.position);
Common::Matrix44::FromMatrix33(GetRotationalMatrix(
-m_tilt_state.angle - m_swing_state.angle - m_cursor_state.angle)) *
Common::Matrix44::Translate(-m_swing_state.position - m_cursor_state.position);
}

Common::Matrix33 Wiimote::GetOrientation() const
{
return Common::Matrix33::RotateZ(float(MathUtil::TAU / -4 * IsSideways())) *
Common::Matrix33::RotateX(float(MathUtil::TAU / 4 * IsUpright()));
}

} // namespace WiimoteEmu
@@ -137,10 +137,20 @@ class Wiimote : public ControllerEmu::EmulatedController

void UpdateButtonsStatus();

// Returns simulated accelerometer data in m/s^2.
Common::Vec3 GetAcceleration();
// Used for simulating camera data. Does not include orientation transformations.

// Returns simulated gyroscope data in radians/s.
Common::Vec3 GetAngularVelocity();

// Returns the transformation of the world around the wiimote.
// Used for simulating camera data and for rotating acceleration data.
// Does not include orientation transformations.
Common::Matrix44 GetTransformation() const;

// Returns the world rotation from the effects of sideways/upright settings.
Common::Matrix33 GetOrientation() const;

void HIDOutputReport(const void* data, u32 size);

void HandleReportRumble(const WiimoteCommon::OutputReportRumble&);
@@ -236,7 +246,7 @@ class Wiimote : public ControllerEmu::EmulatedController
ControllerEmu::SettingValue<bool> m_upright_setting;
ControllerEmu::SettingValue<double> m_battery_setting;
ControllerEmu::SettingValue<double> m_speaker_pan_setting;
// ControllerEmu::SettingValue<bool> m_motion_plus_setting;
ControllerEmu::SettingValue<bool> m_motion_plus_setting;

SpeakerLogic m_speaker_logic;
MotionPlus m_motion_plus;
@@ -267,6 +277,7 @@ class Wiimote : public ControllerEmu::EmulatedController
// Dynamics:
MotionState m_swing_state;
RotationalState m_tilt_state;
MotionState m_cursor_state;
PositionalState m_shake_state;
};
} // namespace WiimoteEmu
@@ -74,7 +74,7 @@ static Common::Event g_compressAndDumpStateSyncEvent;
static std::thread g_save_thread;

// Don't forget to increase this after doing changes on the savestate system
static const u32 STATE_VERSION = 108; // Last changed in PR 7870
static const u32 STATE_VERSION = 109; // Last changed in PR 7861

// Maps savestate versions to Dolphin versions.
// Versions after 42 don't need to be added to this list,
@@ -217,7 +217,7 @@ void MappingIndicator::DrawCursor(ControllerEmu::Cursor& cursor)
QRectF(-scale, raw_coord.z * scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));

// Adjusted Z (if not hidden):
if (adj_coord.z && adj_coord.x < 10000)
if (adj_coord.IsVisible())
{
p.setBrush(GetAdjustedInputColor());
p.drawRect(
@@ -250,7 +250,7 @@ void MappingIndicator::DrawCursor(ControllerEmu::Cursor& cursor)
p.drawEllipse(QPointF{raw_coord.x, raw_coord.y} * scale, INPUT_DOT_RADIUS, INPUT_DOT_RADIUS);

// Adjusted cursor position (if not hidden):
if (adj_coord.x < 10000)
if (adj_coord.IsVisible())
{
p.setPen(Qt::NoPen);
p.setBrush(GetAdjustedInputColor());
@@ -492,11 +492,19 @@ void MappingIndicator::DrawForce(ControllerEmu::Force& force)
QRectF(-scale, raw_coord.z * scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));

// Adjusted Z:
if (adj_coord.y)
const auto curve_point =
std::max(std::abs(m_motion_state.angle.x), std::abs(m_motion_state.angle.z)) / MathUtil::TAU;
if (adj_coord.y || curve_point)
{
p.setBrush(GetAdjustedInputColor());
p.drawRect(
QRectF(-scale, adj_coord.y * -scale - INPUT_DOT_RADIUS / 2, scale * 2, INPUT_DOT_RADIUS));
// Show off the angle somewhat with a curved line.
QPainterPath path;
path.moveTo(-scale, (adj_coord.y + curve_point) * -scale);
path.quadTo({0, (adj_coord.y - curve_point) * -scale},
{scale, (adj_coord.y + curve_point) * -scale});

p.setBrush(Qt::NoBrush);
p.setPen(QPen(GetAdjustedInputColor(), INPUT_DOT_RADIUS));
p.drawPath(path);
}

// Draw "gate" shape.
@@ -55,7 +55,7 @@ void WiimoteEmuGeneral::CreateMainLayout()

extension->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);

static_cast<QFormLayout*>(extension->layout())->addRow(m_extension_combo);
static_cast<QFormLayout*>(extension->layout())->insertRow(0, m_extension_combo);

layout->addWidget(extension, 0, 3);
layout->addWidget(CreateGroupBox(tr("Rumble"), Wiimote::GetWiimoteGroup(
@@ -6,6 +6,7 @@

#include <algorithm>
#include <cmath>
#include <limits>
#include <memory>
#include <string>

@@ -153,8 +154,7 @@ Cursor::StateData Cursor::GetState(const bool adjusted)
// If auto-hide time is up or hide button is held:
if (!m_auto_hide_timer || controls[6]->control_ref->State() > BUTTON_THRESHOLD)
{
// TODO: Use NaN or something:
result.x = 10000;
result.x = std::numeric_limits<ControlState>::quiet_NaN();
result.y = 0;
}

@@ -176,4 +176,9 @@ ControlState Cursor::GetVerticalOffset() const
return m_vertical_offset_setting.GetValue() / 100;
}

bool Cursor::StateData::IsVisible() const
{
return !std::isnan(x);
}

} // namespace ControllerEmu
@@ -20,6 +20,8 @@ class Cursor : public ReshapableInput
ControlState x{};
ControlState y{};
ControlState z{};

bool IsVisible() const;
};

explicit Cursor(const std::string& name);
@@ -31,24 +31,38 @@ Force::Force(const std::string& name_) : ReshapableInput(name_, name_, GroupType
_trans("cm"),
// i18n: Refering to emulated wii remote swing movement.
_trans("Distance of travel from neutral position.")},
25, 0, 100);

AddSetting(&m_jerk_setting,
// i18n: "Jerk" as it relates to physics. The time derivative of acceleration.
{_trans("Jerk"),
// i18n: The symbol/abbreviation for meters per second to the 3rd power.
_trans("m/s³"),
50, 1, 100);

// These speed settings are used to calculate a maximum jerk (change in acceleration).
// The calculation uses a travel distance of 1 meter.
// The maximum value of 40 m/s is the approximate speed of the head of a golf club.
// Games seem to not even properly detect motions at this speed.
// Values result in an exponentially increasing jerk.

AddSetting(&m_speed_setting,
{_trans("Speed"),
// i18n: The symbol/abbreviation for meters per second.
_trans("m/s"),
// i18n: Refering to emulated wii remote swing movement.
_trans("Peak velocity of outward swing movements.")},
16, 1, 40);

// "Return Speed" allows for a "slow return" that won't trigger additional actions.
AddSetting(&m_return_speed_setting,
{_trans("Return Speed"),
// i18n: The symbol/abbreviation for meters per second.
_trans("m/s"),
// i18n: Refering to emulated wii remote swing movement.
_trans("Maximum change in acceleration.")},
500, 1, 1000);
_trans("Peak velocity of movements to neutral position.")},
2, 1, 40);

AddSetting(&m_angle_setting,
{_trans("Angle"),
// i18n: The symbol/abbreviation for degrees (unit of angular measure).
_trans("°"),
// i18n: Refering to emulated wii remote swing movement.
_trans("Rotation applied at extremities of swing.")},
45, 0, 180);
90, 1, 180);
}

Force::ReshapeData Force::GetReshapableState(bool adjusted)
@@ -70,8 +84,8 @@ Force::StateData Force::GetState(bool adjusted)

if (adjusted)
{
// Apply deadzone to z.
z = ApplyDeadzone(z, GetDeadzonePercentage());
// Apply deadzone to z and scale.
z = ApplyDeadzone(z, GetDeadzonePercentage()) * GetMaxDistance();
}

return {float(state.x), float(state.y), float(z)};
@@ -83,9 +97,14 @@ ControlState Force::GetGateRadiusAtAngle(double) const
return GetMaxDistance();
}

ControlState Force::GetMaxJerk() const
ControlState Force::GetSpeed() const
{
return m_speed_setting.GetValue();
}

ControlState Force::GetReturnSpeed() const
{
return m_jerk_setting.GetValue();
return m_return_speed_setting.GetValue();
}

ControlState Force::GetTwistAngle() const
@@ -26,8 +26,9 @@ class Force : public ReshapableInput

StateData GetState(bool adjusted = true);

// Return jerk in m/s^3.
ControlState GetMaxJerk() const;
// Velocities returned in m/s.
ControlState GetSpeed() const;
ControlState GetReturnSpeed() const;

// Return twist angle in radians.
ControlState GetTwistAngle() const;
@@ -37,7 +38,8 @@ class Force : public ReshapableInput

private:
SettingValue<double> m_distance_setting;
SettingValue<double> m_jerk_setting;
SettingValue<double> m_speed_setting;
SettingValue<double> m_return_speed_setting;
SettingValue<double> m_angle_setting;
};