void parse_mf_daq_msg(void)
{
mf_daq.nb = dl_buffer[2];
if (mf_daq.nb > 0) {
if (mf_daq.nb > MF_DAQ_SIZE) {
mf_daq.nb = MF_DAQ_SIZE;
}
// Store data struct directly from dl_buffer
float *buf = (float*)(dl_buffer+3);
memcpy(mf_daq.values, buf, mf_daq.nb * sizeof(float));
// Log on SD card
if (log_started) {
DOWNLINK_SEND_PAYLOAD_FLOAT(pprzlog_tp, chibios_sdlog, mf_daq.nb, mf_daq.values);
DOWNLINK_SEND_MF_DAQ_STATE(pprzlog_tp, chibios_sdlog,
&autopilot_flight_time,
&stateGetBodyRates_f()->p,
&stateGetBodyRates_f()->q,
&stateGetBodyRates_f()->r,
&stateGetNedToBodyEulers_f()->phi,
&stateGetNedToBodyEulers_f()->theta,
&stateGetNedToBodyEulers_f()->psi,
&stateGetAccelNed_f()->x,
&stateGetAccelNed_f()->y,
&stateGetAccelNed_f()->z,
&stateGetSpeedEnu_f()->x,
&stateGetSpeedEnu_f()->y,
&stateGetSpeedEnu_f()->z,
&stateGetPositionLla_f()->lat,
&stateGetPositionLla_f()->lon,
&stateGetPositionLla_f()->alt,
&stateGetHorizontalWindspeed_f()->y,
&stateGetHorizontalWindspeed_f()->x);
}
}
}
static inline void v_ctl_set_pitch ( void ) {
static float last_err = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
v_ctl_auto_pitch_sum_err = 0;
// Compute errors
float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
float d_err = err - last_err;
last_err = err;
if (v_ctl_auto_pitch_igain > 0.) {
v_ctl_auto_pitch_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
}
// PI loop + feedforward ctl
v_ctl_pitch_setpoint = nav_pitch
+ v_ctl_pitch_trim
+ v_ctl_auto_throttle_pitch_of_vz_pgain * v_ctl_climb_setpoint
+ v_ctl_auto_pitch_pgain * err
+ v_ctl_auto_pitch_dgain * d_err
+ v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err;
}
static void ac_char_update(float throttle, float pitch, float climb, float accelerate)
{
if ((accelerate > -0.02) && (accelerate < 0.02)) {
if (throttle >= 1.0f) {
ac_char_climb_count++;
ac_char_average(&ac_char_climb_pitch, pitch * 57.6f, ac_char_climb_count);
ac_char_average(&ac_char_climb_max , stateGetSpeedEnu_f()->z, ac_char_climb_count);
} else if (throttle <= 0.0f) {
ac_char_descend_count++;
ac_char_average(&ac_char_descend_pitch, pitch * 57.6f , ac_char_descend_count);
ac_char_average(&ac_char_descend_max , stateGetSpeedEnu_f()->z , ac_char_descend_count);
} else if ((climb > -0.125) && (climb < 0.125)) {
ac_char_cruise_count++;
ac_char_average(&ac_char_cruise_throttle , throttle , ac_char_cruise_count);
ac_char_average(&ac_char_cruise_pitch , pitch * 57.6f , ac_char_cruise_count);
}
}
}
inline static void v_ctl_climb_auto_pitch_loop(void)
{
float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
v_ctl_throttle_setpoint = nav_throttle_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_setpoint = v_ctl_pitch_trim - v_ctl_auto_pitch_pgain *
(err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
Bound(v_ctl_pitch_setpoint, V_CTL_AUTO_PITCH_MIN_PITCH, V_CTL_AUTO_PITCH_MAX_PITCH);
}
void mf_daq_send_state(void)
{
// Send aircraft state to DAQ board
DOWNLINK_SEND_MF_DAQ_STATE(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
&autopilot_flight_time,
&stateGetBodyRates_f()->p,
&stateGetBodyRates_f()->q,
&stateGetBodyRates_f()->r,
&stateGetNedToBodyEulers_f()->phi,
&stateGetNedToBodyEulers_f()->theta,
&stateGetNedToBodyEulers_f()->psi,
&stateGetAccelNed_f()->x,
&stateGetAccelNed_f()->y,
&stateGetAccelNed_f()->z,
&stateGetSpeedEnu_f()->x,
&stateGetSpeedEnu_f()->y,
&stateGetSpeedEnu_f()->z,
&stateGetPositionLla_f()->lat,
&stateGetPositionLla_f()->lon,
&stateGetPositionLla_f()->alt,
&stateGetHorizontalWindspeed_f()->y,
&stateGetHorizontalWindspeed_f()->x);
}
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);
}
static inline void v_ctl_set_throttle( void ) {
static float last_err = 0.;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0)
v_ctl_auto_throttle_sum_err = 0;
// Compute errors
float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
float d_err = err - last_err;
last_err = err;
if (v_ctl_auto_throttle_igain > 0.) {
v_ctl_auto_throttle_sum_err += err*(1./60.);
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR / v_ctl_auto_throttle_igain);
}
// PID loop + feedforward ctl
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
+ v_ctl_auto_throttle_dgain * d_err
+ v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err;
}
/**
* 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);
}
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;
}
static void send_estimator(void) {
DOWNLINK_SEND_ESTIMATOR(DefaultChannel, DefaultDevice,
&(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z));
}
void max7456_periodic(void) {
float temp = 0;
//This code is executed always and checks if the "osd_enable" var has been changed by telemetry.
//If yes then it commands a reset but this time turns on or off the osd overlay, not the video.
if (max7456_osd_status == OSD_IDLE) {
if(osd_enable > 1)
osd_enable = 1;
if ((osd_enable<<3) != osd_enable_val) {
osd_enable_val = (osd_enable<<3);
max7456_osd_status = OSD_UNINIT;
}
}
//INITIALIZATION OF THE OSD
if (max7456_osd_status == OSD_UNINIT) {
step = 0;
max7456_trans.status = SPITransDone;
max7456_trans.output_buf[0] = OSD_VM0_REG;
//This operation needs at least 100us but when the periodic function will be invoked again
//sufficient time will have elapsed even with at a periodic frequency of 1000 Hz
max7456_trans.output_buf[1] = OSD_RESET;
max7456_osd_status = OSD_INIT1;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
else
if (max7456_osd_status == OSD_INIT2) {
max7456_trans.output_length = 1;
max7456_trans.input_length = 1;
max7456_trans.output_buf[0] = OSD_OSDBL_REG_R;
max7456_osd_status = OSD_INIT3;
spi_submit(&(MAX7456_SPI_DEV), &max7456_trans);
}
else
if (max7456_osd_status == OSD_IDLE && osd_enable > 0) { // DRAW THE OSD SCREEN
//draw_osd();
switch (step) {
case (0):
osd_put_s("HDG", FALSE, 3, 0, 13);
step = 10;
break;
case (10):
temp = ((float)electrical.vsupply)/10;
osd_sprintf(osd_string, "%.1fV", temp );
if (temp > LOW_BAT_LEVEL)
osd_put_s(osd_string, FALSE, 8, 0, 2);
else
osd_put_s(osd_string, BLINK|INVERT, 8, 0, 2);
step = 20;
break;
case (20):
#if MAG_HEADING_AVAILABLE && !defined(SITL)
temp = DegOfRad(MAG_Heading);
if (temp < 0)
temp += 360;
#else
temp = DegOfRad(state.h_speed_dir_f);
if (temp < 0)
temp += 360;
#endif
osd_sprintf(osd_string, "%.0f", temp);
osd_put_s(osd_string, FALSE, 8, 1, 13);
step = 30;
break;
case (30):
osd_sprintf(osd_string, "%.0fKm", (state.h_speed_norm_f*3.6));
osd_put_s(osd_string, FALSE, 8, 0, 24);
step = 40;
break;
case (40):
osd_sprintf(osd_string, "%.0fm", GetPosAlt() );
osd_put_s(osd_string, FALSE, 10, 13, 2);
step = 50;
break;
case (50):
osd_sprintf(osd_string, "%.1fVZ", stateGetSpeedEnu_f()->z);
osd_put_s(osd_string, FALSE, 7, 13, 24);
step = 10;
break;
default: break;
}
}
return;
}
/* conflicts detection and monitoring */
void tcas_periodic_task_1Hz( void ) {
// no TCAS under security_height
if (stateGetPositionEnu_f()->z < GROUND_ALT + SECURITY_HEIGHT) {
uint8_t i;
for (i = 0; i < NB_ACS; i++) tcas_acs_status[i].status = TCAS_NO_ALARM;
return;
}
// test possible conflicts
float tau_min = tcas_tau_ta;
uint8_t ac_id_close = AC_ID;
uint8_t i;
float vx = (*stateGetHorizontalSpeedNorm_f()) * sinf((*stateGetHorizontalSpeedDir_f()));
float vy = (*stateGetHorizontalSpeedNorm_f()) * cosf((*stateGetHorizontalSpeedDir_f()));
for (i = 2; i < NB_ACS; i++) {
if (the_acs[i].ac_id == 0) continue; // no AC data
uint32_t dt = gps.tow - the_acs[i].itow;
if (dt > 3*TCAS_DT_MAX) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // timeout, reset status
continue;
}
if (dt > TCAS_DT_MAX) continue; // lost com but keep current status
float dx = the_acs[i].east - stateGetPositionEnu_f()->x;
float dy = the_acs[i].north - stateGetPositionEnu_f()->y;
float dz = the_acs[i].alt - stateGetPositionEnu_f()->z;
float dvx = vx - the_acs[i].gspeed * sinf(the_acs[i].course);
float dvy = vy - the_acs[i].gspeed * cosf(the_acs[i].course);
float dvz = stateGetSpeedEnu_f()->z - the_acs[i].climb;
float scal = dvx*dx + dvy*dy + dvz*dz;
float ddh = dx*dx + dy*dy;
float ddv = dz*dz;
float tau = TCAS_HUGE_TAU;
if (scal > 0.) tau = (ddh + ddv) / scal;
// monitor conflicts
uint8_t inside = TCAS_IsInside();
//enum tcas_resolve test_dir = RA_NONE;
switch (tcas_acs_status[i].status) {
case TCAS_RA:
if (tau >= TCAS_HUGE_TAU && !inside) {
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
tcas_acs_status[i].resolve = RA_NONE;
DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
}
break;
case TCAS_TA:
if (tau < tcas_tau_ra || inside) {
tcas_acs_status[i].status = TCAS_RA; // TA -> RA
// Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);// FIXME only one closest AC ???
break;
}
if (tau > tcas_tau_ta && !inside)
tcas_acs_status[i].status = TCAS_NO_ALARM; // conflict is now resolved
tcas_acs_status[i].resolve = RA_NONE;
DOWNLINK_SEND_TCAS_RESOLVED(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
break;
case TCAS_NO_ALARM:
if (tau < tcas_tau_ta || inside) {
tcas_acs_status[i].status = TCAS_TA; // NO_ALARM -> TA
// Downlink warning
DOWNLINK_SEND_TCAS_TA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id));
}
if (tau < tcas_tau_ra || inside) {
tcas_acs_status[i].status = TCAS_RA; // NO_ALARM -> RA = big problem ?
// Downlink alert
//test_dir = tcas_test_direction(the_acs[i].ac_id);
//DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&(the_acs[i].ac_id),&test_dir);
}
break;
}
// store closest AC
if (tau < tau_min) {
tau_min = tau;
ac_id_close = the_acs[i].ac_id;
}
}
// set current conflict mode
if (tcas_status == TCAS_RA && tcas_ac_RA != AC_ID && tcas_acs_status[the_acs_id[tcas_ac_RA]].status == TCAS_RA) {
ac_id_close = tcas_ac_RA; // keep RA until resolved
}
tcas_status = tcas_acs_status[the_acs_id[ac_id_close]].status;
// at least one in conflict, deal with closest one
if (tcas_status == TCAS_RA) {
tcas_ac_RA = ac_id_close;
tcas_resolve = tcas_test_direction(tcas_ac_RA);
uint8_t ac_resolve = tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve;
if (ac_resolve != RA_NONE) { // first resolution, no message received
if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
else tcas_resolve = RA_CLIMB;
}
tcas_acs_status[the_acs_id[tcas_ac_RA]].resolve = RA_LEVEL; // assuming level flight for now
}
else { // second resolution or message received
if (ac_resolve != RA_LEVEL) { // message received
if (ac_resolve == tcas_resolve) { // same direction, lowest id go down
if (AC_ID < tcas_ac_RA) tcas_resolve = RA_DESCEND;
else tcas_resolve = RA_CLIMB;
}
}
else { // no message
if (tcas_resolve == RA_CLIMB && the_acs[the_acs_id[tcas_ac_RA]].climb > 1.0) tcas_resolve = RA_DESCEND; // revert resolve
else if (tcas_resolve == RA_DESCEND && the_acs[the_acs_id[tcas_ac_RA]].climb < -1.0) tcas_resolve = RA_CLIMB; // revert resolve
}
}
// Downlink alert
DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice,&tcas_ac_RA,&tcas_resolve);
}
else tcas_ac_RA = AC_ID; // no conflict
#ifdef TCAS_DEBUG
if (tcas_status == TCAS_RA) DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice,&ac_id_close,&tau_min);
#endif
}
inline static void v_ctl_climb_auto_throttle_loop(void)
{
static float last_err;
float f_throttle = 0;
float err = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
float d_err = err - last_err;
last_err = err;
float 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 + v_ctl_auto_throttle_igain * v_ctl_auto_throttle_sum_err
+ v_ctl_auto_throttle_dgain * d_err);
/* pitch pre-command */
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;
#if defined AGR_CLIMB
switch (v_ctl_auto_throttle_submode) {
case V_CTL_AUTO_THROTTLE_AGRESSIVE:
if (v_ctl_climb_setpoint > 0) { /* Climbing */
f_throttle = AGR_CLIMB_THROTTLE;
v_ctl_pitch_setpoint = AGR_CLIMB_PITCH;
} else { /* Going down */
f_throttle = AGR_DESCENT_THROTTLE;
v_ctl_pitch_setpoint = AGR_DESCENT_PITCH;
}
break;
case V_CTL_AUTO_THROTTLE_BLENDED: {
float ratio = (fabs(v_ctl_altitude_error) - AGR_BLEND_END)
/ (AGR_BLEND_START - AGR_BLEND_END);
f_throttle = (1 - ratio) * controlled_throttle;
v_ctl_pitch_setpoint = (1 - ratio) * (v_ctl_pitch_of_vz + v_ctl_pitch_trim);
v_ctl_auto_throttle_sum_err += (1 - ratio) * err;
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
/* positive error -> too low */
if (v_ctl_altitude_error > 0) {
f_throttle += ratio * AGR_CLIMB_THROTTLE;
v_ctl_pitch_setpoint += ratio * AGR_CLIMB_PITCH;
} else {
f_throttle += ratio * AGR_DESCENT_THROTTLE;
v_ctl_pitch_setpoint += ratio * AGR_DESCENT_PITCH;
}
break;
}
case V_CTL_AUTO_THROTTLE_STANDARD:
default:
#endif
f_throttle = controlled_throttle;
v_ctl_auto_throttle_sum_err += err;
BoundAbs(v_ctl_auto_throttle_sum_err, V_CTL_AUTO_THROTTLE_MAX_SUM_ERR);
v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + v_ctl_pitch_trim + nav_pitch;
#if defined AGR_CLIMB
break;
} /* switch submode */
#endif
v_ctl_throttle_setpoint = TRIM_UPPRZ(f_throttle * MAX_PPRZ);
}
// 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;
}
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;
}
static void send_estimator(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ESTIMATOR(trans, dev, AC_ID,
&(stateGetPositionUtm_f()->alt), &(stateGetSpeedEnu_f()->z));
}