void ltp_of_ecef_rmat_from_lla_i(struct Int32RMat *ltp_of_ecef, struct LlaCoor_i *lla)
{
#if USE_SINGLE_PRECISION_TRIG
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
#else // use double precision by default
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
#endif
ltp_of_ecef->m[0] = -sin_lon;
ltp_of_ecef->m[1] = cos_lon;
ltp_of_ecef->m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
ltp_of_ecef->m[3] = (int32_t)((-(int64_t)sin_lat * (int64_t)cos_lon) >> HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[4] = (int32_t)((-(int64_t)sin_lat * (int64_t)sin_lon) >> HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[5] = cos_lat;
ltp_of_ecef->m[6] = (int32_t)(((int64_t)cos_lat * (int64_t)cos_lon) >> HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[7] = (int32_t)(((int64_t)cos_lat * (int64_t)sin_lon) >> HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[8] = sin_lat;
}
void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) {
/* store the origin of the tangeant plane */
LLA_COPY(def->lla, *lla);
/* compute the ecef representation of the origin */
ecef_of_lla_i(&def->ecef, &def->lla);
/* store the rotation matrix */
#if 1
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif
def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[5] = cos_lat;
def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[8] = sin_lat;
}
bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time)
{
uint16_t time_at_wp;
uint32_t dist_to_point;
static uint16_t wp_entry_time = 0;
static bool_t wp_reached = FALSE;
static struct EnuCoor_i wp_last = { 0, 0, 0 };
struct Int32Vect2 diff;
if ((wp_last.x != wp->x) || (wp_last.y != wp->y)) {
wp_reached = FALSE;
wp_last = *wp;
}
VECT2_DIFF(diff, *wp, *stateGetPositionEnu_i());
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
dist_to_point = int32_vect2_norm(&diff);
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
if (!wp_reached) {
wp_reached = TRUE;
wp_entry_time = autopilot_flight_time;
time_at_wp = 0;
} else {
time_at_wp = autopilot_flight_time - wp_entry_time;
}
} else {
time_at_wp = 0;
wp_reached = FALSE;
}
if (time_at_wp > stay_time) {
INT_VECT3_ZERO(wp_last);
return TRUE;
}
return FALSE;
}
void gain_scheduling_periodic(void)
{
#if NUMBER_OF_GAINSETS > 1
uint8_t section = 0;
//Find out between which gainsets to interpolate
while (FLOAT_OF_BFP(SCHEDULING_VARIABLE, SCHEDULING_VARIABLE_FRAC) > scheduling_points[section]) {
section++;
if (section == NUMBER_OF_GAINSETS) { break; }
}
//Get pointers for the two gainsets and the stabilization_gains
struct Int32AttitudeGains *ga, *gb, *gblend;
gblend = &stabilization_gains;
if (section == 0) {
set_gainset(0);
} else if (section == NUMBER_OF_GAINSETS) {
set_gainset(NUMBER_OF_GAINSETS - 1);
} else {
ga = &gainlibrary[section - 1];
gb = &gainlibrary[section];
//Calculate the ratio between the scheduling points
int32_t ratio;
ratio = BFP_OF_REAL((FLOAT_OF_BFP(SCHEDULING_VARIABLE,
SCHEDULING_VARIABLE_FRAC) - scheduling_points[section - 1]) / (scheduling_points[section] -
scheduling_points[section - 1]), INT32_RATIO_FRAC);
int64_t g1, g2, gbl;
//Loop through the gains and interpolate
for (int i = 0; i < (sizeof(struct Int32AttitudeGains) / sizeof(int32_t)); i++) {
g1 = *(((int32_t *) ga) + i);
g1 *= (1 << INT32_RATIO_FRAC) - ratio;
g2 = *(((int32_t *) gb) + i);
g2 *= ratio;
gbl = (g1 + g2) >> INT32_RATIO_FRAC;
*(((int32_t *) gblend) + i) = (int32_t) gbl;
}
}
#endif
}
bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
{
int32_t dist_to_point;
struct Int32Vect2 diff;
struct EnuCoor_i *pos = stateGetPositionEnu_i();
/* if an approaching_time is given, estimate diff after approching_time secs */
if (approaching_time > 0) {
struct Int32Vect2 estimated_pos;
struct Int32Vect2 estimated_progress;
struct EnuCoor_i *speed = stateGetSpeedEnu_i();
VECT2_SMUL(estimated_progress, *speed, approaching_time);
INT32_VECT2_RSHIFT(estimated_progress, estimated_progress, (INT32_SPEED_FRAC - INT32_POS_FRAC));
VECT2_SUM(estimated_pos, *pos, estimated_progress);
VECT2_DIFF(diff, *wp, estimated_pos);
}
/* else use current position */
else {
VECT2_DIFF(diff, *wp, *pos);
}
/* compute distance of estimated/current pos to target wp
* distance with half metric precision (6.25 cm)
*/
INT32_VECT2_RSHIFT(diff, diff, INT32_POS_FRAC / 2);
dist_to_point = int32_vect2_norm(&diff);
/* return TRUE if we have arrived */
if (dist_to_point < BFP_OF_REAL(ARRIVED_AT_WAYPOINT, INT32_POS_FRAC / 2)) {
return TRUE;
}
/* if coming from a valid waypoint */
if (from != NULL) {
/* return TRUE if normal line at the end of the segment is crossed */
struct Int32Vect2 from_diff;
VECT2_DIFF(from_diff, *wp, *from);
INT32_VECT2_RSHIFT(from_diff, from_diff, INT32_POS_FRAC / 2);
return (diff.x * from_diff.x + diff.y * from_diff.y < 0);
}
return FALSE;
}
void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {
/* store the origin of the tangeant plane */
/* 保存原始的切平面数据*/
VECT3_COPY(def->ecef, *ecef);
/* compute the lla representation of the origin */
/* 计算LLA原始点的位置信息（lon:rad*1e7,lat:rad*1e7,alt：mm）*/
lla_of_ecef_i(&def->lla, &def->ecef);
/* store the rotation matrix */
/* 存储旋转矩阵值*/
#if 1
//计算经度和纬度的正弦和余弦的值（先转换成float型，再求sin和cos ,最后*<<20,求得整数值）
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif
/* 以下几位ECEF到lla坐标系的转换矩阵*/
def->ltp_of_ecef.m[0] = -sin_lon;
def->ltp_of_ecef.m[1] = cos_lon;
def->ltp_of_ecef.m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
def->ltp_of_ecef.m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[5] = cos_lat;
def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[8] = sin_lat;
}
/** Adaptation function.
* @param zdd_meas vert accel measurement in m/s^2 with #INT32_ACCEL_FRAC
* @param thrust_applied controller input [0 : MAX_PPRZ]
* @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC
*/
void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref)
{
static const int32_t gv_adapt_min_cmd = GUIDANCE_V_ADAPT_MIN_CMD * MAX_PPRZ;
static const int32_t gv_adapt_max_cmd = GUIDANCE_V_ADAPT_MAX_CMD * MAX_PPRZ;
static const int32_t gv_adapt_max_accel = ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL);
/* Update only if accel and commands are in a valid range */
/* This also ensures we don't divide by zero */
if (thrust_applied < gv_adapt_min_cmd || thrust_applied > gv_adapt_max_cmd
|| zdd_meas < -gv_adapt_max_accel || zdd_meas > gv_adapt_max_accel) {
return;
}
/* We don't propagate state, it's constant ! */
/* We propagate our covariance */
gv_adapt_P = gv_adapt_P + GV_ADAPT_SYS_NOISE;
/* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 30 bits */
const int32_t g_m_zdd = ((int32_t)BFP_OF_REAL(9.81,
INT32_ACCEL_FRAC) - zdd_meas) << (GV_ADAPT_X_FRAC - INT32_ACCEL_FRAC);
if (g_m_zdd > 0) {
gv_adapt_Xmeas = (g_m_zdd + (thrust_applied >> 1)) / thrust_applied;
} else {
int32_t gv_adapt_Xmeas;
/* System noises */
#ifndef GV_ADAPT_SYS_NOISE_F
#define GV_ADAPT_SYS_NOISE_F 0.00005
#endif
#define GV_ADAPT_SYS_NOISE BFP_OF_REAL(GV_ADAPT_SYS_NOISE_F, GV_ADAPT_P_FRAC)
/* Measuremement noises */
#define GV_ADAPT_MEAS_NOISE_HOVER_F (50.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
#define GV_ADAPT_MEAS_NOISE_HOVER BFP_OF_REAL(GV_ADAPT_MEAS_NOISE_HOVER_F, GV_ADAPT_P_FRAC)
#define GV_ADAPT_MEAS_NOISE_OF_ZD (100.0*GUIDANCE_V_ADAPT_NOISE_FACTOR)
/* Initial Covariance */
#define GV_ADAPT_P0_F 0.1
static const int32_t gv_adapt_P0 = BFP_OF_REAL(GV_ADAPT_P0_F, GV_ADAPT_P_FRAC);
static const int32_t gv_adapt_X0 = BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) /
(GUIDANCE_V_ADAPT_INITIAL_HOVER_THROTTLE *MAX_PPRZ);
void gv_adapt_init(void)
{
gv_adapt_X = gv_adapt_X0;
gv_adapt_P = gv_adapt_P0;
}
#define K_FRAC 12
/** Adaptation function.
* @param zdd_meas vert accel measurement in m/s^2 with #INT32_ACCEL_FRAC
* @param thrust_applied controller input [0 : MAX_PPRZ]
* @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC
void guidance_flip_run(void)
{
uint32_t timer;
int32_t phi;
static uint32_t timer_save = 0;
timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY;
phi = stateGetNedToBodyEulers_i()->phi;
switch (flip_state) {
case 0:
flip_cmd_earth.x = 0;
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first
timer_save = 0;
if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) {
flip_state++;
}
break;
case 1:
stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
flip_state++;
}
break;
case 2:
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust?
if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) {
timer_save = timer;
flip_state++;
}
break;
case 3:
flip_cmd_earth.x = 0;
flip_cmd_earth.y = 0;
stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth,
heading_save);
stabilization_attitude_run(autopilot_in_flight);
stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling
if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) {
flip_state++;
}
break;
default:
autopilot_mode_auto2 = autopilot_mode_old;
autopilot_set_mode(autopilot_mode_old);
stab_att_sp_euler.psi = heading_save;
flip_rollout = false;
flip_counter = 0;
timer_save = 0;
flip_state = 0;
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll?
break;
}
}
