From d9333a199354c0dfbe742cf396a90dc064cc5195 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:20:40 +0200 Subject: [PATCH 01/10] manual control setpoint: rename variables --- .../uORB/topics/manual_control_setpoint.h | 36 ++++++++++++------- 1 file changed, 23 insertions(+), 13 deletions(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index a23d89cd27..593e9453f9 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -64,22 +64,32 @@ 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 */ + float y; /**< stick position in y direction -1..1 + in general corresponds to right/left motion or roll of vehicle */ + float z; /**< throttle stick position 0..1 + in general corresponds to up/down motion or thrust of vehicle */ + float r; /**< yaw stick/twist positon, -1..1 + in general corresponds to 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; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ + switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** From cde4c9addbe2e8ccd782c53daf519fcf9669626a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:21:27 +0200 Subject: [PATCH 02/10] commander: use new manual control setpoint variable names --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/commander/rc_calibration.cpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfab9d4d6f..fb644a8dba 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -367,7 +367,7 @@ static orb_advert_t status_pub; transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; - + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); @@ -376,7 +376,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } - + return arming_res; } @@ -1164,7 +1164,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 */ @@ -1182,7 +1182,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 41f3ca0aa1..0776894fb7 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 */ From 3779e216be53540eb9d2e9470ba4077d5d33d534 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:22:07 +0200 Subject: [PATCH 03/10] fw att control: use new manual control setpoint variable names --- src/modules/fw_att_control/fw_att_control_main.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) 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 5276b1c134..5c83f85a1c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -717,9 +717,9 @@ 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) + _parameters.rollsp_offset_rad; - pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - throttle_sp = _manual.throttle; + roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad; + pitch_sp = (-_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; /* @@ -825,10 +825,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; } From 1795d7d6e1611e9365548bf4395323cdea9ec71d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:22:20 +0200 Subject: [PATCH 04/10] fw pos control: use new manual control setpoint variable names --- .../fw_pos_control_l1_main.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) 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 7f13df7854..68301648fc 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 @@ -1060,7 +1060,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + _seatbelt_hold_heading = _att.yaw + _manual.r; } //XXX not used @@ -1078,12 +1078,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX check if ground speed undershoot should be applied here float seatbelt_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.throttle; + _manual.z; _l1_control.navigate_heading(_seatbelt_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, seatbelt_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, @@ -1099,7 +1099,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + _seatbelt_hold_heading = _att.yaw + _manual.r; } /* if in seatbelt mode, set airspeed based on manual control */ @@ -1107,10 +1107,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX check if ground speed undershoot should be applied here float seatbelt_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); @@ -1120,14 +1120,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(_seatbelt_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, seatbelt_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, From 08002fbc15d7194a99f527fc21b6cae6398787fa Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:23:20 +0200 Subject: [PATCH 05/10] mavlink receiver: use new manual control setpoint variable names --- src/modules/mavlink/mavlink_receiver.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 64fc41838d..b03a68c076 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); From 6d9ea86bc9d50d84fcfd8625676e8ff1c53ca1fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:23:51 +0200 Subject: [PATCH 06/10] mavlink receiver: use new manual control setpoint variable names and fix sending of manual control setpoint mavlink message --- src/modules/mavlink/mavlink_messages.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9c552515d3..c2490c7815 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); } } From de4c4561961562d424e77c4557b51c6166bfd1d2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:24:06 +0200 Subject: [PATCH 07/10] mc att control: use new manual control setpoint variable names --- src/modules/mc_att_control/mc_att_control_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 36d95bf063..5f6963ce99 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -478,7 +478,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; } @@ -496,7 +496,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; @@ -512,8 +512,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; } From 299918295201c235dd7b6c96f3a5e3241f3e2813 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:24:22 +0200 Subject: [PATCH 08/10] mc pos control: use new manual control setpoint variable names --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 7c625a0c53..bd63a100bb 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; From 8cbd38061ccccf2173b16ea4b5db69bb1fbd2fd4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:24:49 +0200 Subject: [PATCH 09/10] sensors: use new manual control setpoint variable names --- src/modules/sensors/sensors.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index e260aae45c..22204ae5ff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1402,10 +1402,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); @@ -1433,10 +1433,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; From 15699549a21e08e9bf384dba7a1b65d092f1cb9a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 13:35:11 +0200 Subject: [PATCH 10/10] manual control setpoint: add comment about sign --- src/modules/uORB/topics/manual_control_setpoint.h | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index a8fb795f41..19a29635b2 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -71,13 +71,20 @@ struct manual_control_setpoint_s { * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000) */ float x; /**< stick position in x direction -1..1 - in general corresponds to forward/back motion or pitch of vehicle */ + 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 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 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 rotation around the vertical + 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 */