void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ float eta; /* * As the commanded heading is the only reference * (and no crosstrack correction occurs), * target and navigation bearing become the same */ _target_bearing = _nav_bearing = _wrap_pi(navigation_heading); eta = _target_bearing - _wrap_pi(current_heading); eta = _wrap_pi(eta); /* consequently the bearing error is exactly eta: */ _bearing_error = eta; /* ground speed is the length of the ground speed vector */ float ground_speed = ground_speed_vector.length(); /* adjust L1 distance to keep constant frequency */ _L1_distance = ground_speed / _heading_omega; float omega_vel = ground_speed * _heading_omega; /* not circling a waypoint */ _circle_mode = false; /* navigating heading means by definition no crosstrack error */ _crosstrack_error = 0; /* limit eta to 90 degrees */ eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); _lateral_accel = 2.0f * sinf(eta) * omega_vel; }
__EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) { // This function returns the distance to the nearest point on the track line. Distance is positive if current // position is right of the track and negative if left of the track as seen from a point on the track line // headed towards the end point. float dist_to_end; float bearing_end; float bearing_track; float bearing_diff; int return_value = ERROR; // Set error flag, cleared when valid result calculated. crosstrack_error->past_end = false; crosstrack_error->distance = 0.0f; crosstrack_error->bearing = 0.0f; dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); // Return error if arguments are bad if (dist_to_end < 0.1f) { return ERROR; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); bearing_diff = bearing_track - bearing_end; bearing_diff = _wrap_pi(bearing_diff); // Return past_end = true if past end point of line if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { crosstrack_error->past_end = true; return_value = OK; return return_value; } crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); } else { crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); } return_value = OK; return return_value; }
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data) { /* Do not calculate control signal with bad inputs */ if (!(ISFINITE(ctl_data.yaw_setpoint) && ISFINITE(ctl_data.yaw))) { return _rate_setpoint; } /* Calculate the error */ float yaw_error = _wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw); /* Apply P controller: rate setpoint from current error and time constant */ _rate_setpoint = yaw_error / _tc; /* limit the rate */ if (_max_rate > 0.01f) { if (_rate_setpoint > 0.0f) { _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint; } else { _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint; } } return _rate_setpoint; }
int MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { /* only support global waypoints for now */ switch (mavlink_mission_item->frame) { case MAV_FRAME_GLOBAL: mission_item->lat = (double)mavlink_mission_item->x; mission_item->lon = (double)mavlink_mission_item->y; mission_item->altitude = mavlink_mission_item->z; mission_item->altitude_is_relative = false; break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: mission_item->lat = (double)mavlink_mission_item->x; mission_item->lon = (double)mavlink_mission_item->y; mission_item->altitude = mavlink_mission_item->z; mission_item->altitude_is_relative = true; break; case MAV_FRAME_LOCAL_NED: case MAV_FRAME_LOCAL_ENU: return MAV_MISSION_UNSUPPORTED_FRAME; case MAV_FRAME_MISSION: default: return MAV_MISSION_ERROR; } switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: mission_item->pitch_min = mavlink_mission_item->param1; break; case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; mission_item->do_jump_current_count = 0; mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; mission_item->time_inside = mavlink_mission_item->param1; break; } mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; /* reset DO_JUMP count */ mission_item->do_jump_current_count = 0; return MAV_MISSION_ACCEPTED; }
void Delivery::heading_sp_update() { if (_takeoff) { /* we don't want to be yawing during takeoff */ return; } struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !isfinite(_on_arrival_yaw)) { return; } /* Don't do FOH for landing and takeoff waypoints, the ground may be near * and the FW controller has a custom landing logic */ if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { return; } /* set yaw angle for the waypoint iff a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { _mission_item.yaw = _on_arrival_yaw; /* always keep the front of the rotary wing pointing to the next waypoint */ } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _mission_item.lat, _mission_item.lon); /* always keep the back of the rotary wing pointing towards home */ } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { _mission_item.yaw = get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); /* always keep the back of the rotary wing pointing towards home */ } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_home_position()->lat, _navigator->get_home_position()->lon) + M_PI_F); } mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); }
__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); theta = _wrap_pi(theta); return theta; }
void MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) { mavlink_hil_gps_t gps; mavlink_msg_hil_gps_decode(msg, &gps); uint64_t timestamp = hrt_absolute_time(); struct vehicle_gps_position_s hil_gps; memset(&hil_gps, 0, sizeof(hil_gps)); hil_gps.timestamp_time = timestamp; hil_gps.time_gps_usec = gps.time_usec; hil_gps.timestamp_position = timestamp; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m hil_gps.vel_ned_valid = true; hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f); hil_gps.timestamp_satellites = timestamp; hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; if (_gps_pub < 0) { _gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } else { orb_publish(ORB_ID(vehicle_gps_position), _gps_pub, &hil_gps); } }
int set_vehicle_global_position_setpoint (int index, float yaw_sp, struct vehicle_global_position_setpoint_s* g_sp) { mission_command_t cmd = mission.command_list[index]; float navigation_alt, navigation_lat, navigation_lon; navigation_alt = cmd.option1; navigation_lat = (cmd.name == accepted_command_rtl || cmd.name == accepted_command_land)? home_position.latitude : cmd.option2; navigation_lon = (cmd.name == accepted_command_rtl || cmd.name == accepted_command_land)? home_position.longitude : cmd.option3; /* Update setpoints */ g_sp->altitude = navigation_alt; g_sp->latitude = navigation_lat * 1e7f; g_sp->longitude = navigation_lon * 1e7f; g_sp->altitude_is_relative = 0; g_sp->yaw = _wrap_pi(yaw_sp / 180.0f * M_PI); return set_special_fields(cmd.name, cmd.option4, cmd.option5, g_sp); }
bool MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { case NAV_CMD_DO_SET_SERVO: return true; case NAV_CMD_LAND: /* fall through */ case NAV_CMD_VTOL_LAND: return _navigator->get_land_detected()->landed; case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: return false; case NAV_CMD_DO_LAND_START: case NAV_CMD_DO_TRIGGER_CONTROL: case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: case NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL: case NAV_CMD_SET_CAMERA_MODE: return true; case NAV_CMD_DO_VTOL_TRANSITION: /* * We wait half a second to give the transition command time to propagate. * Then monitor the transition status for completion. */ // TODO: check desired transition state achieved and drop _action_start if (hrt_absolute_time() - _action_start > 500000 && !_navigator->get_vstatus()->in_transition_mode) { _action_start = 0; return true; } else { return false; } case NAV_CMD_DO_CHANGE_SPEED: return true; default: /* do nothing, this is a 3D waypoint */ break; } hrt_abstime now = hrt_absolute_time(); if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); /* FW special case for NAV_CMD_WAYPOINT to achieve altitude via loiter */ if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_WAYPOINT)) { struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; /* close to waypoint, but altitude error greater than twice acceptance */ if ((dist >= 0.0f) && (dist_z > 2 * _navigator->get_altitude_acceptance_radius()) && (dist_xy < 2 * _navigator->get_loiter_radius())) { /* SETPOINT_TYPE_POSITION -> SETPOINT_TYPE_LOITER */ if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER; curr_sp->loiter_radius = _navigator->get_loiter_radius(); curr_sp->loiter_direction = 1; _navigator->set_position_setpoint_triplet_updated(); } } else { /* restore SETPOINT_TYPE_POSITION */ if (curr_sp->type == position_setpoint_s::SETPOINT_TYPE_LOITER) { /* loiter acceptance criteria required to revert back to SETPOINT_TYPE_POSITION */ if ((dist >= 0.0f) && (dist_z < _navigator->get_loiter_radius()) && (dist_xy <= _navigator->get_loiter_radius() * 1.2f)) { curr_sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; _navigator->set_position_setpoint_triplet_updated(); } } } } if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->is_rotary_wing) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow * take-off procedures like leaving the landing gear down. */ float takeoff_alt = _mission_item.altitude_is_relative ? _mission_item.altitude : (_mission_item.altitude - _navigator->get_home_position()->alt); float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); /* It should be safe to just use half of the takoeff_alt as an acceptance radius. */ if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { altitude_acceptance_radius = takeoff_alt / 2.0f; } /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } else { _time_first_inside_orbit = 0; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter // first check if the altitude setpoint is the mission setpoint struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) { // check if the initial loiter has been accepted dist_xy = -1.0f; dist_z = -1.0f; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item curr_sp->alt = altitude_amsl; _navigator->set_position_setpoint_triplet_updated(); } } else { if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; // set required yaw from bearing to the next mission item if (_mission_item.force_heading) { struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; if (next_sp.valid) { _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, next_sp.lat, next_sp.lon); _waypoint_yaw_reached = false; } else { _waypoint_yaw_reached = true; } } } } } else if (_mission_item.nav_cmd == NAV_CMD_DELAY) { _waypoint_position_reached = true; _waypoint_yaw_reached = true; _time_wp_reached = now; } else { /* for normal mission items used their acceptance radius */ float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); /* if set to zero use the default instead */ if (mission_acceptance_radius < NAV_EPSILON_POSITION) { mission_acceptance_radius = _navigator->get_acceptance_radius(); } /* for vtol back transition calculate acceptance radius based on time and ground speed */ if (_mission_item.vtol_back_transition) { float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx + _navigator->get_local_position()->vy * _navigator->get_local_position()->vy); if (_param_back_trans_dec_mss.get() > FLT_EPSILON && velocity > FLT_EPSILON) { mission_acceptance_radius = ((velocity / _param_back_trans_dec_mss.get() / 2) * velocity) + _param_reverse_delay.get() * velocity; } } if (dist >= 0.0f && dist <= mission_acceptance_radius && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } if (_waypoint_position_reached) { // reached just now _time_wp_reached = now; } } /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { if ((_navigator->get_vstatus()->is_rotary_wing || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading)) && PX4_ISFINITE(_mission_item.yaw)) { /* check course if defined only for rotary wing except takeoff */ float cog = _navigator->get_vstatus()->is_rotary_wing ? _navigator->get_global_position()->yaw : atan2f( _navigator->get_global_position()->vel_e, _navigator->get_global_position()->vel_n); float yaw_err = _wrap_pi(_mission_item.yaw - cog); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && (_param_yaw_timeout.get() >= FLT_EPSILON) && (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { _navigator->set_mission_failure("unable to reach heading within timeout"); } } else { _waypoint_yaw_reached = true; } } /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; } /* check if the MAV was long enough inside the waypoint orbit */ if ((get_time_inside(_mission_item) < FLT_EPSILON) || (now - _time_first_inside_orbit >= (hrt_abstime)(get_time_inside(_mission_item) * 1e6f))) { position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current; const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next; const float range = get_distance_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); // exit xtrack location // reset lat/lon of loiter waypoint so vehicle follows a tangent if (_mission_item.loiter_exit_xtrack && next_sp.valid && PX4_ISFINITE(range) && (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon); float inner_angle = M_PI_2_F - asinf(_mission_item.loiter_radius / range); // Compute "ideal" tangent origin if (curr_sp.loiter_direction > 0) { bearing -= inner_angle; } else { bearing += inner_angle; } // Replace current setpoint lat/lon with tangent coordinate waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon, bearing, curr_sp.loiter_radius, &curr_sp.lat, &curr_sp.lon); } return true; } } // all acceptance criteria must be met in the same iteration _waypoint_position_reached = false; _waypoint_yaw_reached = false; return false; }
/** * This callback is executed each time a waypoint changes. * * It publishes the vehicle_global_position_setpoint_s or the * vehicle_local_position_setpoint_s topic, depending on the type of waypoint */ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, float param2, float param3, float param4, float param5_lat_x, float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command) { static orb_advert_t global_position_setpoint_pub = -1; static orb_advert_t global_position_set_triplet_pub = -1; static orb_advert_t local_position_setpoint_pub = -1; static unsigned last_waypoint_index = -1; char buf[50] = {0}; // XXX include check if WP is supported, jump to next if not /* Update controller setpoints */ if (frame == (int)MAV_FRAME_GLOBAL) { /* global, absolute waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = false; sp.yaw = _wrap_pi(param4 / 180.0f * M_PI_F); set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize setpoint publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } /* fill triplet: previous, current, next waypoint */ struct vehicle_global_position_set_triplet_s triplet; /* current waypoint is same as sp */ memcpy(&(triplet.current), &sp, sizeof(sp)); /* * Check if previous WP (in mission, not in execution order) * is available and identify correct index */ int last_setpoint_index = -1; bool last_setpoint_valid = false; if (index > 0) { last_setpoint_index = index - 1; } while (last_setpoint_index >= 0) { if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { last_setpoint_valid = true; break; } last_setpoint_index--; } /* * Check if next WP (in mission, not in execution order) * is available and identify correct index */ int next_setpoint_index = -1; bool next_setpoint_valid = false; /* next waypoint */ if (wpm->size > 1) { next_setpoint_index = index + 1; } while (next_setpoint_index < wpm->size - 1) { if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) { next_setpoint_valid = true; break; } next_setpoint_index++; } /* populate last and next */ triplet.previous_valid = false; triplet.next_valid = false; if (last_setpoint_valid) { triplet.previous_valid = true; struct vehicle_global_position_setpoint_s sp; sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f; sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[last_setpoint_index].z; sp.altitude_is_relative = false; sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(wpm->waypoints[last_setpoint_index].param1, wpm->waypoints[last_setpoint_index].param2, wpm->waypoints[last_setpoint_index].param3, wpm->waypoints[last_setpoint_index].param4, wpm->waypoints[last_setpoint_index].command, &sp); memcpy(&(triplet.previous), &sp, sizeof(sp)); } if (next_setpoint_valid) { triplet.next_valid = true; struct vehicle_global_position_setpoint_s sp; sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f; sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f; sp.altitude = wpm->waypoints[next_setpoint_index].z; sp.altitude_is_relative = false; sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(wpm->waypoints[next_setpoint_index].param1, wpm->waypoints[next_setpoint_index].param2, wpm->waypoints[next_setpoint_index].param3, wpm->waypoints[next_setpoint_index].param4, wpm->waypoints[next_setpoint_index].command, &sp); memcpy(&(triplet.next), &sp, sizeof(sp)); } /* Initialize triplet publication if necessary */ if (global_position_set_triplet_pub < 0) { global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet); } else { orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet); } sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { /* global, relative alt (in relation to HOME) waypoint */ struct vehicle_global_position_setpoint_s sp; sp.lat = param5_lat_x * 1e7f; sp.lon = param6_lon_y * 1e7f; sp.altitude = param7_alt_z; sp.altitude_is_relative = true; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; set_special_fields(param1, param2, param3, param4, command, &sp); /* Initialize publication if necessary */ if (global_position_setpoint_pub < 0) { global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) { /* local, absolute waypoint */ struct vehicle_local_position_setpoint_s sp; sp.x = param5_lat_x; sp.y = param6_lon_y; sp.z = param7_alt_z; sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F; /* Initialize publication if necessary */ if (local_position_setpoint_pub < 0) { local_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &sp); } else { orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); } sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4); } else { warnx("non-navigation WP, ignoring"); mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring."); return; } /* only set this for known waypoint types (non-navigation types would have returned earlier) */ last_waypoint_index = index; mavlink_missionlib_send_gcs_string(buf); printf("%s\n", buf); //printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); }
bool MissionBlock::is_mission_item_reached() { if (_mission_item.nav_cmd == NAV_CMD_IDLE) { return false; } if (_mission_item.nav_cmd == NAV_CMD_LAND) { return _navigator->get_vstatus()->condition_landed; } /* TODO: count turns */ if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { return false; } hrt_abstime now = hrt_absolute_time(); if (!_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > altitude_amsl - _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { _waypoint_position_reached = true; } } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { _waypoint_position_reached = true; } } } if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ _waypoint_yaw_reached = true; } } else { _waypoint_yaw_reached = true; } } /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; // if (_mission_item.time_inside > 0.01f) { // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", // (double)_mission_item.time_inside); // } } /* check if the MAV was long enough inside the waypoint orbit */ if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { return true; } } return false; }
int MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL || mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { if (_int_mode) { /* The argument is actually a mavlink_mission_item_int_t in int_mode. * mavlink_mission_item_t and mavlink_mission_item_int_t have the same * alignment, so we can just swap float for int32_t. */ const mavlink_mission_item_int_t *item_int = reinterpret_cast<const mavlink_mission_item_int_t *>(mavlink_mission_item); mission_item->lat = ((double)item_int->x)*1e-7; mission_item->lon = ((double)item_int->y)*1e-7; } else { mission_item->lat = (double)mavlink_mission_item->x; mission_item->lon = (double)mavlink_mission_item->y; } mission_item->altitude = mavlink_mission_item->z; if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL) { mission_item->altitude_is_relative = false; } else if (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT) { mission_item->altitude_is_relative = true; } mission_item->time_inside = 0.0f; switch (mavlink_mission_item->command) { case MAV_CMD_NAV_WAYPOINT: mission_item->nav_cmd = NAV_CMD_WAYPOINT; mission_item->time_inside = mavlink_mission_item->param1; mission_item->acceptance_radius = mavlink_mission_item->param2; mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; case MAV_CMD_NAV_LOITER_UNLIM: mission_item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; case MAV_CMD_NAV_LOITER_TIME: mission_item->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; mission_item->time_inside = mavlink_mission_item->param1; mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; break; case MAV_CMD_NAV_LAND: mission_item->nav_cmd = NAV_CMD_LAND; // TODO: abort alt param1 mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; case MAV_CMD_NAV_TAKEOFF: mission_item->nav_cmd = NAV_CMD_TAKEOFF; mission_item->pitch_min = mavlink_mission_item->param1; mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; case MAV_CMD_NAV_LOITER_TO_ALT: mission_item->nav_cmd = NAV_CMD_LOITER_TO_ALT; mission_item->force_heading = (mavlink_mission_item->param1 > 0) ? true : false; mission_item->loiter_radius = fabsf(mavlink_mission_item->param2); mission_item->loiter_direction = (mavlink_mission_item->param2 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ mission_item->loiter_exit_xtrack = (mavlink_mission_item->param4 > 0) ? true : false; break; case MAV_CMD_NAV_VTOL_TAKEOFF: case MAV_CMD_NAV_VTOL_LAND: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); break; default: mission_item->nav_cmd = NAV_CMD_INVALID; return MAV_MISSION_UNSUPPORTED; } mission_item->frame = mavlink_mission_item->frame; } else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) { // this is a mission item with no coordinates mission_item->params[0] = mavlink_mission_item->param1; mission_item->params[1] = mavlink_mission_item->param2; mission_item->params[2] = mavlink_mission_item->param3; mission_item->params[3] = mavlink_mission_item->param4; mission_item->params[4] = mavlink_mission_item->x; mission_item->params[5] = mavlink_mission_item->y; mission_item->params[6] = mavlink_mission_item->z; switch (mavlink_mission_item->command) { case MAV_CMD_DO_JUMP: mission_item->nav_cmd = NAV_CMD_DO_JUMP; mission_item->do_jump_mission_index = mavlink_mission_item->param1; mission_item->do_jump_current_count = 0; mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; case MAV_CMD_DO_CHANGE_SPEED: case MAV_CMD_DO_SET_SERVO: case MAV_CMD_DO_DIGICAM_CONTROL: case MAV_CMD_DO_MOUNT_CONFIGURE: case MAV_CMD_DO_MOUNT_CONTROL: case MAV_CMD_DO_SET_CAM_TRIGG_DIST: case MAV_CMD_DO_VTOL_TRANSITION: mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; break; default: mission_item->nav_cmd = NAV_CMD_INVALID; return MAV_MISSION_UNSUPPORTED; } mission_item->frame = MAV_FRAME_MISSION; } else { return MAV_MISSION_UNSUPPORTED_FRAME; } mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; /* reset DO_JUMP count */ mission_item->do_jump_current_count = 0; mission_item->origin = ORIGIN_MAVLINK; return MAV_MISSION_ACCEPTED; }
float ECL_L1_Pos_Controller::nav_bearing() { return _wrap_pi(_nav_bearing); }
bool MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { case NAV_CMD_DO_SET_SERVO: return true; case NAV_CMD_LAND: /* fall through */ case NAV_CMD_VTOL_LAND: return _navigator->get_land_detected()->landed; case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: return false; case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_IMAGE_START_CAPTURE: case NAV_CMD_IMAGE_STOP_CAPTURE: case NAV_CMD_VIDEO_START_CAPTURE: case NAV_CMD_VIDEO_STOP_CAPTURE: case NAV_CMD_DO_MOUNT_CONFIGURE: case NAV_CMD_DO_MOUNT_CONTROL: case NAV_CMD_DO_SET_ROI: case NAV_CMD_ROI: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: return true; case NAV_CMD_DO_VTOL_TRANSITION: /* * We wait half a second to give the transition command time to propagate. * Then monitor the transition status for completion. */ if (hrt_absolute_time() - _action_start > 500000 && !_navigator->get_vstatus()->in_transition_mode) { _action_start = 0; return true; } else { return false; } case NAV_CMD_DO_CHANGE_SPEED: // XXX not differentiating ground and airspeed yet if (_mission_item.params[1] > 0.0f) { _navigator->set_cruising_speed(_mission_item.params[1]); } else { _navigator->set_cruising_speed(); /* if no speed target was given try to set throttle */ if (_mission_item.params[2] > 0.0f) { _navigator->set_cruising_throttle(_mission_item.params[2] / 100); } else { _navigator->set_cruising_throttle(); } } return true; default: /* do nothing, this is a 3D waypoint */ break; } hrt_abstime now = hrt_absolute_time(); if ((_navigator->get_land_detected()->landed == false) && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) && _navigator->get_vstatus()->is_rotary_wing) { /* We want to avoid the edge case where the acceptance radius is bigger or equal than * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow * take-off procedures like leaving the landing gear down. */ float takeoff_alt = _mission_item.altitude_is_relative ? _mission_item.altitude : (_mission_item.altitude - _navigator->get_home_position()->alt); float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius(); /* It should be safe to just use half of the takoeff_alt as an acceptance radius. */ if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) { altitude_acceptance_radius = takeoff_alt / 2.0f; } /* require only altitude for takeoff for multicopter */ if (_navigator->get_global_position()->alt > altitude_amsl - altitude_acceptance_radius) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius() && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } else { _time_first_inside_orbit = 0; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // NAV_CMD_LOITER_TO_ALT only uses mission item altitude once it's in the loiter // first check if the altitude setpoint is the mission setpoint struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; if (fabsf(curr_sp->alt - altitude_amsl) >= FLT_EPSILON) { // check if the initial loiter has been accepted dist = -1.0f; dist_xy = -1.0f; dist_z = -1.0f; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { // now set the loiter to the final altitude in the NAV_CMD_LOITER_TO_ALT mission item curr_sp->alt = altitude_amsl; _navigator->set_position_setpoint_triplet_updated(); } } else { if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(fabsf(_mission_item.loiter_radius) * 1.2f) && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; // set required yaw from bearing to the next mission item if (_mission_item.force_heading) { struct position_setpoint_s next_sp = _navigator->get_position_setpoint_triplet()->next; if (next_sp.valid) { _mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, next_sp.lat, next_sp.lon); _waypoint_yaw_reached = false; } else { _waypoint_yaw_reached = true; } } } } } else { /* for normal mission items used their acceptance radius */ float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); /* if set to zero use the default instead */ if (mission_acceptance_radius < NAV_EPSILON_POSITION) { mission_acceptance_radius = _navigator->get_acceptance_radius(); } if (dist >= 0.0f && dist <= mission_acceptance_radius && dist_z <= _navigator->get_altitude_acceptance_radius()) { _waypoint_position_reached = true; } } if (_waypoint_position_reached) { // reached just now _time_wp_reached = now; } } /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { if ((_navigator->get_vstatus()->is_rotary_wing || (_mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT && _mission_item.force_heading)) && PX4_ISFINITE(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && (_param_yaw_timeout.get() >= FLT_EPSILON) && (now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f)) { _navigator->set_mission_failure("unable to reach heading within timeout"); } } else { _waypoint_yaw_reached = true; } } /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; } /* check if the MAV was long enough inside the waypoint orbit */ if ((Navigator::get_time_inside(_mission_item) < FLT_EPSILON) || (now - _time_first_inside_orbit >= (hrt_abstime)(Navigator::get_time_inside(_mission_item) * 1e6f))) { // exit xtrack location if (_mission_item.loiter_exit_xtrack && (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT)) { // reset lat/lon of loiter waypoint so vehicle exits on a tangent struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; curr_sp->lat = _navigator->get_global_position()->lat; curr_sp->lon = _navigator->get_global_position()->lon; } return true; } } // all acceptance criteria must be met in the same iteration _waypoint_position_reached = false; _waypoint_yaw_reached = false; return false; }
void Standard::update_mc_state() { VtolType::update_mc_state(); // enable MC motors here in case we transitioned directly to MC mode if (_flag_enable_mc_motors) { set_max_mc(2000); set_idle_mc(); _flag_enable_mc_motors = false; } // if the thrust scale param is zero or the drone is on manual mode, // then the pusher-for-pitch strategy is disabled and we can return if (_params_standard.forward_thrust_scale < FLT_EPSILON || !_v_control_mode->flag_control_position_enabled) { return; } // Do not engage pusher assist during a failsafe event // There could be a problem with the fixed wing drive if (_attc->get_vtol_vehicle_status()->vtol_transition_failsafe) { return; } // disable pusher assist during landing if (_attc->get_pos_sp_triplet()->current.valid && _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { return; } matrix::Dcmf R(matrix::Quatf(_v_att->q)); matrix::Dcmf R_sp(matrix::Quatf(_v_att_sp->q_d)); matrix::Eulerf euler(R); matrix::Eulerf euler_sp(R_sp); _pusher_throttle = 0.0f; // direction of desired body z axis represented in earth frame matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); // rotate desired body z axis into new frame which is rotated in z by the current // heading of the vehicle. we refer to this as the heading frame. matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2)); body_z_sp = R_yaw * body_z_sp; body_z_sp.normalize(); // calculate the desired pitch seen in the heading frame // this value corresponds to the amount the vehicle would try to pitch forward float pitch_forward = atan2f(body_z_sp(0), body_z_sp(2)); // only allow pitching forward up to threshold, the rest of the desired // forward acceleration will be compensated by the pusher if (pitch_forward < -_params_standard.down_pitch_max) { // desired roll angle in heading frame stays the same float roll_new = -asinf(body_z_sp(1)); _pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) * _params_standard.forward_thrust_scale; // return the vehicle to level position float pitch_new = 0.0f; // create corrected desired body z axis in heading frame matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f); matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); // rotate the vector into a new frame which is rotated in z by the desired heading // with respect to the earh frame. float yaw_error = _wrap_pi(euler_sp(2) - euler(2)); matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error); tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); _v_att_sp->roll_body = -asinf(tilt_new(1)); R_sp = matrix::Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2)); matrix::Quatf q_sp(R_sp); q_sp.copyTo(_v_att_sp->q_d); } _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle; }
void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) { static uint16_t counter; if (!global_pos->valid && !local_pos->xy_valid) { /* nothing to check here, return */ return; } if (wpm->current_active_wp_id < wpm->size) { float orbit; if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) { orbit = wpm->waypoints[wpm->current_active_wp_id].param2; } else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { orbit = wpm->waypoints[wpm->current_active_wp_id].param3; } else { // XXX set default orbit via param orbit = 15.0f; } int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { /* Check if conditions of mission item are satisfied */ // XXX TODO } if (dist >= 0.f && dist <= orbit) { wpm->pos_reached = true; } // check if required yaw reached float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); if (fabsf(yaw_err) < 0.05f) { wpm->yaw_reached = true; } } //check if the current waypoint was reached if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { if (wpm->current_active_wp_id < wpm->size) { mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); if (wpm->timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached mavlink_wpm_send_waypoint_reached(cur_wp->seq); wpm->timestamp_firstinside_orbit = now; } // check if the MAV was long enough inside the waypoint orbit //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) bool time_elapsed = false; if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { time_elapsed = true; } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { time_elapsed = true; } if (time_elapsed) { if (cur_wp->autocontinue) { cur_wp->current = 0; float navigation_lat = -1.0f; float navigation_lon = -1.0f; float navigation_alt = -1.0f; int navigation_frame = -1; /* initialize to current position in case we don't find a suitable navigation waypoint */ if (global_pos->valid) { navigation_lat = global_pos->lat/1e7; navigation_lon = global_pos->lon/1e7; navigation_alt = global_pos->alt; navigation_frame = MAV_FRAME_GLOBAL; } else if (local_pos->xy_valid && local_pos->z_valid) { navigation_lat = local_pos->x; navigation_lon = local_pos->y; navigation_alt = local_pos->z; navigation_frame = MAV_FRAME_LOCAL_NED; } /* only accept supported navigation waypoints, skip unknown ones */ do { /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { /* this is a navigation waypoint */ navigation_frame = cur_wp->frame; navigation_lat = cur_wp->x; navigation_lon = cur_wp->y; navigation_alt = cur_wp->z; } if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { /* the last waypoint was reached, if auto continue is * activated keep the system loitering there. */ cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius cur_wp->frame = navigation_frame; cur_wp->x = navigation_lat; cur_wp->y = navigation_lon; cur_wp->z = navigation_alt; cur_wp->autocontinue = false; /* we risk an endless loop for missions without navigation waypoints, abort. */ break; } else { if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size) wpm->current_active_wp_id++; } } while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM)); // Fly to next waypoint wpm->timestamp_firstinside_orbit = 0; mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); mavlink_wpm_send_setpoint(wpm->current_active_wp_id); wpm->waypoints[wpm->current_active_wp_id].current = true; wpm->pos_reached = false; wpm->yaw_reached = false; printf("Set new waypoint (%u)\n", wpm->current_active_wp_id); } } } } else { wpm->timestamp_lastoutside_orbit = now; } counter++; }
void Mission::heading_sp_update() { if (_takeoff) { /* we don't want to be yawing during takeoff */ return; } struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(_on_arrival_yaw)) { return; } /* Don't change heading for takeoff waypoints, the ground may be near */ if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { return; } /* set yaw angle for the waypoint iff a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { _mission_item.yaw = _on_arrival_yaw; } else { /* calculate direction the vehicle should point to: * normal waypoint: current position to {waypoint or home or home + 180deg} * landing waypoint: last waypoint to {waypoint or home or home + 180deg} * For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground */ double point_from_latlon[2]; if (_mission_item.nav_cmd == NAV_CMD_LAND) { point_from_latlon[0] = pos_sp_triplet->previous.lat; point_from_latlon[1] = pos_sp_triplet->previous.lon; } else { point_from_latlon[0] = _navigator->get_global_position()->lat; point_from_latlon[1] = _navigator->get_global_position()->lon; } /* always keep the front of the rotary wing pointing to the next waypoint */ if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { _mission_item.yaw = get_bearing_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], _mission_item.lat, _mission_item.lon); /* always keep the back of the rotary wing pointing towards home */ } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { _mission_item.yaw = get_bearing_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], _navigator->get_home_position()->lat, _navigator->get_home_position()->lon); /* always keep the back of the rotary wing pointing towards home */ } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], _navigator->get_home_position()->lat, _navigator->get_home_position()->lon) + M_PI_F); } } mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); _navigator->set_position_setpoint_triplet_updated(); }
int MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) { /* only support global waypoints for now */ switch (mavlink_mission_item->frame) { case MAV_FRAME_GLOBAL: mission_item->lat = (double)mavlink_mission_item->x; mission_item->lon = (double)mavlink_mission_item->y; mission_item->altitude = mavlink_mission_item->z; mission_item->altitude_is_relative = false; break; case MAV_FRAME_GLOBAL_RELATIVE_ALT: mission_item->lat = (double)mavlink_mission_item->x; mission_item->lon = (double)mavlink_mission_item->y; mission_item->altitude = mavlink_mission_item->z; mission_item->altitude_is_relative = true; break; case MAV_FRAME_MISSION: // This is a mission item with no coordinate mission_item->params[0] = mavlink_mission_item->param1; mission_item->params[1] = mavlink_mission_item->param2; mission_item->params[2] = mavlink_mission_item->param3; mission_item->params[3] = mavlink_mission_item->param4; mission_item->params[4] = mavlink_mission_item->x; mission_item->params[5] = mavlink_mission_item->y; mission_item->params[6] = mavlink_mission_item->z; break; default: return MAV_MISSION_UNSUPPORTED_FRAME; } mission_item->frame = mavlink_mission_item->frame; switch (mavlink_mission_item->command) { case MAV_CMD_NAV_TAKEOFF: mission_item->pitch_min = mavlink_mission_item->param1; break; case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; mission_item->do_jump_current_count = 0; mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; mission_item->time_inside = mavlink_mission_item->param1; break; } mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ /* unsigned, so can't be negative, only check for overflow */ if (mavlink_mission_item->command >= NAV_CMD_INVALID) { mission_item->nav_cmd = NAV_CMD_INVALID; } else { mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; } mission_item->autocontinue = mavlink_mission_item->autocontinue; // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; /* reset DO_JUMP count */ mission_item->do_jump_current_count = 0; return MAV_MISSION_ACCEPTED; }
void check_waypoints_reached(absolute_time now, const struct vehicle_global_position_s *global_pos, float turn_distance) { mission_command_t current_command; bool_t time_elapsed = 0; float vertical_switch_distance, yaw_sp, yaw_err; float dist = -1.0f, dist_xy = -1.0f, dist_z = -1.0f; float current_lat = -1.0f, current_lon = -1.0f, current_alt = -1.0f; float orbit = mission_waypoint_manager_parameters.orbit_def_radius; float orbit_hold_time = mission_waypoint_manager_parameters.orbit_hold_time; /* position not valid */ if (!global_pos->valid || check_out_of_bounds(wp_manager.current_setpoint_index, 0, mission.n_commands)) { /* nothing to check here, return */ return; } current_command = mission.command_list[wp_manager.current_setpoint_index]; if (wp_manager.current_setpoint_index < mission.n_commands) { if (current_command.name == accepted_command_waypoint || current_command.name == accepted_command_loiter_time || current_command.name == accepted_command_loiter_circle || current_command.name == accepted_command_loiter_unlim) { if (current_command.option4 >= mission_waypoint_manager_parameters.orbit_min_radius) orbit = current_command.option4; } /* keep vertical orbit */ vertical_switch_distance = orbit; /* Take the larger turn distance - orbit or turn_distance */ if (orbit < turn_distance) orbit = turn_distance; current_alt = current_command.option1; current_lat = (current_command.name == accepted_command_rtl || current_command.name == accepted_command_land)? home_position.latitude : current_command.option2; current_lon = (current_command.name == accepted_command_rtl || current_command.name == accepted_command_land)? home_position.longitude : current_command.option3; dist = wpm_distance_to_point_global_wgs84(current_alt, current_lat, current_lon, global_pos->altitude, (float)global_pos->latitude * 1e-7f, (float)global_pos->longitude * 1e-7f, &dist_xy, &dist_z); if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { wp_manager.pos_reached = 1; } // check if required yaw reached yaw_sp = get_yaw_to_next_waypoint (wp_manager.current_setpoint_index, wp_manager.next_setpoint_index); yaw_err = _wrap_pi(yaw_sp - global_pos->yaw); if (fabsf(yaw_err) < 0.05f) { wp_manager.yaw_reached = 1; } } //check if the current waypoint was reached if (wp_manager.pos_reached /* && wp_manager.yaw_reached */) { if (wp_manager.timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached //wpm_send_waypoint_reached(wp_manager.current_setpoint_index); wp_manager.timestamp_firstinside_orbit = now; } // check if the MAV was long enough inside the waypoint orbit if (current_command.name != accepted_command_loiter_unlim && // XXX check this (now - wp_manager.timestamp_firstinside_orbit >= orbit_hold_time * 1000000 || current_command.name == accepted_command_takeoff)) { time_elapsed = 1; } if (time_elapsed) { current_command.current = 0; wp_manager.last_setpoint_index = wp_manager.current_setpoint_index; wp_manager.last_setpoint_valid = wp_manager.current_setpoint_valid; wp_manager.current_setpoint_index = wp_manager.next_setpoint_index; wp_manager.current_setpoint_valid = wp_manager.next_setpoint_valid; if (wp_manager.current_setpoint_valid) find_next_setpoint_index (wp_manager.current_setpoint_index, &wp_manager.next_setpoint_index, &wp_manager.next_setpoint_valid); /* if an error occur abort and return to home (then loiter unlim) */ if (!wp_manager.current_setpoint_valid) { wp_manager.current_setpoint_index = mission.n_commands; wp_manager.current_setpoint_valid = 1; } // Fly to next waypoint wp_manager.timestamp_firstinside_orbit = 0; wp_manager.timestamp_lastoutside_orbit = 0; wp_manager.pos_reached = 0; wp_manager.yaw_reached = 0; mission.command_list[wp_manager.current_setpoint_index].current = 1; //wpm_send_waypoint_current(wp_manager.current_setpoint_index); missionlib_current_waypoint_changed(); } } else { wp_manager.timestamp_lastoutside_orbit = now; } }
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); }
void BlockMultiModeBacksideAutopilot::update() { // wait for a sensor update, check for exit condition every 100 ms if (poll(&_attPoll, 1, 100) < 0) return; // poll error uint64_t newTimeStamp = hrt_absolute_time(); float dt = (newTimeStamp - _timeStamp) / 1.0e6f; _timeStamp = newTimeStamp; // check for sane values of dt // to prevent large control responses if (dt > 1.0f || dt < 0) return; // set dt for all child blocks setDt(dt); // store old position command before update if new command sent if (_posCmd.updated()) { _lastPosCmd = _posCmd.getData(); } // check for new updates if (_param_update.updated()) updateParams(); // get new information from subscriptions updateSubscriptions(); // default all output to zero unless handled by mode for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; // only update guidance in auto mode if (_status.state_machine == SYSTEM_STATE_AUTO) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } // XXX handle STABILIZED (loiter on spot) as well // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position // handle autopilot modes if (_status.state_machine == SYSTEM_STATE_AUTO || _status.state_machine == SYSTEM_STATE_STABILIZED) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); // calculate velocity, XXX should be airspeed, but using ground speed for now float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz); // altitude hold float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt); // heading hold float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw); float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); float pCmd = _phi2P.update(phiCmd - _att.roll); // velocity hold // negative sign because nose over to increase speed float thetaCmd = _theLimit.update(-_v2Theta.update( _vLimit.update(_vCmd.get()) - v)); float qCmd = _theta2Q.update(thetaCmd - _att.pitch); // yaw rate cmd float rCmd = 0; // stabilization _stabilization.update(pCmd, qCmd, rCmd, _att.rollspeed, _att.pitchspeed, _att.yawspeed); // output _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); _actuators.control[CH_THR] = dThrottle + _trimThr.get(); // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once // a first binary release can be targeted. // This is not a hack, but a design choice. /* do not limit in HIL */ if (!_status.flag_hil_enabled) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { // calculate velocity, XXX should be airspeed, but using ground speed for now float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding // from +1 -> -1 m/s for rate of climb //float dThrottle = _roc2Thr.update( //_rocMax.get()*_manual.pitch - _pos.vz); // roll channel -> bank angle float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); float pCmd = _phi2P.update(phiCmd - _att.roll); // throttle channel -> velocity // negative sign because nose over to increase speed float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin(); float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v)); float qCmd = _theta2Q.update(thetaCmd - _att.pitch); // yaw rate cmd float rCmd = 0; // stabilization _stabilization.update(pCmd, qCmd, rCmd, _att.rollspeed, _att.pitchspeed, _att.yawspeed); // output _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); // currenlty using manual throttle // XXX if you enable this watch out, vz might be very noisy //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); _actuators.control[CH_THR] = _manual.throttle; // XXX limit throttle to manual setting (safety) for now. // If it turns out to be confusing, it can be removed later once // a first binary release can be targeted. // This is not a hack, but a design choice. /* do not limit in HIL */ if (!_status.flag_hil_enabled) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } } // body rates controller, disabled for now else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, _att.rollspeed, _att.pitchspeed, _att.yawspeed); _actuators.control[CH_AIL] = _stabilization.getAileron(); _actuators.control[CH_ELV] = _stabilization.getElevator(); _actuators.control[CH_RDR] = _stabilization.getRudder(); _actuators.control[CH_THR] = _manual.throttle; } } // update all publications updatePublications(); }
float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p) { float psiError = _wrap_pi(psiCmd - psi); float phiCmd = _phiLimit.update(_psi2Phi.update(psiError)); return _phi2P.update(phiCmd - phi); }
/* * Attitude controller. * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) */ void MulticopterAttitudeControl::control_attitude(float dt) { float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ vehicle_attitude_setpoint_poll(); } 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.z; publish_att_sp = true; } if (!_armed.armed) { /* reset yaw setpoint when disarmed */ _reset_yaw_sp = true; } /* move yaw setpoint in all modes */ if (_v_att_sp.thrust < 0.1f) { // TODO //if (_status.condition_landed) { /* reset yaw setpoint if on ground */ // reset_yaw_sp = true; //} } else { /* move yaw setpoint */ 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); float yaw_offs_max = _params.man_yaw_max / _params.att_p(2); float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw); if (yaw_offs < - yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max); } else if (yaw_offs > yaw_offs_max) { _v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max); } _v_att_sp.R_valid = false; publish_att_sp = true; } /* reset yaw setpint to current position if needed */ if (_reset_yaw_sp) { _reset_yaw_sp = false; _v_att_sp.yaw_body = _v_att.yaw; _v_att_sp.R_valid = false; publish_att_sp = true; } 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.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; } } else { /* in non-manual mode use 'vehicle_attitude_setpoint' topic */ vehicle_attitude_setpoint_poll(); /* reset yaw setpoint after non-manual control mode */ _reset_yaw_sp = true; } _thrust_sp = _v_att_sp.thrust; /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ R_sp.set(&_v_att_sp.R_body[0][0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } /* publish the attitude setpoint if needed */ if (publish_att_sp) { _v_att_sp.timestamp = hrt_absolute_time(); if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp); } else { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp); } } /* rotation matrix for current state */ math::Matrix<3, 3> R; R.set(_v_att.R); /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); float e_R_z_cos = R_z * R_sp_z; /* calculate weight for yaw control */ float yaw_w = R_sp(2, 2) * R_sp(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; if (e_R_z_sin > 0.0f) { /* get axis-angle representation */ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos); math::Vector<3> e_R_z_axis = e_R / e_R_z_sin; e_R = e_R_z_axis * e_R_z_angle; /* cross product matrix for e_R_axis */ math::Matrix<3, 3> e_R_cp; e_R_cp.zero(); e_R_cp(0, 1) = -e_R_z_axis(2); e_R_cp(0, 2) = e_R_z_axis(1); e_R_cp(1, 0) = e_R_z_axis(2); e_R_cp(1, 2) = -e_R_z_axis(0); e_R_cp(2, 0) = -e_R_z_axis(1); e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ R_rp = R; } /* R_rp and R_sp has the same Z axis, calculate yaw error */ math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; if (e_R_z_cos < 0.0f) { /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); /* use fusion of Z axis based rotation and direct rotation */ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w; } /* calculate angular rates setpoint */ _rates_sp = _params.att_p.emult(e_R); /* limit yaw rate */ _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max); /* feed forward yaw setpoint rate */ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff; }
void FollowTarget::on_active() { struct map_projection_reference_s target_ref; math::Vector<3> target_reported_velocity(0, 0, 0); follow_target_s target_motion_with_offset = {}; uint64_t current_time = hrt_absolute_time(); bool _radius_entered = false; bool _radius_exited = false; bool updated = false; float dt_ms = 0; orb_check(_follow_target_sub, &updated); if (updated) { follow_target_s target_motion; _target_updates++; // save last known motion topic _previous_target_motion = _current_target_motion; orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion); if (_current_target_motion.timestamp == 0) { _current_target_motion = target_motion; } _current_target_motion.timestamp = target_motion.timestamp; _current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)( 1 - _responsiveness); _current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)( 1 - _responsiveness); target_reported_velocity(0) = _current_target_motion.vx; target_reported_velocity(1) = _current_target_motion.vy; } else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) { reset_target_validity(); } // update distance to target if (target_position_valid()) { // get distance to target map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0), &_target_distance(1)); } // update target velocity if (target_velocity_valid() && updated) { dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000); // ignore a small dt if (dt_ms > 10.0F) { // get last gps known reference for target map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon); // calculate distance the target has moved map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &(_target_position_delta(0)), &(_target_position_delta(1))); // update the average velocity of the target based on the position _est_target_vel = _target_position_delta / (dt_ms / 1000.0f); // if the target is moving add an offset and rotation if (_est_target_vel.length() > .5F) { _target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset; } // are we within the target acceptance radius? // give a buffer to exit/enter the radius to give the velocity controller // a chance to catch up _radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f); _radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M); // to keep the velocity increase/decrease smooth // calculate how many velocity increments/decrements // it will take to reach the targets velocity // with the given amount of steps also add a feed forward input that adjusts the // velocity as the position gap increases since // just traveling at the exact velocity of the target will not // get any closer or farther from the target _step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K; _step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS); _step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS); // if we are less than 1 meter from the target don't worry about trying to yaw // lock the yaw until we are at a distance that makes sense if ((_target_distance).length() > 1.0F) { // yaw rate smoothing // this really needs to control the yaw rate directly in the attitude pid controller // but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode _yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _current_target_motion.lat, _current_target_motion.lon); _yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F); _yaw_rate = _wrap_pi(_yaw_rate); _yaw_rate = math::constrain(_yaw_rate, -1.0F * _yaw_auto_max, _yaw_auto_max); } else { _yaw_angle = _yaw_rate = NAN; } } // warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f", // (double) _step_vel(0), // (double) _step_vel(1), // (double) _current_vel(0), // (double) _current_vel(1), // (double) _est_target_vel(0), // (double) _est_target_vel(1), // (double) (_target_distance).length(), // (double) (_target_position_offset + _target_distance).length(), // _follow_target_state, // (double)_avg_cos_ratio, (double) _yaw_rate); } if (target_position_valid()) { // get the target position using the calculated offset map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon); map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1), &target_motion_with_offset.lat, &target_motion_with_offset.lon); } // clamp yaw rate smoothing if we are with in // 3 degrees of facing target if (PX4_ISFINITE(_yaw_rate)) { if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) < math::radians(3.0F)) { _yaw_rate = NAN; } } // update state machine switch (_follow_target_state) { case TRACK_POSITION: { if (_radius_entered == true) { _follow_target_state = TRACK_VELOCITY; } else if (target_velocity_valid()) { set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); // keep the current velocity updated with the target velocity for when it's needed _current_vel = _est_target_vel; update_position_sp(true, true, _yaw_rate); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } case TRACK_VELOCITY: { if (_radius_exited == true) { _follow_target_state = TRACK_POSITION; } else if (target_velocity_valid()) { if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) { _current_vel += _step_vel; _last_update_time = current_time; } set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle); update_position_sp(true, false, _yaw_rate); } else { _follow_target_state = SET_WAIT_FOR_TARGET_POSITION; } break; } case SET_WAIT_FOR_TARGET_POSITION: { // Climb to the minimum altitude // and wait until a position is received follow_target_s target = {}; // for now set the target at the minimum height above the uav target.lat = _navigator->get_global_position()->lat; target.lon = _navigator->get_global_position()->lon; target.alt = 0.0F; set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle); update_position_sp(false, false, _yaw_rate); _follow_target_state = WAIT_FOR_TARGET_POSITION; } /* FALLTHROUGH */ case WAIT_FOR_TARGET_POSITION: { if (is_mission_item_reached() && target_velocity_valid()) { _target_position_offset(0) = _follow_offset; _follow_target_state = TRACK_POSITION; } break; } } }
bool MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { case NAV_CMD_DO_SET_SERVO: return true; case NAV_CMD_LAND: return _navigator->get_vstatus()->condition_landed; /* TODO: count turns */ /*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ case NAV_CMD_IDLE: /* fall through */ case NAV_CMD_LOITER_UNLIMITED: return false; case NAV_CMD_DO_DIGICAM_CONTROL: case NAV_CMD_DO_SET_CAM_TRIGG_DIST: return true; case NAV_CMD_DO_VTOL_TRANSITION: /* * We wait half a second to give the transition command time to propagate. * As soon as the timeout is over or when we're in transition mode let the mission continue. */ if (hrt_absolute_time() - _action_start > 500000 || _navigator->get_vstatus()->in_transition_mode) { _action_start = 0; return true; } else { return false; } case vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED: // XXX not differentiating ground and airspeed yet if (_mission_item.params[1] > 0.0f) { _navigator->set_cruising_speed(_mission_item.params[1]); } else { _navigator->set_cruising_speed(); } return true; default: /* do nothing, this is a 3D waypoint */ break; } hrt_abstime now = hrt_absolute_time(); if ((_navigator->get_vstatus()->condition_landed == false) && !_waypoint_position_reached) { float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; float altitude_amsl = _mission_item.altitude_is_relative ? _mission_item.altitude + _navigator->get_home_position()->alt : _mission_item.altitude; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, _navigator->get_global_position()->alt, &dist_xy, &dist_z); if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { /* require only altitude for takeoff for multicopter, do not use waypoint acceptance radius */ if (_navigator->get_global_position()->alt > altitude_amsl - _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { /* for takeoff mission items use the parameter for the takeoff acceptance radius */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } } else if (!_navigator->get_vstatus()->is_rotary_wing && (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the * coordinates with a radius equal to the loiter_radius field. It is not flying * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) { _waypoint_position_reached = true; } } else { /* for normal mission items used their acceptance radius */ float mission_acceptance_radius = _navigator->get_acceptance_radius(_mission_item.acceptance_radius); /* if set to zero use the default instead */ if (mission_acceptance_radius < NAV_EPSILON_POSITION) { mission_acceptance_radius = _navigator->get_acceptance_radius(); } if (dist >= 0.0f && dist <= mission_acceptance_radius) { _waypoint_position_reached = true; } } if (_waypoint_position_reached) { // reached just now _time_wp_reached = now; } } /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ if (_navigator->get_vstatus()->is_rotary_wing && PX4_ISFINITE(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); /* accept yaw if reached or if timeout is set in which case we ignore not forced headings */ if (fabsf(yaw_err) < math::radians(_param_yaw_err.get()) || (_param_yaw_timeout.get() >= FLT_EPSILON && !_mission_item.force_heading)) { _waypoint_yaw_reached = true; } /* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */ if (!_waypoint_yaw_reached && _mission_item.force_heading && _param_yaw_timeout.get() >= FLT_EPSILON && now - _time_wp_reached >= (hrt_abstime)_param_yaw_timeout.get() * 1e6f) { _navigator->set_mission_failure("unable to reach heading within timeout"); } } else { _waypoint_yaw_reached = true; } } /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; // if (_mission_item.time_inside > 0.01f) { // mavlink_log_critical(_mavlink_log_pub, "waypoint reached, wait for %.1fs", // (double)_mission_item.time_inside); // } } /* check if the MAV was long enough inside the waypoint orbit */ if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { return true; } } return false; }
bool Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ if (!_mission_item_valid) { return false; } if (_mission_item.nav_cmd == NAV_CMD_LAND) { return _vstatus.condition_landed; } /* XXX TODO count turns */ if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && _mission_item.loiter_radius > 0.01f) { return false; } uint64_t now = hrt_absolute_time(); if (!_waypoint_position_reached) { float acceptance_radius; if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { acceptance_radius = _mission_item.acceptance_radius; } else { acceptance_radius = _parameters.acceptance_radius; } float dist = -1.0f; float dist_xy = -1.0f; float dist_z = -1.0f; /* calculate AMSL altitude for this waypoint */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) wp_alt_amsl += _home_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } } } if (_waypoint_position_reached && !_waypoint_yaw_reached) { if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } } else { _waypoint_yaw_reached = true; } } /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); } } /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { reset_reached(); return true; } } return false; }
__EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, float radius, float arc_start_bearing, float arc_sweep) { // This function returns the distance to the nearest point on the track arc. Distance is positive if current // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and // headed towards the end point. // Determine if the current position is inside or outside the sector between the line from the center // to the arc start and the line from the center to the arc end float bearing_sector_start; float bearing_sector_end; float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); bool in_sector; int return_value = ERROR; // Set error flag, cleared when valid result calculated. crosstrack_error->past_end = false; crosstrack_error->distance = 0.0f; crosstrack_error->bearing = 0.0f; // Return error if arguments are bad if (radius < 0.1f) { return return_value; } if (arc_sweep >= 0.0f) { bearing_sector_start = arc_start_bearing; bearing_sector_end = arc_start_bearing + arc_sweep; if (bearing_sector_end > 2.0f * M_PI_F) { bearing_sector_end -= M_TWOPI_F; } } else { bearing_sector_end = arc_start_bearing; bearing_sector_start = arc_start_bearing - arc_sweep; if (bearing_sector_start < 0.0f) { bearing_sector_start += M_TWOPI_F; } } in_sector = false; // Case where sector does not span zero if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) { in_sector = true; } // Case where sector does span zero if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) { in_sector = true; } // If in the sector then calculate distance and bearing to closest point if (in_sector) { crosstrack_error->past_end = false; float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); if (dist_to_center <= radius) { crosstrack_error->distance = radius - dist_to_center; crosstrack_error->bearing = bearing_now + M_PI_F; } else { crosstrack_error->distance = dist_to_center - radius; crosstrack_error->bearing = bearing_now; } // If out of the sector then calculate dist and bearing to start or end point } else { // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to // calculate the position of the start and end points. We should not be doing this often // as this function generally will not be called repeatedly when we are out of the sector. double start_disp_x = (double)radius * sin(arc_start_bearing); double start_disp_y = (double)radius * cos(arc_start_bearing); double end_disp_x = (double)radius * sin(_wrap_pi((double)(arc_start_bearing + arc_sweep))); double end_disp_y = (double)radius * cos(_wrap_pi((double)(arc_start_bearing + arc_sweep))); double lon_start = lon_now + start_disp_x / 111111.0; double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; double lon_end = lon_now + end_disp_x / 111111.0; double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0; double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); } } crosstrack_error->bearing = _wrap_pi((double)crosstrack_error->bearing); return_value = OK; return return_value; }
void Standard::update_mc_state() { VtolType::update_mc_state(); // enable MC motors here in case we transitioned directly to MC mode if (_flag_enable_mc_motors) { set_max_mc(2000); set_idle_mc(); _flag_enable_mc_motors = false; } // if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return if (_params_standard.forward_thrust_scale < FLT_EPSILON) { return; } matrix::Dcmf R(matrix::Quatf(_v_att->q)); matrix::Dcmf R_sp(&_v_att_sp->R_body[0]); matrix::Eulerf euler(R); matrix::Eulerf euler_sp(R_sp); _pusher_throttle = 0.0f; // direction of desired body z axis represented in earth frame matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); // rotate desired body z axis into new frame which is rotated in z by the current // heading of the vehicle. we refer to this as the heading frame. matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2)); body_z_sp = R_yaw * body_z_sp; body_z_sp.normalize(); // calculate the desired pitch seen in the heading frame // this value corresponds to the amount the vehicle would try to pitch forward float pitch_forward = asinf(body_z_sp(0)); // only allow pitching forward up to threshold, the rest of the desired // forward acceleration will be compensated by the pusher if (pitch_forward < -_params_standard.down_pitch_max) { // desired roll angle in heading frame stays the same float roll_new = -atan2f(body_z_sp(1), body_z_sp(2)); _pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) * _v_att_sp->thrust * _params_standard.forward_thrust_scale; // limit desired pitch float pitch_new = -_params_standard.down_pitch_max; // create corrected desired body z axis in heading frame matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f); matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); // rotate the vector into a new frame which is rotated in z by the desired heading // with respect to the earh frame. float yaw_error = _wrap_pi(euler_sp(2) - euler(2)); matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error); tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints float pitch = asinf(tilt_new(0)); float roll = -atan2f(tilt_new(1), tilt_new(2)); R_sp = matrix::Eulerf(roll, pitch, euler_sp(2)); matrix::Quatf q_sp(R_sp); memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body)); memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d)); } _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle; }
void BottleDrop::task_main() { _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(_mavlink_fd, "[bottle_drop] started"); _command_sub = orb_subscribe(ORB_ID(vehicle_command)); _wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate)); bool updated = false; float z_0; // ground properties float turn_radius; // turn radius of the UAV float precision; // Expected precision of the UAV float ground_distance = _alt_clearance; // Replace by closer estimate in loop // constant float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2] float m = 0.5f; // mass of bottle [kg] float rho = 1.2f; // air density [kg/m^3] float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2] float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s] // Has to be estimated by experiment float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 [] float t_signal = 0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s] float t_door = 0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s] // Definition float h_0; // height over target float az; // acceleration in z direction[m/s^2] float vz; // velocity in z direction [m/s] float z; // fallen distance [m] float h; // height over target [m] float ax; // acceleration in x direction [m/s^2] float vx; // ground speed in x direction [m/s] float x; // traveled distance in x direction [m] float vw; // wind speed [m/s] float vrx; // relative velocity in x direction [m/s] float v; // relative speed vector [m/s] float Fd; // Drag force [N] float Fdx; // Drag force in x direction [N] float Fdz; // Drag force in z direction [N] float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED) float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED) float x_l, y_l; // local position in projected coordinates float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED float distance_open_door; // The distance the UAV travels during its doors open [m] float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector float distance_real = 0; // The distance between the UAVs position and the drop point [m] float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m] unsigned counter = 0; param_t param_gproperties = param_find("BD_GPROPERTIES"); param_t param_turn_radius = param_find("BD_TURNRADIUS"); param_t param_precision = param_find("BD_PRECISION"); param_t param_cd = param_find("BD_OBJ_CD"); param_t param_mass = param_find("BD_OBJ_MASS"); param_t param_surface = param_find("BD_OBJ_SURFACE"); param_get(param_precision, &precision); param_get(param_turn_radius, &turn_radius); param_get(param_gproperties, &z_0); param_get(param_cd, &cd); param_get(param_mass, &m); param_get(param_surface, &A); int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct parameter_update_s update; memset(&update, 0, sizeof(update)); int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); struct mission_item_s flight_vector_s {}; struct mission_item_s flight_vector_e {}; flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_s.acceptance_radius = 50; // TODO: make parameter flight_vector_s.autocontinue = true; flight_vector_s.altitude_is_relative = false; flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT; flight_vector_e.acceptance_radius = 50; // TODO: make parameter flight_vector_e.autocontinue = true; flight_vector_s.altitude_is_relative = false; struct wind_estimate_s wind; // wakeup source(s) struct pollfd fds[1]; // Setup of loop fds[0].fd = _command_sub; fds[0].events = POLLIN; // Whatever state the bay is in, we want it closed on startup lock_release(); close_bay(); while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50); /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { warn("poll error %d, %d", pret, errno); continue; } /* vehicle commands updated */ if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } orb_check(vehicle_global_position_sub, &updated); if (updated) { /* copy global position */ orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } if (_global_pos.timestamp == 0) { continue; } const unsigned sleeptime_us = 9500; hrt_abstime last_run = hrt_absolute_time(); float dt_runs = sleeptime_us / 1e6f; // switch to faster updates during the drop while (_drop_state > DROP_STATE_INIT) { // Get wind estimate orb_check(_wind_estimate_sub, &updated); if (updated) { orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind); } // Get vehicle position orb_check(vehicle_global_position_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos); } // Get parameter updates orb_check(parameter_update_sub, &updated); if (updated) { // copy global position orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); // update all parameters param_get(param_gproperties, &z_0); param_get(param_turn_radius, &turn_radius); param_get(param_precision, &precision); } orb_check(_command_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_command), _command_sub, &_command); handle_command(&_command); } float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east); float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e); ground_distance = _global_pos.alt - _target_position.alt; // Distance to drop position and angle error to approach vector // are relevant in all states greater than target valid (which calculates these positions) if (_drop_state > DROP_STATE_TARGET_VALID) { distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon)); float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n); float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); approach_error = _wrap_pi(ground_direction - approach_direction); if (counter % 90 == 0) { mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error)); } } switch (_drop_state) { case DROP_STATE_INIT: // do nothing break; case DROP_STATE_TARGET_VALID: { az = g; // acceleration in z direction[m/s^2] vz = 0; // velocity in z direction [m/s] z = 0; // fallen distance [m] h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m] h = h_0; // height over target [m] ax = 0; // acceleration in x direction [m/s^2] vx = groundspeed_body;// XXX project // ground speed in x direction [m/s] x = 0; // traveled distance in x direction [m] vw = 0; // wind speed [m/s] vrx = 0; // relative velocity in x direction [m/s] v = groundspeed_body; // relative speed vector [m/s] Fd = 0; // Drag force [N] Fdx = 0; // Drag force in x direction [N] Fdz = 0; // Drag force in z direction [N] // Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x while (h > 0.05f) { // z-direction vz = vz + az * dt_freefall_prediction; z = z + vz * dt_freefall_prediction; h = h_0 - z; // x-direction vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0); vx = vx + ax * dt_freefall_prediction; x = x + vx * dt_freefall_prediction; vrx = vx + vw; // drag force v = sqrtf(vz * vz + vrx * vrx); Fd = 0.5f * rho * A * cd * (v * v); Fdx = Fd * vrx / v; Fdz = Fd * vz / v; // acceleration az = g - Fdz / m; ax = -Fdx / m; } // compute drop vector x = groundspeed_body * t_signal + x; x_t = 0.0f; y_t = 0.0f; float wind_direction_n, wind_direction_e; if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen wind_direction_n = 1.0f; wind_direction_e = 0.0f; } else { wind_direction_n = wind.windspeed_north / windspeed_norm; wind_direction_e = wind.windspeed_east / windspeed_norm; } x_drop = x_t + x * wind_direction_n; y_drop = y_t + x * wind_direction_e; map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon); _drop_position.alt = _target_position.alt + _alt_clearance; // Compute flight vector map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e, &(flight_vector_s.lat), &(flight_vector_s.lon)); flight_vector_s.altitude = _drop_position.alt; map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e, &flight_vector_e.lat, &flight_vector_e.lon); flight_vector_e.altitude = _drop_position.alt; // Save WPs in datamanager const ssize_t len = sizeof(struct mission_item_s); if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) { warnx("ERROR: could not save onboard WP"); } if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) { warnx("ERROR: could not save onboard WP"); } _onboard_mission.count = 2; _onboard_mission.current_seq = 0; if (_onboard_mission_pub > 0) { orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission); } float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon); mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F)); _drop_state = DROP_STATE_TARGET_SET; } break; case DROP_STATE_TARGET_SET: { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } else { // We're close enough - open the bay distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body)); if (isfinite(distance_real) && distance_real < distance_open_door && fabsf(approach_error) < math::radians(20.0f)) { open_bay(); _drop_state = DROP_STATE_BAY_OPEN; mavlink_log_info(_mavlink_fd, "#audio: opening bay"); } } } break; case DROP_STATE_BAY_OPEN: { if (_drop_approval) { map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l); x_f = x_l + _global_pos.vel_n * dt_runs; y_f = y_l + _global_pos.vel_e * dt_runs; map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED); future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon); if (isfinite(distance_real) && (distance_real < precision) && ((distance_real < future_distance))) { drop(); _drop_state = DROP_STATE_DROPPED; mavlink_log_info(_mavlink_fd, "#audio: payload dropped"); } else { float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon); if (distance_wp2 < distance_real) { _onboard_mission.current_seq = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } } } } break; case DROP_STATE_DROPPED: /* 2s after drop, reset and close everything again */ if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) { _drop_state = DROP_STATE_INIT; _drop_approval = false; lock_release(); close_bay(); mavlink_log_info(_mavlink_fd, "#audio: closing bay"); // remove onboard mission _onboard_mission.current_seq = -1; _onboard_mission.count = 0; orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; case DROP_STATE_BAY_CLOSED: // do nothing break; } counter++; // update_actuators(); // run at roughly 100 Hz usleep(sleeptime_us); dt_runs = hrt_elapsed_time(&last_run) / 1e6f; last_run = hrt_absolute_time(); } } warnx("exiting."); _main_task = -1; _exit(0); }
void Mission::heading_sp_update() { /* we don't want to be yawing during takeoff, landing or aligning for a transition */ if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND || _work_item_type == WORK_ITEM_TYPE_ALIGN) { return; } struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); /* Don't change setpoint if last and current waypoint are not valid */ if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid) { return; } /* set yaw angle for the waypoint if a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { // XXX: should actually be param4 from mission item // at the moment it will just keep the heading it has //_mission_item.yaw = _on_arrival_yaw; //pos_sp_triplet->current.yaw = _mission_item.yaw; } else { /* Calculate direction the vehicle should point to. */ double point_from_latlon[2]; double point_to_latlon[2]; point_from_latlon[0] = _navigator->get_global_position()->lat; point_from_latlon[1] = _navigator->get_global_position()->lon; /* target location is home */ if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) // need to be rotary wing for this but not in a transition // in VTOL mode this will prevent updating yaw during FW flight // (which would result in a wrong yaw setpoint spike during back transition) && _navigator->get_vstatus()->is_rotary_wing && !(_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION || _navigator->get_vstatus()->in_transition_mode)) { point_to_latlon[0] = _navigator->get_home_position()->lat; point_to_latlon[1] = _navigator->get_home_position()->lon; /* target location is next (current) waypoint */ } else { point_to_latlon[0] = pos_sp_triplet->current.lat; point_to_latlon[1] = pos_sp_triplet->current.lon; } float d_current = get_distance_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); /* stop if positions are close together to prevent excessive yawing */ if (d_current > _navigator->get_acceptance_radius()) { float yaw = get_bearing_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); /* always keep the back of the rotary wing pointing towards home */ if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { _mission_item.yaw = _wrap_pi(yaw + M_PI_F); pos_sp_triplet->current.yaw = _mission_item.yaw; } else { _mission_item.yaw = yaw; pos_sp_triplet->current.yaw = _mission_item.yaw; } } } // we set yaw directly so we can run this in parallel to the FOH update _navigator->set_position_setpoint_triplet_updated(); }