diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 4 | ||||
-rw-r--r-- | src/modules/commander/rc_calibration.cpp | 6 | ||||
-rw-r--r-- | src/modules/fw_att_control/fw_att_control_main.cpp | 14 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 20 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 8 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 10 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 8 | ||||
-rw-r--r-- | src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 16 | ||||
-rw-r--r-- | src/modules/uORB/topics/manual_control_setpoint.h | 35 |
10 files changed, 73 insertions, 56 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 22504642f..17d3d3dcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1188,7 +1188,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1206,7 +1206,7 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 41f3ca0aa..0776894fb 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; + float p = sp.y; param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; + p = sp.x; param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; + p = sp.r; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 654b7d56b..1c411fa06 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -716,11 +716,11 @@ FixedwingAttitudeControl::task_main() * the intended attitude setpoint. Later, after the rate control step the * trim is added again to the control signal. */ - roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; - pitch_sp = -(_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - throttle_sp = _manual.throttle; + throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* @@ -826,10 +826,10 @@ FixedwingAttitudeControl::task_main() } else { /* manual/direct control */ - _actuators.control[0] = _manual.roll; - _actuators.control[1] = -_manual.pitch; - _actuators.control[2] = _manual.yaw; - _actuators.control[3] = _manual.throttle; + _actuators.control[0] = _manual.y; + _actuators.control[1] = -_manual.x; + _actuators.control[2] = _manual.r; + _actuators.control[3] = _manual.z; _actuators.control[4] = _manual.flaps; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index c62a2d4f3..9cbc26efe 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1114,7 +1114,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.yaw; + _altctrl_hold_heading = _att.yaw + _manual.r; } //XXX not used @@ -1132,12 +1132,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX check if ground speed undershoot should be applied here float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, @@ -1153,7 +1153,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (0/* altctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.yaw; + _altctrl_hold_heading = _att.yaw + _manual.r; } /* if in altctrl mode, set airspeed based on manual control */ @@ -1161,10 +1161,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX check if ground speed undershoot should be applied here float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; /* user switched off throttle */ - if (_manual.throttle < 0.1f) { + if (_manual.z < 0.1f) { throttle_max = 0.0f; /* switch to pure pitch based altitude control, give up speed */ _tecs.set_speed_weight(0.0f); @@ -1174,14 +1174,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi bool climb_out = false; /* user wants to climb out */ - if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + if (_manual.x > 0.3f && _manual.z > 0.8f) { climb_out = true; } _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _manual.roll; - _att_sp.yaw_body = _manual.yaw; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + _att_sp.roll_body = _manual.y; + _att_sp.yaw_body = _manual.r; + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b8879157e..79dd88657 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1138,10 +1138,10 @@ protected: if (manual_sub->update(t)) { mavlink_msg_manual_control_send(_channel, mavlink_system.sysid, - manual->roll * 1000, - manual->pitch * 1000, - manual->yaw * 1000, - manual->throttle * 1000, + manual->x * 1000, + manual->y * 1000, + manual->z * 1000, + manual->r * 1000, 0); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 64fc41838..b03a68c07 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -191,7 +191,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } } - + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); @@ -438,10 +438,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) memset(&manual, 0, sizeof(manual)); manual.timestamp = hrt_absolute_time(); - manual.pitch = man.x / 1000.0f; - manual.roll = man.y / 1000.0f; - manual.yaw = man.r / 1000.0f; - manual.throttle = man.z / 1000.0f; + manual.x = man.x / 1000.0f; + manual.y = man.y / 1000.0f; + manual.r = man.r / 1000.0f; + manual.z = man.z / 1000.0f; if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 57450cf39..a69153bf0 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -486,7 +486,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_climb_rate_enabled) { /* pass throttle directly if not in altitude stabilized mode */ - _v_att_sp.thrust = _manual_control_sp.throttle; + _v_att_sp.thrust = _manual_control_sp.z; publish_att_sp = true; } @@ -504,7 +504,7 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max; + yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -520,8 +520,8 @@ MulticopterAttitudeControl::control_attitude(float dt) if (!_v_control_mode.flag_control_velocity_enabled) { /* update attitude setpoint if not in position control mode */ - _v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max; - _v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max; + _v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max; + _v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max; _v_att_sp.R_valid = false; publish_att_sp = true; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 45ff8c3c0..6e611d4ab 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -617,7 +617,7 @@ MulticopterPositionControl::task_main() reset_alt_sp(); /* move altitude setpoint with throttle stick */ - sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { @@ -625,8 +625,8 @@ MulticopterPositionControl::task_main() reset_pos_sp(); /* move position setpoint with roll/pitch stick */ - sp_move_rate(0) = _manual.pitch; - sp_move_rate(1) = _manual.roll; + sp_move_rate(0) = _manual.x; + sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ @@ -782,7 +782,7 @@ MulticopterPositionControl::task_main() float i = _params.thr_min; if (reset_int_z_manual) { - i = _manual.throttle; + i = _manual.z; if (i < _params.thr_min) { i = _params.thr_min; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 65165bbb2..18bf97f8d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1486,10 +1486,10 @@ Sensors::rc_poll() manual.timestamp = rc_input.timestamp_last_signal; /* limit controls */ - manual.roll = get_rc_value(ROLL, -1.0, 1.0); - manual.pitch = get_rc_value(PITCH, -1.0, 1.0); - manual.yaw = get_rc_value(YAW, -1.0, 1.0); - manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0); + manual.y = get_rc_value(ROLL, -1.0, 1.0); + manual.x = get_rc_value(PITCH, -1.0, 1.0); + manual.r = get_rc_value(YAW, -1.0, 1.0); + manual.z = get_rc_value(THROTTLE, 0.0, 1.0); manual.flaps = get_rc_value(FLAPS, -1.0, 1.0); manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0); manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0); @@ -1517,10 +1517,10 @@ Sensors::rc_poll() actuator_group_3.timestamp = rc_input.timestamp_last_signal; - actuator_group_3.control[0] = manual.roll; - actuator_group_3.control[1] = manual.pitch; - actuator_group_3.control[2] = manual.yaw; - actuator_group_3.control[3] = manual.throttle; + actuator_group_3.control[0] = manual.y; + actuator_group_3.control[1] = manual.x; + actuator_group_3.control[2] = manual.r; + actuator_group_3.control[3] = manual.z; actuator_group_3.control[4] = manual.flaps; actuator_group_3.control[5] = manual.aux1; actuator_group_3.control[6] = manual.aux2; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index ee159b998..19a29635b 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -64,17 +64,34 @@ struct manual_control_setpoint_s { /** * Any of the channels may not be available and be set to NaN * to indicate that it does not contain valid data. + * The variable names follow the definition of the + * MANUAL_CONTROL mavlink message. + * The default range is from -1 to 1 (mavlink message -1000 to 1000) + * The range for the z variable is defined from 0 to 1. (The z field of + * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) */ - float roll; /**< ailerons roll / roll rate input, -1..1 */ - float pitch; /**< elevator / pitch / pitch rate, -1..1 */ - float yaw; /**< rudder / yaw rate / yaw, -1..1 */ - float throttle; /**< throttle / collective thrust / altitude, 0..1 */ + float x; /**< stick position in x direction -1..1 + in general corresponds to forward/back motion or pitch of vehicle, + in general a positive value means forward or negative pitch and + a negative value means backward or positive pitch */ + float y; /**< stick position in y direction -1..1 + in general corresponds to right/left motion or roll of vehicle, + in general a positive value means right or positive roll and + a negative value means left or negative roll */ + float z; /**< throttle stick position 0..1 + in general corresponds to up/down motion or thrust of vehicle, + in general the value corresponds to the demanded throttle by the user, + if the input is used for setting the setpoint of a vertical position + controller any value > 0.5 means up and any value < 0.5 means down */ + float r; /**< yaw stick/twist positon, -1..1 + in general corresponds to the righthand rotation around the vertical + (downwards) axis of the vehicle */ float flaps; /**< flap position */ - float aux1; /**< default function: camera yaw / azimuth */ - float aux2; /**< default function: camera pitch / tilt */ - float aux3; /**< default function: camera trigger */ - float aux4; /**< default function: camera roll */ - float aux5; /**< default function: payload drop */ + float aux1; /**< default function: camera yaw / azimuth */ + float aux2; /**< default function: camera pitch / tilt */ + float aux3; /**< default function: camera trigger */ + float aux4; /**< default function: camera roll */ + float aux5; /**< default function: payload drop */ switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */ switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ |