dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge remote-tracking branch 'upstream/beta' into beta

This commit is contained in:
Thomas Gubler 2014-02-01 11:59:56 +01:00
parent 589f6cdb44 8897894b19
commit 7bb3197871
9 changed files with 133 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/lib/geo/geo.c
View File

@ -436,22 +436,22 @@ __EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, flo
__EXPORT float _wrap_pi(float bearing)
{
/* value is inf or NaN */
if (!isfinite(bearing) || bearing == 0) {
if (!isfinite(bearing)) {
return bearing;
}
int c = 0;
while (bearing > M_PI_F && c < 30) {
while (bearing > M_PI_F) {
bearing -= M_TWOPI_F;
c++;
if (c++ > 3)
return NAN;
}
c = 0;
while (bearing <= -M_PI_F && c < 30) {
while (bearing <= -M_PI_F) {
bearing += M_TWOPI_F;
c++;
if (c++ > 3)
return NAN;
}
return bearing;
@ -464,12 +464,18 @@ __EXPORT float _wrap_2pi(float bearing)
return bearing;
}
while (bearing >= M_TWOPI_F) {
bearing = bearing - M_TWOPI_F;
int c = 0;
while (bearing > M_TWOPI_F) {
bearing -= M_TWOPI_F;
if (c++ > 3)
return NAN;
}
while (bearing < 0.0f) {
bearing = bearing + M_TWOPI_F;
c = 0;
while (bearing <= 0.0f) {
bearing += M_TWOPI_F;
if (c++ > 3)
return NAN;
}
return bearing;
@ -482,12 +488,18 @@ __EXPORT float _wrap_180(float bearing)
return bearing;
}
int c = 0;
while (bearing > 180.0f) {
bearing = bearing - 360.0f;
bearing -= 360.0f;
if (c++ > 3)
return NAN;
}
while (bearing <= -180.0f) {
bearing = bearing + 360.0f;
c = 0;
while (bearing <= -180.0f) {
bearing += 360.0f;
if (c++ > 3)
return NAN;
}
return bearing;
@ -500,12 +512,18 @@ __EXPORT float _wrap_360(float bearing)
return bearing;
}
while (bearing >= 360.0f) {
bearing = bearing - 360.0f;
int c = 0;
while (bearing > 360.0f) {
bearing -= 360.0f;
if (c++ > 3)
return NAN;
}
while (bearing < 0.0f) {
bearing = bearing + 360.0f;
c = 0;
while (bearing <= 0.0f) {
bearing += 360.0f;
if (c++ > 3)
return NAN;
}
return bearing;

2
src/modules/commander/commander.cpp
View File

@ -369,6 +369,7 @@ int arm()
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}
@ -381,6 +382,7 @@ int disarm()
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline");
return 0;
} else {
return 1;
}

2
src/modules/commander/state_machine_helper.cpp
View File

@ -382,6 +382,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
break;
case FAILSAFE_STATE_RTL:
/* global position and home position required for RTL */
if (status->condition_global_position_valid && status->condition_home_position_valid) {
status->set_nav_state = NAV_STATE_RTL;
@ -392,6 +393,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
break;
case FAILSAFE_STATE_LAND:
/* at least relative altitude estimate required for landing */
if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
status->set_nav_state = NAV_STATE_LAND;

6
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
//}
} else {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
if (_manual_control_sp.yaw > 0.0f) {
yaw_sp_move_rate -= YAW_DEADZONE;
} else {
yaw_sp_move_rate += YAW_DEADZONE;
}
yaw_sp_move_rate *= _params.rc_scale_yaw;
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
@ -660,7 +664,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
_rates_int(i) = rate_i;
}
}

71
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global)
{
if (global != _use_global_alt) {
_use_global_alt = global;
if (global) {
/* switch from barometric to global altitude */
_alt_sp += _global_pos.alt - _global_pos.baro_alt;
} else {
/* switch from global to barometric altitude */
_alt_sp += _global_pos.baro_alt - _global_pos.alt;
@ -589,6 +591,7 @@ MulticopterPositionControl::task_main()
if (_control_mode.flag_control_manual_enabled) {
/* select altitude source and update setpoint */
select_alt(_global_pos.global_valid);
if (!_use_global_alt) {
alt = _global_pos.baro_alt;
}
@ -672,7 +675,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 +691,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 +716,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 +750,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,40 +844,54 @@ 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 */
float att_comp;
if (_att.R[2][2] > TILT_COS_MAX) {
att_comp = 1.0f / _att.R[2][2];
} else if (_att.R[2][2] > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;

5
src/modules/navigator/navigator_main.cpp
View File

@ -689,7 +689,7 @@ Navigator::task_main()
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
/* switch to RTL if not already landed after RTL and home position set */
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
_vstatus.condition_home_position_valid) {
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
@ -747,7 +747,7 @@ Navigator::task_main()
case NAV_STATE_RTL:
if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
_vstatus.condition_home_position_valid) {
_vstatus.condition_home_position_valid) {
dispatch(EVENT_RTL_REQUESTED);
}
@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached()
if (_rtl_state == RTL_STATE_DESCEND) {
/* hovering above home position, land if needed or loiter */
mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
if (_mission_item.autocontinue) {
dispatch(EVENT_LAND_REQUESTED);

34
src/modules/navigator/navigator_mission.cpp
View File

@ -128,31 +128,37 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
if (current_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
_current_mission_type = MISSION_TYPE_ONBOARD;
*onboard = true;
*index = _current_onboard_mission_index;
/* otherwise fallback to offboard */
/* otherwise fallback to offboard */
} else if (current_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
_current_mission_type = MISSION_TYPE_NONE;
return ERROR;
}
_current_mission_type = MISSION_TYPE_OFFBOARD;
*onboard = false;
*index = _current_offboard_mission_index;
@ -173,23 +179,27 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
if (next_onboard_mission_available()) {
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
}
/* otherwise fallback to offboard */
/* otherwise fallback to offboard */
} else if (next_offboard_mission_available()) {
dm_item_t dm_current;
if (_offboard_dataman_id == 0) {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
} else {
dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
}
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return ERROR;
@ -244,14 +254,16 @@ void
Mission::move_to_next()
{
switch (_current_mission_type) {
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
case MISSION_TYPE_ONBOARD:
_current_onboard_mission_index++;
break;
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
case MISSION_TYPE_NONE:
default:
break;
}
}

2
src/systemcmds/param/param.c
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);
}

22
src/systemcmds/tests/test_mixer.cpp
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";