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;
}
