Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 20 Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:PX4/Firmware into driver_framework

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-21 11:20:27 +01:00
parent 9c80375a78 ca5b854ce8
commit c6510bd2de
5 changed files with 97 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/generate_listener.py
+1
View File
@@ -200,6 +200,7 @@ for index,m in enumerate(messages[1:]):
print("\t} else {")
print("\t\t printf(\" Topic did not match any known topics\\n\");")
print("\t}")
print("\t\torb_unsubscribe(sub);")
print("\t return 0;")
print("}")
src/drivers/pwm_out_sim/pwm_out_sim.cpp
+2 -1
View File
@@ -474,8 +474,9 @@ PWMSim::task_main()
}
/* do mixing */
actuator_outputs_s outputs;
actuator_outputs_s outputs = {};
num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
outputs.noutputs = num_outputs;
outputs.timestamp = hrt_absolute_time();
/* disable unused ports by setting their output to NaN */
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+4 -2
View File
@@ -894,8 +894,10 @@ void AttitudePositionEstimatorEKF::publishControlState()
_ctrl_state.q[3] = _ekf->states[3];
/* Airspeed (Groundspeed - Windspeed) */
_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
//_ctrl_state.airspeed = sqrt(pow(_ekf->states[4] - _ekf->states[14], 2) + pow(_ekf->states[5] - _ekf->states[15], 2) + pow(_ekf->states[6], 2));
// the line above was introduced by the control state PR. The airspeed it gives is totally wrong and leads to horrible flight performance in SITL
// and in outdoor tests
_ctrl_state.airspeed = _airspeed.true_airspeed_m_s;
/* Attitude Rates */
_ctrl_state.roll_rate = _LP_att_P.apply(_ekf->dAngIMU.x / _ekf->dtIMU) - _ekf->states[10] / _ekf->dtIMUfilt;
_ctrl_state.pitch_rate = _LP_att_Q.apply(_ekf->dAngIMU.y / _ekf->dtIMU) - _ekf->states[11] / _ekf->dtIMUfilt;
src/modules/mc_pos_control/mc_pos_control_main.cpp
+43 -25
View File
@@ -703,7 +703,8 @@ MulticopterPositionControl::control_manual(float dt)
/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(_params.vel_max); // in NED and scaled to actual velocity
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
_params.vel_max); // in NED and scaled to actual velocity
/*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
@@ -711,20 +712,20 @@ MulticopterPositionControl::control_manual(float dt)
*/
/* horizontal axes */
if (_control_mode.flag_control_position_enabled)
{
if (_control_mode.flag_control_position_enabled) {
/* check for pos. hold */
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz)
{
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
if (!_pos_hold_engaged) {
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) {
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
&& fabsf(_vel(1)) < _params.hold_max_xy)) {
_pos_hold_engaged = true;
} else {
_pos_hold_engaged = false;
}
}
}
else {
} else {
_pos_hold_engaged = false;
}
@@ -739,25 +740,27 @@ MulticopterPositionControl::control_manual(float dt)
}
/* vertical axis */
if (_control_mode.flag_control_altitude_enabled)
{
if (_control_mode.flag_control_altitude_enabled) {
/* check for pos. hold */
if (fabsf(req_vel_sp(2)) < _params.hold_z_dz)
{
if (!_alt_hold_engaged && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z))
{
if (fabsf(req_vel_sp(2)) < _params.hold_z_dz) {
if (!_alt_hold_engaged) {
if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
_alt_hold_engaged = true;
}
}
else {
} else {
_alt_hold_engaged = false;
}
}
} else {
_alt_hold_engaged = false;
_pos_sp(2) = _pos(2);
}
/* set requested velocity setpoint */
if (!_alt_hold_engaged) {
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
_vel_sp(2) = req_vel_sp_scaled(2);
_pos_sp(2) = _pos(2);
}
}
}
@@ -777,6 +780,7 @@ MulticopterPositionControl::control_offboard(float dt)
/* control position */
_pos_sp(0) = _pos_sp_triplet.current.x;
_pos_sp(1) = _pos_sp_triplet.current.y;
} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
/* control velocity */
/* reset position setpoint to current position if needed */
@@ -791,6 +795,7 @@ MulticopterPositionControl::control_offboard(float dt)
if (_pos_sp_triplet.current.yaw_valid) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
} else if (_pos_sp_triplet.current.yawspeed_valid) {
_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
}
@@ -798,6 +803,7 @@ MulticopterPositionControl::control_offboard(float dt)
if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
/* Control altitude */
_pos_sp(2) = _pos_sp_triplet.current.z;
} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
@@ -807,6 +813,7 @@ MulticopterPositionControl::control_offboard(float dt)
_run_alt_control = false; /* request velocity setpoint to be used, instead of position setpoint */
}
} else {
reset_pos_sp();
reset_alt_sp();
@@ -850,6 +857,7 @@ void MulticopterPositionControl::control_auto(float dt)
//Poll position setpoint
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
@@ -920,6 +928,7 @@ void MulticopterPositionControl::control_auto(float dt)
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
if (curr_pos_s_len < 1.0f) {
/* copter is closer to waypoint than unit radius */
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
@@ -945,6 +954,7 @@ void MulticopterPositionControl::control_auto(float dt)
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* if curr - next distance is larger than unit radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
@@ -961,6 +971,7 @@ void MulticopterPositionControl::control_auto(float dt)
} else {
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
if (near) {
/* unit sphere crosses trajectory */
@@ -988,6 +999,7 @@ void MulticopterPositionControl::control_auto(float dt)
/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();
if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}
@@ -1145,7 +1157,8 @@ MulticopterPositionControl::task_main()
control_auto(dt);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
@@ -1180,6 +1193,7 @@ MulticopterPositionControl::task_main()
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
@@ -1188,6 +1202,7 @@ MulticopterPositionControl::task_main()
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2) * _vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {
_vel_sp(2) = _vel_sp(2) * _params.vel_max(2) / vel_norm_z;
}
@@ -1210,7 +1225,8 @@ MulticopterPositionControl::task_main()
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid
&& _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
@@ -1510,6 +1526,7 @@ MulticopterPositionControl::task_main()
/* publish local position setpoint */
if (_local_pos_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
@@ -1533,8 +1550,7 @@ MulticopterPositionControl::task_main()
}
/* do not move yaw while arming */
else if (_manual.z > 0.1f)
{
else if (_manual.z > 0.1f) {
const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
@@ -1574,8 +1590,8 @@ MulticopterPositionControl::task_main()
q_sp.from_dcm(R_sp);
memcpy(&_att_sp.q_d[0], &q_sp.data[0], sizeof(_att_sp.q_d));
_att_sp.timestamp = hrt_absolute_time();
}
else {
} else {
reset_yaw_sp = true;
}
@@ -1591,13 +1607,15 @@ MulticopterPositionControl::task_main()
_control_mode.flag_control_velocity_enabled))) {
if (_att_sp_pub != nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else if (_att_sp_pub == nullptr && (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode)) {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled
&& !_control_mode.flag_control_climb_rate_enabled;
}
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
src/modules/uavcan/uavcan_servers.cpp
+7 -2
View File
@@ -523,8 +523,12 @@ pthread_addr_t UavcanServers::run(pthread_addr_t)
switch (command_id) {
case 1: {
// Param save request
int node_id;
node_id = get_next_dirty_node_id(1);
if (node_id < 128) {
_param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
param_opcode(get_next_dirty_node_id(1));
param_opcode(node_id);
}
break;
}
case 2: {
@@ -575,7 +579,8 @@ void UavcanServers::cb_getset(const uavcan::ServiceCallResult<uavcan::protocol::
if (result.isSuccessful()) {
uavcan::protocol::param::GetSet::Response resp = result.getResponse();
if (resp.name.size()) {
_param_counts[node_id] = _count_index++;
_count_index++;
_param_counts[node_id] = _count_index;
uavcan::protocol::param::GetSet::Request req;
req.index = _count_index;