fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 03:40:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #922 from PX4/manualcontrolrename

Browse Source 
Rename variables for manual control setpoint
This commit is contained in:
Julian Oes
2014-05-13 09:40:56 +02:00
parent 80ecaf7946 8f6cd3a3ae
commit 634157210c
10 changed files with 73 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+2 -2
View File
@@ -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.");
src/modules/commander/rc_calibration.cpp
+3 -3
View File
@@ -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 */
src/modules/fw_att_control/fw_att_control_main.cpp
+7 -7
View File
@@ -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;
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+10 -10
View File
@@ -1114,7 +1114,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_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> &current_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> &current_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> &current_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> &current_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,
src/modules/mavlink/mavlink_messages.cpp
+4 -4
View File
@@ -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);
}
}
src/modules/mavlink/mavlink_receiver.cpp
+5 -5
View File
@@ -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);
src/modules/mc_att_control/mc_att_control_main.cpp
+4 -4
View File
@@ -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;
}
src/modules/mc_pos_control/mc_pos_control_main.cpp
+4 -4
View File
@@ -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;
src/modules/sensors/sensors.cpp
+8 -8
View File
@@ -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;
src/modules/uORB/topics/manual_control_setpoint.h
+26 -9
View File
@@ -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 */