static void send_airspeed(void) { #ifdef MEASURE_AIRSPEED float* airspeed = stateGetAirspeed_f(); DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, airspeed, airspeed, airspeed, airspeed); #elif USE_AIRSPEED DOWNLINK_SEND_AIRSPEED(DefaultChannel, DefaultDevice, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); #endif }
inline static void loiter(void) { float pitch_trim; static float last_pitch_trim; #if USE_AIRSPEED if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) { pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); } else { pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); } #else float throttle_diff = v_ctl_auto_throttle_cruise_throttle - v_ctl_auto_throttle_nominal_cruise_throttle; if (throttle_diff > 0) { float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1); pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim; } else { float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1); pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim; } #endif // Pitch trim rate limiter float max_change = (v_ctl_pitch_loiter_trim - v_ctl_pitch_dash_trim) * H_CTL_REF_DT/ PITCH_TRIM_RATE_LIMITER; Bound(pitch_trim, last_pitch_trim - max_change, last_pitch_trim + max_change); last_pitch_trim = pitch_trim; h_ctl_pitch_loop_setpoint += pitch_trim; }
/** Compute a first approximation for the RELEASE waypoint from wind and expected airspeed and altitude */ unit_t nav_drop_compute_approach( uint8_t wp_target, uint8_t wp_start, float nav_drop_radius ) { waypoints[WP_RELEASE].a = waypoints[wp_start].a; nav_drop_z = waypoints[WP_RELEASE].a - waypoints[wp_target].a; nav_drop_x = 0.; nav_drop_y = 0.; /* We approximate vx and vy, taking into account that RELEASE is next to TARGET */ float x_0 = waypoints[wp_target].x - waypoints[wp_start].x; float y_0 = waypoints[wp_target].y - waypoints[wp_start].y; /* Unit vector from START to TARGET */ float d = sqrt(x_0*x_0+y_0*y_0); float x1 = x_0 / d; float y_1 = y_0 / d; waypoints[WP_BASELEG].x = waypoints[wp_start].x + y_1 * nav_drop_radius; waypoints[WP_BASELEG].y = waypoints[wp_start].y - x1 * nav_drop_radius; waypoints[WP_BASELEG].a = waypoints[wp_start].a; nav_drop_start_qdr = M_PI - atan2(-y_1, -x1); if (nav_drop_radius < 0) nav_drop_start_qdr += M_PI; // wind in NED frame if (stateIsAirspeedValid()) { nav_drop_vx = x1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->y; nav_drop_vy = y_1 * *stateGetAirspeed_f() + stateGetHorizontalWindspeed_f()->x; } else { // use approximate airspeed, initially set to AIRSPEED_AT_RELEASE nav_drop_vx = x1 * airspeed + stateGetHorizontalWindspeed_f()->y; nav_drop_vy = y_1 * airspeed + stateGetHorizontalWindspeed_f()->x; } nav_drop_vz = 0.; float vx0 = nav_drop_vx; float vy0 = nav_drop_vy; integrate(wp_target); waypoints[WP_CLIMB].x = waypoints[WP_RELEASE].x + (CLIMB_TIME + CARROT) * vx0; waypoints[WP_CLIMB].y = waypoints[WP_RELEASE].y + (CLIMB_TIME + CARROT) * vy0; waypoints[WP_CLIMB].a = waypoints[WP_RELEASE].a + SAFE_CLIMB; return 0; }
/*�Adjusting a circle around CA, tangent in A, to end at snav_desired_tow */ bool snav_on_time(float nominal_radius) { nominal_radius = fabs(nominal_radius); float current_qdr = M_PI_2 - atan2(stateGetPositionEnu_f()->y - wp_ca.y, stateGetPositionEnu_f()->x - wp_ca.x); float remaining_angle = Norm2Pi(Sign(a_radius) * (qdr_a - current_qdr)); float remaining_time = snav_desired_tow - gps.tow / 1000.; /* Use the nominal airspeed if the estimated one is not realistic */ float airspeed = stateGetAirspeed_f(); if (airspeed < NOMINAL_AIRSPEED / 2. || airspeed > 2.* NOMINAL_AIRSPEED) { airspeed = NOMINAL_AIRSPEED; } /* Recompute ground speeds every 10 s */ if (ground_speed_timer == 0) { ground_speed_timer = 40; /* every 10s, called at 40Hz */ compute_ground_speed(airspeed, stateGetHorizontalWindspeed_f()->y, stateGetHorizontalWindspeed_f()->x); // Wind in NED frame } ground_speed_timer--; /* Time to complete the circle at nominal_radius */ float nominal_time = 0.; float a; float ground_speed = NOMINAL_AIRSPEED; /* Init to avoid a warning */ /* Going one step too far */ for (a = 0; a < remaining_angle + ANGLE_STEP; a += ANGLE_STEP) { float qdr = current_qdr + Sign(a_radius) * a; ground_speed = ground_speed_of_course(qdr + Sign(a_radius) * M_PI_2); nominal_time += ANGLE_STEP * nominal_radius / ground_speed; } /* Removing what exceeds remaining_angle */ nominal_time -= (a - remaining_angle) * nominal_radius / ground_speed; /* Radius size to finish in one single circle */ float radius = remaining_time / nominal_time * nominal_radius; if (radius > 2. * nominal_radius) { radius = nominal_radius; } NavVerticalAutoThrottleMode(0); /* No pitch */ NavVerticalAltitudeMode(wp_cd.a, 0.); radius *= Sign(a_radius); wp_ca.x = WaypointX(wp_a) + radius * u_a_ca_x; wp_ca.y = WaypointY(wp_a) + radius * u_a_ca_y; nav_circle_XY(wp_ca.x, wp_ca.y, radius); /* Stay in this mode until the end of time */ return (remaining_time > 0); }
inline static void h_ctl_cl_loop(void) { #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR #if (!defined SITL || defined USE_NPS) struct Int32Vect3 accel_meas_body, accel_ned; struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i(); struct NedCoor_i *accel_tmp = stateGetAccelNed_i(); VECT3_COPY(accel_ned, (*accel_tmp)); accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f); int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned); float nz = -ACCEL_FLOAT_OF_BFP(accel_meas_body.z) / 9.81f; // max load factor to be taken into acount // to prevent negative flap movement du to negative acceleration Bound(nz, 1.f, 2.f); #else float nz = 1.f; #endif #endif // Compute a corrected airspeed corresponding to the current load factor nz // with Cz the lift coef at 1g, Czn the lift coef at n g, both at the same speed V, // the corrected airspeed Vn is so that nz = Czn/Cz = V^2 / Vn^2, // thus Vn = V / sqrt(nz) #if H_CTL_CL_LOOP_USE_AIRSPEED_SETPOINT float corrected_airspeed = v_ctl_auto_airspeed_setpoint; #else float corrected_airspeed = stateGetAirspeed_f(); #endif #if H_CTL_CL_LOOP_INCREASE_FLAPS_WITH_LOADFACTOR corrected_airspeed /= sqrtf(nz); #endif Bound(corrected_airspeed, STALL_AIRSPEED, RACE_AIRSPEED); float cmd = 0.f; // deadband around NOMINAL_AIRSPEED, rest linear if (corrected_airspeed > NOMINAL_AIRSPEED + H_CTL_CL_DEADBAND) { cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_RACE - H_CTL_CL_FLAPS_NOMINAL) / (RACE_AIRSPEED - NOMINAL_AIRSPEED); } else if (corrected_airspeed < NOMINAL_AIRSPEED - H_CTL_CL_DEADBAND) { cmd = (corrected_airspeed - NOMINAL_AIRSPEED) * (H_CTL_CL_FLAPS_STALL - H_CTL_CL_FLAPS_NOMINAL) / (STALL_AIRSPEED - NOMINAL_AIRSPEED); } // no control in manual mode if (pprz_mode == PPRZ_MODE_MANUAL) { cmd = 0.f; } // bound max flap angle Bound(cmd, H_CTL_CL_FLAPS_RACE, H_CTL_CL_FLAPS_STALL); // from percent to pprz cmd = cmd * MAX_PPRZ; h_ctl_flaps_setpoint = TRIM_PPRZ(cmd); }
inline static void h_ctl_yaw_loop(void) { #if H_CTL_YAW_TRIM_NY // Actual Acceleration from IMU: #if (!defined SITL || defined USE_NPS) struct Int32Vect3 accel_meas_body, accel_ned; struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i(); struct NedCoor_i *accel_tmp = stateGetAccelNed_i(); VECT3_COPY(accel_ned, (*accel_tmp)); accel_ned.z -= ACCEL_BFP_OF_REAL(9.81f); int32_rmat_vmult(&accel_meas_body, ned_to_body_rmat, &accel_ned); float ny = -ACCEL_FLOAT_OF_BFP(accel_meas_body.y) / 9.81f; // Lateral load factor (in g) #else float ny = 0.f; #endif if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { h_ctl_yaw_ny_sum_err = 0.; } else { if (h_ctl_yaw_ny_igain > 0.) { // only update when: phi<60degrees and ny<2g if (fabsf(stateGetNedToBodyEulers_f()->phi) < 1.05 && fabsf(ny) < 2.) { h_ctl_yaw_ny_sum_err += ny * H_CTL_REF_DT; // max half rudder deflection for trim BoundAbs(h_ctl_yaw_ny_sum_err, MAX_PPRZ / (2. * h_ctl_yaw_ny_igain)); } } else { h_ctl_yaw_ny_sum_err = 0.; } } #endif #ifdef USE_AIRSPEED float Vo = stateGetAirspeed_f(); Bound(Vo, STALL_AIRSPEED, RACE_AIRSPEED); #else float Vo = NOMINAL_AIRSPEED; #endif h_ctl_ref.yaw_rate = h_ctl_yaw_rate_setpoint // set by RC + 9.81f / Vo * sinf(h_ctl_roll_setpoint); // for turns float d_err = h_ctl_ref.yaw_rate - stateGetBodyRates_f()->r; float cmd = + h_ctl_yaw_dgain * d_err #if H_CTL_YAW_TRIM_NY + h_ctl_yaw_ny_igain * h_ctl_yaw_ny_sum_err #endif ; cmd /= airspeed_ratio2; h_ctl_rudder_setpoint = TRIM_PPRZ(cmd); }
static inline void compute_airspeed_ratio( void ) { if (use_airspeed_ratio) { // low pass airspeed static float airspeed = 0.; airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16; // compute ratio float airspeed_ratio = airspeed / NOMINAL_AIRSPEED; Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX); airspeed_ratio2 = airspeed_ratio*airspeed_ratio; } else { airspeed_ratio2 = 1.; } }
inline static void v_ctl_climb_auto_throttle_loop(void) { float f_throttle = 0; float controlled_throttle; float v_ctl_pitch_of_vz; // Limit rate of change of climb setpoint (to ensure that airspeed loop can catch-up) static float v_ctl_climb_setpoint_last = 0; float diff_climb = v_ctl_climb_setpoint - v_ctl_climb_setpoint_last; Bound(diff_climb, -V_CTL_AUTO_CLIMB_LIMIT, V_CTL_AUTO_CLIMB_LIMIT); v_ctl_climb_setpoint = v_ctl_climb_setpoint_last + diff_climb; v_ctl_climb_setpoint_last = v_ctl_climb_setpoint; // Pitch control (input: rate of climb error, output: pitch setpoint) float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint; v_ctl_auto_pitch_sum_err += err; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR); v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain * (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err); // Ground speed control loop (input: groundspeed error, output: airspeed controlled) float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f()); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain; // Do not allow controlled airspeed below the setpoint if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint) { v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint; v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain * v_ctl_auto_groundspeed_igain); // reset integrator of ground speed loop } // Airspeed control loop (input: airspeed controlled, output: throttle controlled) float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f()); v_ctl_auto_airspeed_sum_err += err_airspeed; BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR); controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain; // Done, set outputs Bound(controlled_throttle, 0, v_ctl_auto_throttle_max_cruise_throttle); f_throttle = controlled_throttle; v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim; v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ); Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH); }
void generic_com_periodic( void ) { if (com_trans.status != I2CTransDone) { return; } com_trans.buf[0] = active_com; FillBufWith32bit(com_trans.buf, 1, gps.lla_pos.lat); FillBufWith32bit(com_trans.buf, 5, gps.lla_pos.lon); FillBufWith16bit(com_trans.buf, 9, (int16_t)(gps.lla_pos.alt/1000)); // altitude (meters) FillBufWith16bit(com_trans.buf, 11, gps.gspeed); // ground speed (cm/s) FillBufWith16bit(com_trans.buf, 13, (int16_t)(gps.course/1e4)); // course (1e3rad) FillBufWith16bit(com_trans.buf, 15, (uint16_t)((*stateGetAirspeed_f())*100)); // TAS (cm/s) com_trans.buf[17] = electrical.vsupply; // decivolts com_trans.buf[18] = (uint8_t)(energy/100); // deciAh com_trans.buf[19] = (uint8_t)(ap_state->commands[COMMAND_THROTTLE]*100/MAX_PPRZ); com_trans.buf[20] = pprz_mode; com_trans.buf[21] = nav_block; FillBufWith16bit(com_trans.buf, 22, autopilot_flight_time); i2c_transmit(&GENERIC_COM_I2C_DEV, &com_trans, GENERIC_COM_SLAVE_ADDR, NB_DATA); }
/** * Send Metrics typically displayed on a HUD for fixed wing aircraft. */ static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev) { /* Current heading in degrees, in compass units (0..360, 0=north) */ int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi); /* Current throttle setting in integer percent, 0 to 100 */ // is a 16bit unsigned int but supposed to be from 0 to 100?? uint16_t throttle; #ifdef COMMAND_THRUST throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100); #elif defined COMMAND_THROTTLE throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100); #endif mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, stateGetAirspeed_f(), stateGetHorizontalSpeedNorm_f(), // groundspeed heading, throttle, stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL stateGetSpeedNed_f()->z); // climb rate MAVLinkSendMessage(); }
inline static void loiter(void) { float pitch_trim; #if USE_AIRSPEED if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) { pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1); } else { pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1); } #else float throttle_diff = v_ctl_throttle_setpoint / (float)MAX_PPRZ - v_ctl_auto_throttle_nominal_cruise_throttle; if (throttle_diff > 0) { float max_diff = Max(V_CTL_AUTO_THROTTLE_MAX_CRUISE_THROTTLE - v_ctl_auto_throttle_nominal_cruise_throttle, 0.1); pitch_trim = throttle_diff / max_diff * v_ctl_pitch_dash_trim; } else { float max_diff = Max(v_ctl_auto_throttle_nominal_cruise_throttle - V_CTL_AUTO_THROTTLE_MIN_CRUISE_THROTTLE, 0.1); pitch_trim = -throttle_diff / max_diff * v_ctl_pitch_loiter_trim; } #endif h_ctl_pitch_loop_setpoint += pitch_trim; }
void dl_parse_msg(void) { datalink_time = 0; uint8_t msg_id = IdOfMsg(dl_buffer); #if 0 // not ready yet uint8_t sender_id = SenderIdOfMsg(dl_buffer); /* parse telemetry messages coming from other AC */ if (sender_id != 0) { switch (msg_id) { #ifdef TCAS case DL_TCAS_RA: { if (DL_TCAS_RESOLVE_ac_id(dl_buffer) == AC_ID && SenderIdOfMsg(dl_buffer) != AC_ID) { uint8_t ac_id_conflict = SenderIdOfMsg(dl_buffer); tcas_acs_status[the_acs_id[ac_id_conflict]].resolve = DL_TCAS_RA_resolve(dl_buffer); } } #endif } return; } #endif if (msg_id == DL_PING) { DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice) } else #ifdef TRAFFIC_INFO if (msg_id == DL_ACINFO && DL_ACINFO_ac_id(dl_buffer) != AC_ID) { uint8_t id = DL_ACINFO_ac_id(dl_buffer); float ux = MOfCm(DL_ACINFO_utm_east(dl_buffer)); float uy = MOfCm(DL_ACINFO_utm_north(dl_buffer)); float a = MOfCm(DL_ACINFO_alt(dl_buffer)); float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.); float s = MOfCm(DL_ACINFO_speed(dl_buffer)); float cl = MOfCm(DL_ACINFO_climb(dl_buffer)); uint32_t t = DL_ACINFO_itow(dl_buffer); SetAcInfo(id, ux, uy, c, a, s, cl, t); } else #endif #ifdef NAV if (msg_id == DL_MOVE_WP && DL_MOVE_WP_ac_id(dl_buffer) == AC_ID) { uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer); float a = MOfCm(DL_MOVE_WP_alt(dl_buffer)); /* Computes from (lat, long) in the referenced UTM zone */ struct LlaCoor_f lla; lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(dl_buffer) / 1e7)); lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(dl_buffer) / 1e7)); struct UtmCoor_f utm; utm.zone = nav_utm_zone0; utm_of_lla_f(&utm, &lla); nav_move_waypoint(wp_id, utm.east, utm.north, a); /* Waypoint range is limited. Computes the UTM pos back from the relative coordinates */ utm.east = waypoints[wp_id].x + nav_utm_east0; utm.north = waypoints[wp_id].y + nav_utm_north0; DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice, &wp_id, &utm.east, &utm.north, &a, &nav_utm_zone0); } else if (msg_id == DL_BLOCK && DL_BLOCK_ac_id(dl_buffer) == AC_ID) { nav_goto_block(DL_BLOCK_block_id(dl_buffer)); SEND_NAVIGATION(DefaultChannel, DefaultDevice); } else #endif /** NAV */ #ifdef WIND_INFO if (msg_id == DL_WIND_INFO && DL_WIND_INFO_ac_id(dl_buffer) == AC_ID) { struct FloatVect2 wind; wind.x = DL_WIND_INFO_north(dl_buffer); wind.y = DL_WIND_INFO_east(dl_buffer); stateSetHorizontalWindspeed_f(&wind); #if !USE_AIRSPEED float airspeed = DL_WIND_INFO_airspeed(dl_buffer); stateSetAirspeed_f(&airspeed); #endif #ifdef WIND_INFO_RET DOWNLINK_SEND_WIND_INFO_RET(DefaultChannel, DefaultDevice, &wind.y, &wind.x, stateGetAirspeed_f()); #endif } else #endif /** WIND_INFO */ #ifdef HITL /** Infrared and GPS sensors are replaced by messages on the datalink */ if (msg_id == DL_HITL_INFRARED) { /** This code simulates infrared.c:ir_update() */ infrared.roll = DL_HITL_INFRARED_roll(dl_buffer); infrared.pitch = DL_HITL_INFRARED_pitch(dl_buffer); infrared.top = DL_HITL_INFRARED_top(dl_buffer); } else if (msg_id == DL_HITL_UBX) { /** This code simulates gps_ubx.c:parse_ubx() */ if (gps_msg_received) { gps_nb_ovrn++; } else { ubx_class = DL_HITL_UBX_class(dl_buffer); ubx_id = DL_HITL_UBX_id(dl_buffer); uint8_t l = DL_HITL_UBX_ubx_payload_length(dl_buffer); uint8_t *ubx_payload = DL_HITL_UBX_ubx_payload(dl_buffer); memcpy(ubx_msg_buf, ubx_payload, l); gps_msg_received = TRUE; } } else #endif #ifdef DlSetting if (msg_id == DL_SETTING && DL_SETTING_ac_id(dl_buffer) == AC_ID) { uint8_t i = DL_SETTING_index(dl_buffer); float val = DL_SETTING_value(dl_buffer); DlSetting(i, val); DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); } else if (msg_id == DL_GET_SETTING && DL_GET_SETTING_ac_id(dl_buffer) == AC_ID) { uint8_t i = DL_GET_SETTING_index(dl_buffer); float val = settings_get_value(i); DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val); } else #endif /** Else there is no dl_settings section in the flight plan */ #if USE_JOYSTICK if (msg_id == DL_JOYSTICK_RAW && DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) { JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(dl_buffer), DL_JOYSTICK_RAW_pitch(dl_buffer), DL_JOYSTICK_RAW_throttle(dl_buffer)); } else #endif // USE_JOYSTICK #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) { #ifdef RADIO_CONTROL_DATALINK_LED LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif parse_rc_3ch_datalink( DL_RC_3CH_throttle_mode(dl_buffer), DL_RC_3CH_roll(dl_buffer), DL_RC_3CH_pitch(dl_buffer)); } else if (msg_id == DL_RC_4CH && DL_RC_4CH_ac_id(dl_buffer) == AC_ID) { #ifdef RADIO_CONTROL_DATALINK_LED LED_TOGGLE(RADIO_CONTROL_DATALINK_LED); #endif parse_rc_4ch_datalink( DL_RC_4CH_mode(dl_buffer), DL_RC_4CH_throttle(dl_buffer), DL_RC_4CH_roll(dl_buffer), DL_RC_4CH_pitch(dl_buffer), DL_RC_4CH_yaw(dl_buffer)); } else #endif // RC_DATALINK { /* Last else */ /* Parse modules datalink */ modules_parse_datalink(msg_id); } }
/** * Auto-throttle inner loop * \brief */ void v_ctl_climb_loop(void) { // Airspeed setpoint rate limiter: // AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1 float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew; BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude); v_ctl_auto_airspeed_setpoint_slew += airspeed_incr; #ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT // Ground speed control loop (input: groundspeed error, output: airspeed controlled) float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - stateGetHorizontalSpeedNorm_f()); v_ctl_auto_groundspeed_sum_err += err_groundspeed; BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR); v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain; // Do not allow controlled airspeed below the setpoint if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) { v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew; // reset integrator of ground speed loop v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain * v_ctl_auto_groundspeed_igain); } #else v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew; #endif // Airspeed outerloop: positive means we need to accelerate float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f(); // Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f; BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration); // Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration #ifndef SITL struct Int32Vect3 accel_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta); #else float vdot = 0; #endif // Acceleration Error: positive means UAV needs to accelerate: needs extra energy float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot); // Flight Path Outerloop: positive means needs to climb more: needs extra energy float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled; // Total Energy Error: positive means energy should be added float en_tot_err = gamma_err + vdot_err; // Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up float en_dis_err = gamma_err - vdot_err; // Auto Cruise Throttle if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) { v_ctl_auto_throttle_nominal_cruise_throttle += v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude + en_tot_err * v_ctl_energy_total_igain * dt_attidude; Bound(v_ctl_auto_throttle_nominal_cruise_throttle, 0.0f, 1.0f); } // Total Controller float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + v_ctl_auto_throttle_of_airspeed_pgain * speed_error + v_ctl_energy_total_pgain * en_tot_err; if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle == 1)) { // If your energy supply is not sufficient, then neglect the climb requirement en_dis_err = -vdot_err; // adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; } if (v_ctl_climb_setpoint < 0) { v_ctl_climb_setpoint += 30. * dt_attidude; } } /* pitch pre-command */ if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) { v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude + v_ctl_energy_diff_igain * en_dis_err * dt_attidude; Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT); } float v_ctl_pitch_of_vz = + (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain - v_ctl_auto_pitch_of_airspeed_pgain * speed_error + v_ctl_auto_pitch_of_airspeed_dgain * vdot + v_ctl_energy_diff_pgain * en_dis_err + v_ctl_auto_throttle_nominal_cruise_pitch; if (kill_throttle) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; } v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch; Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT) ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration); v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ); }
#include CTRL_TYPE_H static void send_desired(struct transport_tx *trans, struct link_device *dev) { #ifndef USE_AIRSPEED float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; #endif pprz_msg_send_DESIRED(trans, dev, AC_ID, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint); } static void send_airspeed(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { float airspeed = stateGetAirspeed_f(); #if USE_AIRSPEED pprz_msg_send_AIRSPEED(trans, dev, AC_ID, &airspeed, &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); #else float zero = 0; pprz_msg_send_AIRSPEED(trans, dev, AC_ID, &airspeed, &zero, &zero, &zero); #endif } #endif /* PERIODIC_TELEMETRY */ void autopilot_send_mode(void) { // use default telemetry here #if DOWNLINK
{ #ifndef USE_AIRSPEED float v_ctl_auto_airspeed_setpoint = NOMINAL_AIRSPEED; #endif pprz_msg_send_DESIRED(trans, dev, AC_ID, &h_ctl_roll_setpoint, &h_ctl_pitch_loop_setpoint, &h_ctl_course_setpoint, &desired_x, &desired_y, &v_ctl_altitude_setpoint, &v_ctl_climb_setpoint, &v_ctl_auto_airspeed_setpoint); } static void send_airspeed(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused))) { #if USE_AIRSPEED pprz_msg_send_AIRSPEED(trans, dev, AC_ID, stateGetAirspeed_f(), &v_ctl_auto_airspeed_setpoint, &v_ctl_auto_airspeed_controlled, &v_ctl_auto_groundspeed_setpoint); #else float zero = 0; pprz_msg_send_AIRSPEED(trans, dev, AC_ID, stateGetAirspeed_f(), &zero, &zero, &zero); #endif } #endif /* PERIODIC_TELEMETRY */ void autopilot_send_mode(void) { // use default telemetry here #if DOWNLINK send_mode(&(DefaultChannel).trans_tx, &(DefaultDevice).device); #endif }
bool nav_bungee_takeoff_run(void) { float cross = 0.; // Get current position struct FloatVect2 pos; VECT2_ASSIGN(pos, stateGetPositionEnu_f()->x, stateGetPositionEnu_f()->y); switch (CTakeoffStatus) { case Launch: // Recalculate lines if below min speed if (stateGetHorizontalSpeedNorm_f() < BUNGEE_TAKEOFF_MIN_SPEED) { compute_points_from_bungee(); } // Follow Launch Line with takeoff pitch and no throttle NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); NavVerticalThrottleMode(0); // FIXME previously using altitude mode, maybe not wise without motors //NavVerticalAltitudeMode(bungee_point.z + BUNGEE_TAKEOFF_HEIGHT, 0.); nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 1; // Find out if UAV has crossed the line VECT2_DIFF(pos, pos, throttle_point); // position local to throttle_point cross = VECT2_DOT_PRODUCT(pos, takeoff_dir); if (cross > 0. && stateGetHorizontalSpeedNorm_f() > BUNGEE_TAKEOFF_MIN_SPEED) { CTakeoffStatus = Throttle; kill_throttle = 0; nav_init_stage(); } else { // If not crossed stay in this status break; } // Start throttle imidiatelly case Throttle: //Follow Launch Line NavVerticalAutoThrottleMode(BUNGEE_TAKEOFF_PITCH); NavVerticalThrottleMode(MAX_PPRZ * (BUNGEE_TAKEOFF_THROTTLE)); nav_route_xy(init_point.x, init_point.y, throttle_point.x, throttle_point.y); kill_throttle = 0; if ((stateGetPositionUtm_f()->alt > bungee_point.z + BUNGEE_TAKEOFF_HEIGHT) #if USE_AIRSPEED && (stateGetAirspeed_f() > BUNGEE_TAKEOFF_AIRSPEED) #endif ) { CTakeoffStatus = Finished; return false; } else { return true; } break; default: // Invalid status or Finished, end function return false; } return true; }
void flight_benchmark_periodic(void) { float Err_airspeed = 0; float Err_altitude = 0; float Err_position = 0; if (benchm_reset) { flight_benchmark_reset(); benchm_reset = 0; } if (benchm_go) { #if USE_AIRSPEED && defined(BENCHMARK_AIRSPEED) Err_airspeed = fabs(stateGetAirspeed_f() - v_ctl_auto_airspeed_setpoint); if (Err_airspeed > ToleranceAispeed) { Err_airspeed = Err_airspeed - ToleranceAispeed; SquareSumErr_airspeed += (Err_airspeed * Err_airspeed); } #endif #ifdef BENCHMARK_ALTITUDE Err_altitude = fabs(stateGetPositionUtm_f()->alt - v_ctl_altitude_setpoint); if (Err_altitude > ToleranceAltitude) { Err_altitude = Err_altitude - ToleranceAltitude; SquareSumErr_altitude += (Err_altitude * Err_altitude); } #endif #ifdef BENCHMARK_POSITION //---------------This part is a waste of memory and calculation power - but it works - feel free to optimize it ;-) ----------------- // err_temp = waypoints[target].x - stateGetPositionEnu_f()->x; float deltaPlaneX = 0; float deltaPlaneY = 0; float Err_position_segment = 0; float Err_position_circle = 0; // if (nav_in_segment){ float deltaX = nav_segment_x_2 - nav_segment_x_1; float deltaY = nav_segment_y_2 - nav_segment_y_1; float anglePath = atan2(deltaX, deltaY); deltaPlaneX = nav_segment_x_2 - stateGetPositionEnu_f()->x; deltaPlaneY = nav_segment_y_2 - stateGetPositionEnu_f()->y; float anglePlane = atan2(deltaPlaneX, deltaPlaneY); float angleDiff = fabs(anglePlane - anglePath); Err_position_segment = fabs(sin(angleDiff) * sqrt(deltaPlaneX * deltaPlaneX + deltaPlaneY * deltaPlaneY)); // } // if (nav_in_circle){ deltaPlaneX = nav_circle_x - stateGetPositionEnu_f()->x; deltaPlaneY = nav_circle_y - stateGetPositionEnu_f()->y; Err_position_circle = fabs(sqrt(deltaPlaneX * deltaPlaneX + deltaPlaneY * deltaPlaneY) - nav_circle_radius); // } if (Err_position_circle < Err_position_segment) { Err_position = Err_position_circle; } else { Err_position = Err_position_segment; } if (Err_position > TolerancePosition) { SquareSumErr_position += (Err_position * Err_position); } #endif } DOWNLINK_SEND_FLIGHT_BENCHMARK(DefaultChannel, DefaultDevice, &SquareSumErr_airspeed, &SquareSumErr_altitude, &SquareSumErr_position, &Err_airspeed, &Err_altitude, &Err_position) }
// Airspeed control loop (input: [airspeed controlled, climb_setpoint], output: [throttle controlled, pitch setpoint]) static inline void v_ctl_set_airspeed( void ) { static float last_err_vz = 0.; static float last_err_as = 0.; // Bound airspeed setpoint Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX); // Compute errors float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z; float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD; last_err_vz = err_vz; if (v_ctl_auto_throttle_igain > 0.) { v_ctl_auto_throttle_sum_err += err_vz*AIRSPEED_LOOP_PERIOD; BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain); } if (v_ctl_auto_pitch_igain > 0.) { v_ctl_auto_pitch_sum_err += err_vz*AIRSPEED_LOOP_PERIOD; BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain); } float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f(); float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD; last_err_as = err_airspeed; if (v_ctl_auto_airspeed_throttle_igain > 0.) { v_ctl_auto_airspeed_throttle_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD; BoundAbs(v_ctl_auto_airspeed_throttle_sum_err, V_CTL_AUTO_AIRSPEED_THROTTLE_MAX_SUM_ERR / v_ctl_auto_airspeed_throttle_igain); } if (v_ctl_auto_airspeed_pitch_igain > 0.) { v_ctl_auto_airspeed_pitch_sum_err += err_airspeed*AIRSPEED_LOOP_PERIOD; BoundAbs(v_ctl_auto_airspeed_pitch_sum_err, V_CTL_AUTO_AIRSPEED_PITCH_MAX_SUM_ERR / v_ctl_auto_airspeed_pitch_igain); } // Reset integrators in manual or before flight if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) { v_ctl_auto_throttle_sum_err = 0.; v_ctl_auto_pitch_sum_err = 0.; v_ctl_auto_airspeed_throttle_sum_err = 0.; v_ctl_auto_airspeed_pitch_sum_err = 0.; } // Pitch loop v_ctl_pitch_setpoint = v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint + v_ctl_pitch_trim + v_ctl_auto_pitch_pgain * err_vz + v_ctl_auto_pitch_dgain * d_err_vz + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err - v_ctl_auto_airspeed_pitch_pgain * err_airspeed - v_ctl_auto_airspeed_pitch_dgain * d_err_airspeed - v_ctl_auto_airspeed_pitch_igain * v_ctl_auto_airspeed_pitch_sum_err; // Throttle loop controlled_throttle = v_ctl_auto_throttle_cruise_throttle + v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint + v_ctl_auto_throttle_pgain * err_vz + v_ctl_auto_throttle_dgain * d_err_vz + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err + v_ctl_auto_airspeed_throttle_pgain * err_airspeed + v_ctl_auto_airspeed_throttle_dgain * d_err_airspeed + v_ctl_auto_airspeed_throttle_igain * v_ctl_auto_airspeed_throttle_sum_err; }