/** Navigation function to a single waypoint */ static inline bool_t mission_nav_wp(struct _mission_wp * wp) { if (nav_approaching_xy(wp->wp.x, wp->wp.y, last_wp.x, last_wp.y, CARROT)) { last_wp = wp->wp; // store last wp return FALSE; // end of mission element } // set navigation command fly_to_xy(wp->wp.x, wp->wp.y); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(wp->wp.z, 0.); return TRUE; }
/** Navigation function to a single waypoint */ static inline bool mission_nav_wp(struct _mission_wp *wp) { if (nav_approaching_xy(wp->wp.wp_f.x, wp->wp.wp_f.y, last_wp_f.x, last_wp_f.y, CARROT)) { last_wp_f = wp->wp.wp_f; // store last wp return false; // end of mission element } // set navigation command fly_to_xy(wp->wp.wp_f.x, wp->wp.wp_f.y); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(wp->wp.wp_f.z, 0.); return true; }
static inline void nav_follow(uint8_t _ac_id, float _distance, float _height) { struct ac_info_ * ac = get_ac_info(_ac_id); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(Max(ac->alt + _height, ground_alt+SECURITY_HEIGHT), 0.); float alpha = M_PI/2 - ac->course; float ca = cosf(alpha), sa = sinf(alpha); float x = ac->east - _distance*ca; float y = ac->north - _distance*sa; fly_to_xy(x, y); #ifdef NAV_FOLLOW_PGAIN float s = (stateGetPositionEnu_f()->x - x)*ca + (stateGetPositionEnu_f()->y - y)*sa; nav_ground_speed_setpoint = ac->gspeed + NAV_FOLLOW_PGAIN*s; nav_ground_speed_loop(); #endif }
void nav_follow(uint8_t ac_id, float distance, float height) { struct EnuCoor_f *ac = acInfoGetPositionEnu_f(ac_id); NavVerticalAutoThrottleMode(0.); NavVerticalAltitudeMode(Max(ac->z + height, ground_alt + SECURITY_HEIGHT), 0.); float alpha = M_PI / 2 - acInfoGetCourse(ac_id); float ca = cosf(alpha), sa = sinf(alpha); float x = ac->x - distance * ca; float y = ac->y - distance * sa; fly_to_xy(x, y); #ifdef NAV_FOLLOW_PGAIN float s = (stateGetPositionEnu_f()->x - x) * ca + (stateGetPositionEnu_f()->y - y) * sa; nav_ground_speed_setpoint = acInfoGetGspeed(ac_id) + NAV_FOLLOW_PGAIN * s; nav_ground_speed_loop(); #endif }
/** Navigates around (x, y). Clockwise iff radius > 0 */ void nav_circle_XY(float x, float y, float radius) { struct EnuCoor_f *pos = stateGetPositionEnu_f(); float last_trigo_qdr = nav_circle_trigo_qdr; nav_circle_trigo_qdr = atan2f(pos->y - y, pos->x - x); float sign_radius = radius > 0 ? 1 : -1; if (nav_in_circle) { float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr; NormRadAngle(trigo_diff); nav_circle_radians += trigo_diff; trigo_diff *= - sign_radius; if (trigo_diff > 0) { // do not rewind if the change in angle is in the opposite sense than nav_radius nav_circle_radians_no_rewind += trigo_diff; } } float dist2_center = DistanceSquare(pos->x, pos->y, x, y); float dist_carrot = CARROT * NOMINAL_AIRSPEED; radius += -nav_shift; float abs_radius = fabs(radius); /** Computes a prebank. Go straight if inside or outside the circle */ circle_bank = (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : atanf(stateGetHorizontalSpeedNorm_f() * stateGetHorizontalSpeedNorm_f() / (NAV_GRAVITY * radius)); float carrot_angle = dist_carrot / abs_radius; carrot_angle = Min(carrot_angle, M_PI / 4); carrot_angle = Max(carrot_angle, M_PI / 16); float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle; horizontal_mode = HORIZONTAL_MODE_CIRCLE; float radius_carrot = abs_radius; if (nav_mode == NAV_MODE_COURSE) { radius_carrot += (abs_radius / cosf(carrot_angle) - abs_radius); } fly_to_xy(x + cosf(alpha_carrot)*radius_carrot, y + sinf(alpha_carrot)*radius_carrot); nav_in_circle = true; nav_circle_x = x; nav_circle_y = y; nav_circle_radius = radius; }
/** * \brief Computes the carrot position along the desired segment. */ void nav_route_xy(float last_wp_x, float last_wp_y, float wp_x, float wp_y) { float leg_x = wp_x - last_wp_x; float leg_y = wp_y - last_wp_y; float leg2 = Max(leg_x * leg_x + leg_y * leg_y, 1.); nav_leg_progress = ((stateGetPositionEnu_f()->x - last_wp_x) * leg_x + (stateGetPositionEnu_f()->y - last_wp_y) * leg_y) / leg2; nav_leg_length = sqrtf(leg2); /** distance of carrot (in meter) */ float carrot = CARROT * NOMINAL_AIRSPEED; nav_leg_progress += Max(carrot / nav_leg_length, 0.); nav_in_segment = TRUE; nav_segment_x_1 = last_wp_x; nav_segment_y_1 = last_wp_y; nav_segment_x_2 = wp_x; nav_segment_y_2 = wp_y; horizontal_mode = HORIZONTAL_MODE_ROUTE; fly_to_xy(last_wp_x + nav_leg_progress*leg_x +nav_shift*leg_y/nav_leg_length, last_wp_y + nav_leg_progress*leg_y-nav_shift*leg_x/nav_leg_length); }
/** Navigates around (x, y). Clockwise iff radius > 0 */ void nav_circle_XY(float x, float y, float radius) { float last_trigo_qdr = nav_circle_trigo_qdr; nav_circle_trigo_qdr = atan2(estimator_y - y, estimator_x - x); if (nav_in_circle) { float trigo_diff = nav_circle_trigo_qdr - last_trigo_qdr; NormRadAngle(trigo_diff); nav_circle_radians += trigo_diff; } float dist2_center = DistanceSquare(estimator_x, estimator_y, x, y); float dist_carrot = CARROT*NOMINAL_AIRSPEED; float sign_radius = radius > 0 ? 1 : -1; radius += -nav_shift; float abs_radius = fabs(radius); /** Computes a prebank. Go straight if inside or outside the circle */ circle_bank = (dist2_center > Square(abs_radius + dist_carrot) || dist2_center < Square(abs_radius - dist_carrot)) ? 0 : atan(estimator_hspeed_mod*estimator_hspeed_mod / (G*radius)); float carrot_angle = dist_carrot / abs_radius; carrot_angle = Min(carrot_angle, M_PI/4); carrot_angle = Max(carrot_angle, M_PI/16); float alpha_carrot = nav_circle_trigo_qdr - sign_radius * carrot_angle; horizontal_mode = HORIZONTAL_MODE_CIRCLE; float radius_carrot = abs_radius; if (nav_mode == NAV_MODE_COURSE) radius_carrot += (abs_radius / cos(carrot_angle) - abs_radius); fly_to_xy(x+cos(alpha_carrot)*radius_carrot, y+sin(alpha_carrot)*radius_carrot); nav_in_circle = TRUE; nav_circle_x = x; nav_circle_y = y; nav_circle_radius = radius; }
int formation_flight(void) { static uint8_t _1Hz = 0; uint8_t nb = 0, i; float hspeed_dir = stateGetHorizontalSpeedDir_f(); float ch = cosf(hspeed_dir); float sh = sinf(hspeed_dir); form_n = 0.; form_e = 0.; form_a = 0.; form_speed = stateGetHorizontalSpeedNorm_f(); form_speed_n = form_speed * ch; form_speed_e = form_speed * sh; if (AC_ID == leader_id) { stateGetPositionEnu_f()->x += formation[the_acs_id[AC_ID]].east; stateGetPositionEnu_f()->y += formation[the_acs_id[AC_ID]].north; } // set info for this AC set_ac_info(AC_ID, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y, hspeed_dir, stateGetPositionUtm_f()->alt, form_speed, stateGetSpeedEnu_f()->z, gps.tow); // broadcast info uint8_t ac_id = AC_ID; uint8_t status = formation[the_acs_id[AC_ID]].status; DOWNLINK_SEND_FORMATION_STATUS_TM(DefaultChannel, DefaultDevice, &ac_id, &leader_id, &status); if (++_1Hz >= 4) { _1Hz = 0; DOWNLINK_SEND_FORMATION_SLOT_TM(DefaultChannel, DefaultDevice, &ac_id, &form_mode, &formation[the_acs_id[AC_ID]].east, &formation[the_acs_id[AC_ID]].north, &formation[the_acs_id[AC_ID]].alt); } if (formation[the_acs_id[AC_ID]].status != ACTIVE) { return FALSE; } // AC not ready // get leader info struct ac_info_ * leader = get_ac_info(leader_id); if (formation[the_acs_id[leader_id]].status == UNSET || formation[the_acs_id[leader_id]].status == IDLE) { // leader not ready or not in formation return FALSE; } // compute slots in the right reference frame struct slot_ form[NB_ACS]; float cr = 0., sr = 1.; if (form_mode == FORM_MODE_COURSE) { cr = cosf(leader->course); sr = sinf(leader->course); } for (i = 0; i < NB_ACS; ++i) { if (formation[i].status == UNSET) { continue; } form[i].east = formation[i].east * sr - formation[i].north * cr; form[i].north = formation[i].east * cr + formation[i].north * sr; form[i].alt = formation[i].alt; } // compute control forces for (i = 0; i < NB_ACS; ++i) { if (the_acs[i].ac_id == AC_ID) { continue; } struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); if (delta_t > FORM_CARROT) { // if AC not responding for too long formation[i].status = LOST; continue; } else { // compute control if AC is ACTIVE and around the same altitude (maybe not so usefull) formation[i].status = ACTIVE; if (ac->alt > 0 && fabs(stateGetPositionUtm_f()->alt - ac->alt) < form_prox) { form_e += (ac->east + ac->gspeed * sinf(ac->course) * delta_t - stateGetPositionEnu_f()->x) - (form[i].east - form[the_acs_id[AC_ID]].east); form_n += (ac->north + ac->gspeed * cosf(ac->course) * delta_t - stateGetPositionEnu_f()->y) - (form[i].north - form[the_acs_id[AC_ID]].north); form_a += (ac->alt - stateGetPositionUtm_f()->alt) - (formation[i].alt - formation[the_acs_id[AC_ID]].alt); form_speed += ac->gspeed; //form_speed_e += ac->gspeed * sinf(ac->course); //form_speed_n += ac->gspeed * cosf(ac->course); ++nb; } } } uint8_t _nb = Max(1, nb); form_n /= _nb; form_e /= _nb; form_a /= _nb; form_speed = form_speed / (nb + 1) - stateGetHorizontalSpeedNorm_f(); //form_speed_e = form_speed_e / (nb+1) - stateGetHorizontalSpeedNorm_f() * sh; //form_speed_n = form_speed_n / (nb+1) - stateGetHorizontalSpeedNorm_f() * ch; // set commands NavVerticalAutoThrottleMode(0.); // altitude loop float alt = 0.; if (AC_ID == leader_id) { alt = nav_altitude; } else { alt = leader->alt - form[the_acs_id[leader_id]].alt; } alt += formation[the_acs_id[AC_ID]].alt + coef_form_alt * form_a; flight_altitude = Max(alt, ground_alt + SECURITY_HEIGHT); // carrot if (AC_ID != leader_id) { float dx = form[the_acs_id[AC_ID]].east - form[the_acs_id[leader_id]].east; float dy = form[the_acs_id[AC_ID]].north - form[the_acs_id[leader_id]].north; desired_x = leader->east + NOMINAL_AIRSPEED * form_carrot * sinf(leader->course) + dx; desired_y = leader->north + NOMINAL_AIRSPEED * form_carrot * cosf(leader->course) + dy; // fly to desired fly_to_xy(desired_x, desired_y); desired_x = leader->east + dx; desired_y = leader->north + dy; // lateral correction //float diff_heading = asin((dx*ch - dy*sh) / sqrt(dx*dx + dy*dy)); //float diff_course = leader->course - hspeed_dir; //NormRadAngle(diff_course); //h_ctl_roll_setpoint += coef_form_course * diff_course; //h_ctl_roll_setpoint += coef_form_course * diff_heading; } //BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // speed loop if (nb > 0) { float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; cruise += coef_form_pos * (form_n * ch + form_e * sh) + coef_form_speed * form_speed; Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); v_ctl_auto_throttle_cruise_throttle = cruise; } return TRUE; }
int potential_task(void) { uint8_t i; float ch = cosf(stateGetHorizontalSpeedDir_f()); float sh = sinf(stateGetHorizontalSpeedDir_f()); potential_force.east = 0.; potential_force.north = 0.; potential_force.alt = 0.; // compute control forces int8_t nb = 0; for (i = 0; i < NB_ACS; ++i) { if (the_acs[i].ac_id == AC_ID) { continue; } struct ac_info_ * ac = get_ac_info(the_acs[i].ac_id); float delta_t = Max((int)(gps.tow - ac->itow) / 1000., 0.); // if AC not responding for too long, continue, else compute force if (delta_t > CARROT) { continue; } else { float sha = sinf(ac->course); float cha = cosf(ac->course); float de = ac->east + sha * delta_t - stateGetPositionEnu_f()->x; if (de > FORCE_MAX_DIST || de < -FORCE_MAX_DIST) { continue; } float dn = ac->north + cha * delta_t - stateGetPositionEnu_f()->y; if (dn > FORCE_MAX_DIST || dn < -FORCE_MAX_DIST) { continue; } float da = ac->alt + ac->climb * delta_t - stateGetPositionUtm_f()->alt; if (da > FORCE_MAX_DIST || da < -FORCE_MAX_DIST) { continue; } float dist = sqrtf(de * de + dn * dn + da * da); if (dist == 0.) { continue; } float dve = stateGetHorizontalSpeedNorm_f() * sh - ac->gspeed * sha; float dvn = stateGetHorizontalSpeedNorm_f() * ch - ac->gspeed * cha; float dva = stateGetSpeedEnu_f()->z - the_acs[i].climb; float scal = dve * de + dvn * dn + dva * da; if (scal < 0.) { continue; } // No risk of collision float d3 = dist * dist * dist; potential_force.east += scal * de / d3; potential_force.north += scal * dn / d3; potential_force.alt += scal * da / d3; ++nb; } } if (nb == 0) { return true; } //potential_force.east /= nb; //potential_force.north /= nb; //potential_force.alt /= nb; // set commands NavVerticalAutoThrottleMode(0.); // carrot float dx = -force_pos_gain * potential_force.east; float dy = -force_pos_gain * potential_force.north; desired_x += dx; desired_y += dy; // fly to desired fly_to_xy(desired_x, desired_y); // speed loop float cruise = V_CTL_AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE; cruise += -force_speed_gain * (potential_force.north * ch + potential_force.east * sh); Bound(cruise, V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE); potential_force.speed = cruise; v_ctl_auto_throttle_cruise_throttle = cruise; // climb loop potential_force.climb = -force_climb_gain * potential_force.alt; BoundAbs(potential_force.climb, V_CTL_ALTITUDE_MAX_CLIMB); NavVerticalClimbMode(potential_force.climb); DOWNLINK_SEND_POTENTIAL(DefaultChannel, DefaultDevice, &potential_force.east, &potential_force.north, &potential_force.alt, &potential_force.speed, &potential_force.climb); return true; }