diff options
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 6 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 8 | ||||
-rw-r--r-- | apps/sensors/sensor_params.c | 5 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 37 |
4 files changed, 43 insertions, 13 deletions
diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index eec42fa7b..3fb505174 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -140,9 +140,9 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of attitude setpoint */ orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp); - att_sp.roll_body = -manual.roll * M_PI_F / 8.0f; - att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f; - att_sp.yaw_rate_body = -manual.yaw * M_PI_F; + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + att_sp.yaw_rate_body = manual.yaw; att_sp.timestamp = hrt_absolute_time(); if (motor_test_mode) { diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index db2d28601..2e8c61fd2 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -60,17 +60,17 @@ // PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); // PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */ +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.08f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.02f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.02f); PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f); -PARAM_DEFINE_FLOAT(MC_ATT_P, 0.3f); /* 0.15 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); /* 0.15 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_D, 0.1f); /* 0.025 F405 Flamewheel */ +PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); /* 0.025 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.3f); +PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index fcd0608a4..370a2e0cc 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -126,3 +126,8 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); + +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 3.0f); + diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 7c6437608..7d72fb85d 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -193,6 +193,10 @@ private: int rc_map_throttle; int rc_map_mode_sw; + int rc_scale_roll; + int rc_scale_pitch; + int rc_scale_yaw; + float battery_voltage_scaling; } _parameters; /**< local copies of interesting parameters */ @@ -214,6 +218,10 @@ private: param_t rc_map_throttle; param_t rc_map_mode_sw; + param_t rc_scale_roll; + param_t rc_scale_pitch; + param_t rc_scale_yaw; + param_t battery_voltage_scaling; } _parameter_handles; /**< handles for interesting parameters */ @@ -384,6 +392,10 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _parameter_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _parameter_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); _parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF"); @@ -492,6 +504,16 @@ Sensors::parameters_update() warnx("Failed getting mode sw chan index"); } + if (param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)) != OK) { + warnx("Failed getting rc scaling for roll"); + } + if (param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)) != OK) { + warnx("Failed getting rc scaling for pitch"); + } + if (param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)) != OK) { + warnx("Failed getting rc scaling for yaw"); + } + /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); @@ -928,18 +950,21 @@ Sensors::ppm_poll() _rc.chan_count = ppm_decoded_channels; _rc.timestamp = ppm_last_valid_decode; - /* roll input */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled * _parameters.rc_scale_roll; if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - /* pitch input */ - manual_control.pitch = _rc.chan[_rc.function[PITCH]].scaled; + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled * _parameters.rc_scale_pitch; if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - /* yaw input */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled; + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; |