int MulticopterPositionControl::parameters_update(bool force) { bool updated; struct parameter_update_s param_upd; orb_check(_params_sub, &updated); if (updated) { orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); } if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); float v; param_get(_params_handles.xy_p, &v); _params.pos_p(0) = v; _params.pos_p(1) = v; param_get(_params_handles.z_p, &v); _params.pos_p(2) = v; param_get(_params_handles.xy_vel_p, &v); _params.vel_p(0) = v; _params.vel_p(1) = v; param_get(_params_handles.z_vel_p, &v); _params.vel_p(2) = v; param_get(_params_handles.xy_vel_i, &v); _params.vel_i(0) = v; _params.vel_i(1) = v; param_get(_params_handles.z_vel_i, &v); _params.vel_i(2) = v; param_get(_params_handles.xy_vel_d, &v); _params.vel_d(0) = v; _params.vel_d(1) = v; param_get(_params_handles.z_vel_d, &v); _params.vel_d(2) = v; param_get(_params_handles.xy_vel_max, &v); _params.vel_max(0) = v; _params.vel_max(1) = v; param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; } return OK; }
void MulticopterPositionControl::task_main() { warnx("started"); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[mpc] started"); /* * do subscriptions */ _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); parameters_update(true); /* initialize values of critical structs until first regular update */ _arming.armed = false; /* get an initial update for all sensor and status data */ poll_subscriptions(); bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool was_armed = false; hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; math::Vector<3> sp_move_rate; sp_move_rate.zero(); math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; R.identity(); /* wakeup source */ struct pollfd fds[1]; fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } poll_subscriptions(); parameters_update(false); hrt_abstime t = hrt_absolute_time(); float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; t_prev = t; if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; } was_armed = _control_mode.flag_armed; update_ref(); if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; _pos(2) = _local_pos.z; _vel(0) = _local_pos.vx; _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); /* move altitude setpoint with throttle stick */ sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { /* reset position setpoint to current position if needed */ reset_pos_sp(); /* move position setpoint with roll/pitch stick */ sp_move_rate(0) = _manual.x; sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ float sp_move_norm = sp_move_rate.length(); if (sp_move_norm > 1.0f) { sp_move_rate /= sp_move_norm; } /* scale 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); sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max); /* move position setpoint */ _pos_sp += sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } } else { /* AUTO */ 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); } if (_pos_sp_triplet.current.valid) { /* in case of interrupted mission don't go to waypoint but stay at current position */ _reset_pos_sp = true; _reset_alt_sp = true; /* project setpoint to local frame */ map_projection_project(&_ref_pos, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &_pos_sp.data[0], &_pos_sp.data[1]); _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_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_pos_sp(); reset_alt_sp(); } } /* fill local position setpoint */ _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); _local_pos_sp.yaw = _att_sp.yaw_body; /* publish local position setpoint */ if (_local_pos_sp_pub > 0) { 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); } 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)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _att.yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); /* publish attitude setpoint */ if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } else { /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; _vel_sp(2) = 0.0f; } if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } /* 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 == SETPOINT_TYPE_LAND) { _vel_sp(2) = _params.land_speed; } if (!_control_mode.flag_control_manual_enabled) { /* limit 3D speed only in non-manual modes */ float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); if (vel_sp_norm > 1.0f) { _vel_sp /= vel_sp_norm; } } _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); /* publish velocity setpoint */ if (_global_vel_sp_pub > 0) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); } else { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = false; float i = _params.thr_min; if (reset_int_z_manual) { i = _manual.z; if (i < _params.thr_min) { i = _params.thr_min; } else if (i > _params.thr_max) { i = _params.thr_max; } } thrust_int(2) = -i; } } else { reset_int_z = true; } if (_control_mode.flag_control_velocity_enabled) { if (reset_int_xy) { reset_int_xy = false; thrust_int(0) = 0.0f; thrust_int(1) = 0.0f; } } else { reset_int_xy = true; } /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; } if (!_control_mode.flag_control_climb_rate_enabled) { thrust_sp(2) = 0.0f; } /* limit thrust vector and check for saturation */ bool saturation_xy = false; bool saturation_z = false; /* limit min lift */ float thr_min = _params.thr_min; if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { /* don't allow downside thrust direction in manual attitude mode */ thr_min = 0.0f; } float tilt_max = _params.tilt_max_air; /* 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.tilt_max_land; 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; } if (_control_mode.flag_control_velocity_enabled) { /* 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; } thrust_sp(2) *= att_comp; } /* limit max thrust */ float thrust_abs = thrust_sp.length(); if (thrust_abs > _params.thr_max) { if (thrust_sp(2) < 0.0f) { if (-thrust_sp(2) > _params.thr_max) { /* thrust Z component is too large, limit it */ thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; thrust_sp(2) = -_params.thr_max; saturation_xy = true; saturation_z = true; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; } } else { /* Z component is negative, going down, simply limit thrust vector */ float k = _params.thr_max / thrust_abs; thrust_sp *= k; saturation_xy = true; saturation_z = true; } thrust_abs = _params.thr_max; } /* update integrals */ if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; } if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; /* protection against flipping on ground when landing */ if (thrust_int(2) > 0.0f) { thrust_int(2) = 0.0f; } } /* calculate attitude setpoint from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ math::Vector<3> body_x; math::Vector<3> body_y; math::Vector<3> body_z; if (thrust_abs > SIGMA) { body_z = -thrust_sp / thrust_abs; } else { /* no thrust, set Z axis to safe value */ body_z.zero(); body_z(2) = 1.0f; } /* vector of desired yaw direction in XY plane, rotated by PI/2 */ math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); if (fabsf(body_z(2)) > SIGMA) { /* desired body_x axis, orthogonal to body_z */ body_x = y_C % body_z; /* keep nose to front while inverted upside down */ if (body_z(2) < 0.0f) { body_x = -body_x; } body_x.normalize(); } else { /* desired thrust is in XY plane, set X downside to construct correct matrix, * but yaw component will not be used actually */ body_x.zero(); body_x(2) = 1.0f; } /* desired body_y axis */ body_y = body_z % body_x; /* fill rotation matrix */ for (int i = 0; i < 3; i++) { R(i, 0) = body_x(i); R(i, 1) = body_y(i); R(i, 2) = body_z(i); } /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ math::Vector<3> euler = R.to_euler(); _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ } else if (!_control_mode.flag_control_manual_enabled) { /* autonomous altitude control without position control (failsafe landing), * force level attitude, don't change yaw */ R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; _att_sp.timestamp = hrt_absolute_time(); /* publish attitude setpoint */ if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } else { reset_int_z = true; } } } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; reset_int_z = true; reset_int_xy = true; } /* 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; } warnx("stopped"); mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; _exit(0); }
void MulticopterPositionControl::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); /* * do subscriptions */ _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _arming_sub = orb_subscribe(ORB_ID(actuator_armed)); _local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); _pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); _local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); _global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); parameters_update(true); /* initialize values of critical structs until first regular update */ _arming.armed = false; /* get an initial update for all sensor and status data */ poll_subscriptions(); bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool reset_yaw_sp = true; bool was_armed = false; hrt_abstime t_prev = 0; _hover_time = 0.0; // miao: _mode_mission = 1; math::Vector<3> thrust_int; thrust_int.zero(); math::Matrix<3, 3> R; R.identity(); /* wakeup source */ struct pollfd fds[1]; fds[0].fd = _local_pos_sub; fds[0].events = POLLIN; while (!_task_should_exit) { /* wait for up to 500ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500); /* timed out - periodic check for _task_should_exit */ if (pret == 0) { continue; } /* this is undesirable but not much we can do */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } poll_subscriptions(); parameters_update(false); hrt_abstime t = hrt_absolute_time(); float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f; t_prev = t; if (_control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ _reset_pos_sp = true; _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; reset_yaw_sp = true; _reset_mission = true;//miao: } //Update previous arming state was_armed = _control_mode.flag_armed; update_ref(); if (_control_mode.flag_control_altitude_enabled || _control_mode.flag_control_position_enabled || _control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { _pos(0) = _local_pos.x; _pos(1) = _local_pos.y; _pos(2) = _local_pos.z; _vel(0) = _local_pos.vx; _vel(1) = _local_pos.vy; _vel(2) = _local_pos.vz; _vel_ff.zero(); _sp_move_rate.zero(); /* select control source */ if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt); _mode_auto = false; } else { /* AUTO */ 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) { /* 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)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; _att_sp.yaw_body = _att.yaw; _att_sp.thrust = 0.0f; _att_sp.timestamp = hrt_absolute_time(); /* publish attitude setpoint */ if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } } else { /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; _vel_sp(2) = 0.0f; } if (!_control_mode.flag_control_position_enabled) { _reset_pos_sp = true; _vel_sp(0) = 0.0f; _vel_sp(1) = 0.0f; } /* 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) { // miao: for auto landing test with manual mode if (_mode_mission==3) { _vel_sp(2) = _params.land_speed; } _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); /* publish velocity setpoint */ if (_global_vel_sp_pub > 0) { orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp); } else { _global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp); } if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) { /* reset integrals if needed */ if (_control_mode.flag_control_climb_rate_enabled) { if (reset_int_z) { reset_int_z = false; float i = _params.thr_min; if (reset_int_z_manual) { i = _manual.z; if (i < _params.thr_min) { i = _params.thr_min; } else if (i > _params.thr_max) { i = _params.thr_max; } } thrust_int(2) = -i; } } else { reset_int_z = true; } if (_control_mode.flag_control_velocity_enabled) { if (reset_int_xy) { reset_int_xy = false; thrust_int(0) = 0.0f; thrust_int(1) = 0.0f; } } else { reset_int_xy = true; } /* velocity error */ math::Vector<3> vel_err = _vel_sp - _vel; /* derivative of velocity error, not includes setpoint acceleration */ math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt; _vel_prev = _vel; /* thrust vector in NED frame */ math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int; if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; } if (!_control_mode.flag_control_climb_rate_enabled) { thrust_sp(2) = 0.0f; } /* limit thrust vector and check for saturation */ bool saturation_xy = false; bool saturation_z = false; /* limit min lift */ float thr_min = _params.thr_min; if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) { /* don't allow downside thrust direction in manual attitude mode */ thr_min = 0.0f; } float tilt_max = _params.tilt_max_air; /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; 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; } if (_control_mode.flag_control_velocity_enabled) { /* limit max tilt */ if (thr_min >= 0.0f && tilt_max < M_PI_F / 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 (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) { att_comp = 1.0f / PX4_R(_att.R, 2, 2); } else if (PX4_R(_att.R, 2, 2) > 0.0f) { att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f; saturation_z = true; } else { att_comp = 1.0f; saturation_z = true; } thrust_sp(2) *= att_comp; } /* limit max thrust */ float thrust_abs = thrust_sp.length(); if (thrust_abs > _params.thr_max) { if (thrust_sp(2) < 0.0f) { if (-thrust_sp(2) > _params.thr_max) { /* thrust Z component is too large, limit it */ thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; thrust_sp(2) = -_params.thr_max; saturation_xy = true; saturation_z = true; } else { /* preserve thrust Z component and lower XY, keeping altitude is more important than position */ float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2)); float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); float k = thrust_xy_max / thrust_xy_abs; thrust_sp(0) *= k; thrust_sp(1) *= k; saturation_xy = true; } } else { /* Z component is negative, going down, simply limit thrust vector */ float k = _params.thr_max / thrust_abs; thrust_sp *= k; saturation_xy = true; saturation_z = true; } thrust_abs = _params.thr_max; } /* update integrals */ if (_control_mode.flag_control_velocity_enabled && !saturation_xy) { thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt; thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt; } if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) { thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt; /* protection against flipping on ground when landing */ if (thrust_int(2) > 0.0f) { thrust_int(2) = 0.0f; } } /* calculate attitude setpoint from thrust vector */ if (_control_mode.flag_control_velocity_enabled) { /* desired body_z axis = -normalize(thrust_vector) */ math::Vector<3> body_x; math::Vector<3> body_y; math::Vector<3> body_z; if (thrust_abs > SIGMA) { body_z = -thrust_sp / thrust_abs; } else { /* no thrust, set Z axis to safe value */ body_z.zero(); body_z(2) = 1.0f; } /* vector of desired yaw direction in XY plane, rotated by PI/2 */ math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f); if (fabsf(body_z(2)) > SIGMA) { /* desired body_x axis, orthogonal to body_z */ body_x = y_C % body_z; /* keep nose to front while inverted upside down */ if (body_z(2) < 0.0f) { body_x = -body_x; } body_x.normalize(); } else { /* desired thrust is in XY plane, set X downside to construct correct matrix, * but yaw component will not be used actually */ body_x.zero(); body_x(2) = 1.0f; } /* desired body_y axis */ body_y = body_z % body_x; /* fill rotation matrix */ for (int i = 0; i < 3; i++) { R(i, 0) = body_x(i); R(i, 1) = body_y(i); R(i, 2) = body_z(i); } /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; /* calculate euler angles, for logging only, must not be used for control */ math::Vector<3> euler = R.to_euler(); _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ } else if (!_control_mode.flag_control_manual_enabled) { /* autonomous altitude control without position control (failsafe landing), * force level attitude, don't change yaw */ R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); /* copy rotation matrix to attitude setpoint topic */ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body)); _att_sp.R_valid = true; _att_sp.roll_body = 0.0f; _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; /* save thrust setpoint for logging */ _local_pos_sp.acc_x = thrust_sp(0); _local_pos_sp.acc_x = thrust_sp(1); _local_pos_sp.acc_x = thrust_sp(2); _att_sp.timestamp = hrt_absolute_time(); } else { reset_int_z = true; } } /* fill local position, velocity and thrust setpoint */ _local_pos_sp.timestamp = hrt_absolute_time(); _local_pos_sp.x = _pos_sp(0); _local_pos_sp.y = _pos_sp(1); _local_pos_sp.z = _pos_sp(2); _local_pos_sp.yaw = _att_sp.yaw_body; _local_pos_sp.vx = _vel_sp(0); _local_pos_sp.vy = _vel_sp(1); _local_pos_sp.vz = _vel_sp(2); /* publish local position setpoint */ if (_local_pos_sp_pub > 0) { 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); } } else { /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; _mode_auto = false; reset_int_z = true; reset_int_xy = true; } // generate attitude setpoint from manual controls if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { // reset yaw setpoint to current position if needed if (reset_yaw_sp) { reset_yaw_sp = false; _att_sp.yaw_body = _att.yaw; } // do not move yaw while arming 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; _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); if (yaw_offs < - YAW_OFFSET_MAX) { _att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX); } else if (yaw_offs > YAW_OFFSET_MAX) { _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); } } //Control roll and pitch directly if we no aiding velocity controller is active if(!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } //Control climb rate directly if no aiding altitude controller is active if(!_control_mode.flag_control_climb_rate_enabled) { _att_sp.thrust = _manual.z; } //Construct attitude setpoint rotation matrix math::Matrix<3,3> R_sp; R_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body); memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body)); _att_sp.timestamp = hrt_absolute_time(); } else { reset_yaw_sp = true; } /* publish attitude setpoint * Do not publish if offboard is enabled but position/velocity control is disabled, * in this case the attitude setpoint is published by the mavlink app */ if (!(_control_mode.flag_control_offboard_enabled && !(_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled))) { if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); } else { _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; } warnx("stopped"); mavlink_log_info(_mavlink_fd, "[mpc] stopped"); _control_task = -1; _exit(0); }
int MulticopterPositionControl::parameters_update(bool force) { bool updated; struct parameter_update_s param_upd; orb_check(_params_sub, &updated); if (updated) { orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); } if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); float v; param_get(_params_handles.xy_p, &v); _params.pos_p(0) = v; _params.pos_p(1) = v; param_get(_params_handles.z_p, &v); _params.pos_p(2) = v; param_get(_params_handles.xy_vel_p, &v); _params.vel_p(0) = v; _params.vel_p(1) = v; param_get(_params_handles.z_vel_p, &v); _params.vel_p(2) = v; param_get(_params_handles.xy_vel_i, &v); _params.vel_i(0) = v; _params.vel_i(1) = v; param_get(_params_handles.z_vel_i, &v); _params.vel_i(2) = v; param_get(_params_handles.xy_vel_d, &v); _params.vel_d(0) = v; _params.vel_d(1) = v; param_get(_params_handles.z_vel_d, &v); _params.vel_d(2) = v; param_get(_params_handles.xy_vel_max, &v); _params.vel_max(0) = v; _params.vel_max(1) = v; param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; /* mc attitude control parameters*/ /* manual control scale */ param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); param_get(_params_handles.mc_att_yaw_p,&v); _params.mc_att_yaw_p = v; } return OK; }
int MulticopterPositionControl::parameters_update(bool force) { bool updated; struct parameter_update_s param_upd; orb_check(_params_sub, &updated); if (updated) { orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_upd); } if (updated || force) { param_get(_params_handles.thr_min, &_params.thr_min); param_get(_params_handles.thr_max, &_params.thr_max); param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); float v; param_get(_params_handles.xy_p, &v); _params.pos_p(0) = v; _params.pos_p(1) = v; param_get(_params_handles.z_p, &v); _params.pos_p(2) = v; param_get(_params_handles.xy_vel_p, &v); _params.vel_p(0) = v; _params.vel_p(1) = v; param_get(_params_handles.z_vel_p, &v); _params.vel_p(2) = v; param_get(_params_handles.xy_vel_i, &v); _params.vel_i(0) = v; _params.vel_i(1) = v; param_get(_params_handles.z_vel_i, &v); _params.vel_i(2) = v; param_get(_params_handles.xy_vel_d, &v); _params.vel_d(0) = v; _params.vel_d(1) = v; param_get(_params_handles.z_vel_d, &v); _params.vel_d(2) = v; param_get(_params_handles.xy_vel_max, &v); _params.vel_max(0) = v; _params.vel_max(1) = v; param_get(_params_handles.z_vel_max, &v); _params.vel_max(2) = v; param_get(_params_handles.xy_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(0) = v; _params.vel_ff(1) = v; param_get(_params_handles.z_ff, &v); v = math::constrain(v, 0.0f, 1.0f); _params.vel_ff(2) = v; _params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f; /* Update of sonar parameters */ param_get(_sonar_params_handles._snr_activate_sw_h,&_sonar_params._snr_activate_sw); param_get(_sonar_params_handles._snr_fwd_min_th_h,&_sonar_params._snr_fwd_min_th); param_get(_sonar_params_handles._snr_fwd_stab_ratio_h,&_sonar_params._snr_fwd_stab_ratio); param_get(_sonar_params_handles._snr_fwd_wall_ratio_h,&_sonar_params._snr_fwd_wall_ratio); param_get(_sonar_params_handles._snr_fwd_max_response_h,&_sonar_params._snr_fwd_max_response); param_get(_sonar_params_handles._snr_fwd_max_th_h,&_sonar_params._snr_fwd_max_th); param_get(_sonar_params_handles._snr_fwd_adc_ind_h,&_sonar_params._snr_fwd_adc_ind); if(_sonar_params._snr_activate_sw != 0) { _snr_activated_flag = true; } else { _snr_activated_flag = false; } } return OK; }
void derivatives(const StateMatrix& states, StateMatrix& derivs, const ControlMatrix& ctrls, const SimulationParameters& params, const Map & map) { float cd_a_rho = params.linearDrag; // 0.1 coeff of drag * area * density of fluid float k_elastic = params.elasticity; // 4000. // spring constant of ships float rad = 1.; // leave radius 1 - we decided to change map scale instead const Eigen::VectorXf &masses = params.shipDensities; // order of 1.0 Mass of ships float spin_drag_ratio = params.rotationalDrag; // 1.8; // spin friction to translation friction float eps = 1e-5; // Avoid divide by zero special cases float mu = params.shipFriction; // 0.05; // friction coefficient between ships float mu_wall = params.wallFriction; //0.25*?wallFriction; // 0.01; // wall friction parameter float wall_restitution = params.wallRestitution; // 0.5 float ship_restitution = params.shipRestitution; // circa 0.5 float diameter = 2.*rad; // rad(i) + rad(j) for any i, j float inertia_mass_ratio = 0.25; float map_grid = rad * 2. + eps; // must be 2*radius + eps std::unordered_map<std::pair<int, int>, std::vector<uint>, boost::hash<std::pair<int, int>>> bins; uint n = states.rows(); Eigen::MatrixXd f = Eigen::MatrixXd::Zero(n, 2); Eigen::VectorXd trq = Eigen::VectorXd::Zero(n); // rotationalThrust Order +- 10 // linearThrust Order +100 // mapscale order 10 - thats params.pixelsize // Accumulate forces and torques into these: uint collide_checks = 0; // debug count... for (uint i=0; i<n; i++) { Eigen::Vector2f pos_i; pos_i(0) = states(i,0); pos_i(1) = states(i,1); Eigen::Vector2f vel_i; vel_i(0) = states(i,2); vel_i(1) = states(i,3); float theta_i = states(i,4); float w_i = states(i,5); // 1. Control float thrusting = ctrls(i, 0); float turning = ctrls(i, 1); f(i, 0) = thrusting * params.linearThrust * cos(theta_i); f(i, 1) = thrusting * params.linearThrust * sin(theta_i); trq(i) = turning * params.rotationalThrust; // 2. Drag f(i, 0) -= cd_a_rho * vel_i(0); f(i, 1) -= cd_a_rho * vel_i(1); trq(i) -= spin_drag_ratio*cd_a_rho*w_i*rad*rad; // * abs(w_i) // 3. Inter-ship collisions against ships of lower index... // Figure out this ship's hashes: It has 4 in 2 dimensions std::unordered_set<uint> collision_shortlist; std::pair<int, int> my_hash; for (int dx=-1; dx < 2; dx+=2) for (int dy=-1; dy < 2; dy+=2) { float x_mod = pos_i(0) + float(dx)*rad; float y_mod = pos_i(1) + float(dy)*rad; my_hash = std::make_pair(int(x_mod / map_grid), int(y_mod / map_grid)); if (bins.count(my_hash) > 0) { // Already exists - shortlist others and add self std::vector<uint> current_bin = bins.find(my_hash)->second; // -->first is the key as it returns a key/value pair for (uint bin_idx: current_bin) if (bin_idx != i) collision_shortlist.insert(bin_idx); current_bin.push_back(i); } else { // didnt exist - add self, and push into map std::vector<uint> current_bin; current_bin.push_back(i); bins.insert(std::make_pair(my_hash, current_bin)); } } for (uint j: collision_shortlist) { // =i+1; j<n; j++) { collide_checks ++; // std::cout << "Checking " << i << ", " << j << "\n"; Eigen::Vector2f pos_j; pos_j(0) = states(j,0); pos_j(1) = states(j,1); Eigen::Vector2f vel_j; vel_j(0) = states(j,2); vel_j(1) = states(j,3); float theta_j = states(j,4); float w_j = states(j,5); Eigen::Vector2f dP = pos_j - pos_i; float dist = dP.norm() + eps - diameter; Eigen::Vector2f dPhat = dP / (dP.norm() + eps); if (dist < 0) { // we have a collision interaction // A. Direct collision: apply linear spring normal force float f_magnitude = - dist * k_elastic; // dist < = if ((vel_j - vel_i).dot(pos_j - pos_i) > 0) f_magnitude *= ship_restitution; Eigen::Vector2f f_norm = f_magnitude * dPhat; f(i, 0) -= f_norm(0); f(i, 1) -= f_norm(1); f(j, 0) += f_norm(0); f(j, 1) += f_norm(1); // B. Surface frictions: approximate spin effects Eigen::Vector2f perp; // surface tangent pointing +theta direction perp(0) = -dPhat(1); perp(1) = dPhat(0); // relative velocities of surfaces float v_rel = rad*w_i + rad*w_j + perp.dot(vel_i - vel_j); float fric = f_magnitude * mu * sigmoid(v_rel); Eigen::Vector2f f_fric = fric * perp; f(i, 0) += f_fric(0); f(i, 1) += f_fric(1); f(j, 0) -= f_fric(0); f(j, 1) -= f_fric(1); trq(i) -= fric * rad; trq(j) -= fric * rad; } // end collision } // end loop 3. opposing ship // 4. Wall single body collisions // compute distance to wall and local normals float wall_dist, norm_x, norm_y; interpolate_map(pos_i(0), pos_i(1), wall_dist, norm_x, norm_y, map, params); float dist = wall_dist - rad; if (dist < 0) { /* if (dist < -1.) */ /* assert(false); */ // Spring force float f_norm_mag = -dist*k_elastic; // dist is negative, f_norm is +ve if (norm_x*vel_i(0) + norm_y*vel_i(1) > 0) f_norm_mag *= wall_restitution; if (dist > -rad*0.25) { // not significantly through wall yet f(i, 0) += f_norm_mag * norm_x; f(i, 1) += f_norm_mag * norm_y; } else { // uh-oh - lets just SET normal forces and seriously damp vel f(i, 0) = f_norm_mag * norm_x; f(i, 1) = f_norm_mag * norm_y; f(i, 0) -= 100. * vel_i(0); f(i, 1) -= 100. * vel_i(1); } // Surface friction Eigen::Vector2f perp; // surface tangent pointing +theta direction perp(0) = -norm_y; perp(1) = norm_x; float v_rel = w_i * rad + vel_i(0)*norm_y - vel_i(1)*norm_x; float fric = f_norm_mag * mu_wall * sigmoid(v_rel); f(i, 0) -= fric*norm_y; f(i, 1) += fric*norm_x; trq(i) -= fric * rad; } } // end loop current ship // std::cout << "Collision checks:" << collide_checks << "\n"; // Compose the vector of derivatives: float vmax = 40.0; for (int i=0; i<n; i++) { float vx = states(i,2); float vy = states(i,3); float speed = std::sqrt(vx*vx + vy*vy); if (speed > vmax) { vx *= vmax/speed; vy *= vmax/speed; } // x_dot = vx derivs(i, 0) = vx; // y_dot = vy derivs(i, 1) = vy; // vx_dot = fx / m float ax = f(i,0)/masses(i); float ay = f(i,1)/masses(i); derivs(i, 2) = ax; // vy_dot = fy / m derivs(i, 3) = ay; // theta_dot = omega derivs(i, 4) = states(i, 5); // omega_dot = T_r / (inertia_mass_ratio*m) derivs(i,5) = trq(i) / (inertia_mass_ratio * masses(i)); } // ux uy vx vy theta omega // 0 1 2 3 4 5 }