fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 06:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'beta' into acro2

Browse Source
This commit is contained in:
Anton Babushkin
2014-02-01 11:01:30 +01:00
parent 7f4f9a5f5f 8660ea914a
commit 3651552b53
3 changed files with 49 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/mc_pos_control_main.cpp
+37 -27
View File
@@ -645,7 +645,7 @@ MulticopterPositionControl::task_main()
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
add_vector_to_global_position(_global_pos.lat, _global_pos.lon, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
}
@@ -672,7 +672,7 @@ MulticopterPositionControl::task_main()
_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
}
} else if (_control_mode.flag_control_auto_enabled) {
} else {
/* always use AMSL altitude for AUTO */
select_alt(true);
@@ -688,22 +688,24 @@ MulticopterPositionControl::task_main()
_reset_lat_lon_sp = true;
_reset_alt_sp = true;
/* update position setpoint */
_lat_sp = _pos_sp_triplet.current.lat;
_lon_sp = _pos_sp_triplet.current.lon;
_alt_sp = _pos_sp_triplet.current.alt;
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, loiter, reset position setpoint if needed */
reset_lat_lon_sp();
reset_alt_sp();
}
} else {
/* no control, reset setpoint */
reset_lat_lon_sp();
reset_alt_sp();
}
if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
@@ -711,7 +713,7 @@ MulticopterPositionControl::task_main()
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
@@ -745,7 +747,7 @@ MulticopterPositionControl::task_main()
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
@@ -839,30 +841,38 @@ MulticopterPositionControl::task_main()
thr_min = 0.0f;
}
float tilt_max = _params.tilt_max;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max;
if (thr_min < 0.0f)
thr_min = 0.0f;
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
/* limit max tilt */
float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
float tilt_max = _params.tilt_max;
if (!_control_mode.flag_control_manual_enabled) {
if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.land_tilt_max;
if (thr_min < 0.0f)
thr_min = 0.0f;
}
}
if (_control_mode.flag_control_velocity_enabled) {
if (tilt > tilt_max && thr_min >= 0.0f) {
/* crop horizontal component */
float k = tanf(tilt_max) / tanf(tilt);
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
} else {
/* thrust compensation for altitude only control mode */
src/systemcmds/param/param.c
+1 -1
View File
@@ -320,7 +320,7 @@ do_set(const char* name, const char* val)
char* end;
f = strtod(val,&end);
param_set(param, &f);
printf(" -> new: %f\n", f);
printf(" -> new: %4.4f\n", (double)f);
}
src/systemcmds/tests/test_mixer.cpp
+11 -11
View File
@@ -65,20 +65,20 @@ static int mixer_callback(uintptr_t handle,
const unsigned output_max = 8;
static float actuator_controls[output_max];
static bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
int test_mixer(int argc, char *argv[])
{
/*
* PWM limit structure
*/
pwm_limit_t pwm_limit;
static bool should_arm = false;
uint16_t r_page_servo_disarmed[output_max];
uint16_t r_page_servo_control_min[output_max];
uint16_t r_page_servo_control_max[output_max];
uint16_t r_page_servos[output_max];
uint16_t servo_predicted[output_max];
warnx("testing mixer");
char *filename = "/etc/mixers/IO_pass.mix";