mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 07:07:35 +08:00
Merge branch 'master' into dataman_nav_drton
This commit is contained in:
Anton Babushkin
@@ -0,0 +1,28 @@
|
||||
#!nsh
|
||||
#
|
||||
# Hobbyking Micro Integrated PCB Quadcopter
|
||||
# with SimonK ESC firmware and Mystery A1510 motors
|
||||
#
|
||||
# Thomas Gubler <thomasgubler@gmail.com>
|
||||
#
|
||||
echo "HK Micro PCB Quad"
|
||||
|
||||
sh /etc/init.d/4001_quad_x
|
||||
|
||||
if [ $DO_AUTOCONFIG == yes ]
|
||||
then
|
||||
param set MC_ROLL_P 7.0
|
||||
param set MC_ROLLRATE_P 0.1
|
||||
param set MC_ROLLRATE_I 0.0
|
||||
param set MC_ROLLRATE_D 0.003
|
||||
param set MC_PITCH_P 7.0
|
||||
param set MC_PITCHRATE_P 0.1
|
||||
param set MC_PITCHRATE_I 0.0
|
||||
param set MC_PITCHRATE_D 0.003
|
||||
param set MC_YAW_P 2.8
|
||||
param set MC_YAWRATE_P 0.2
|
||||
param set MC_YAWRATE_I 0.0
|
||||
param set MC_YAWRATE_D 0.0
|
||||
fi
|
||||
|
||||
set PWM_MIN 1200
|
||||
@@ -136,6 +136,11 @@ then
|
||||
sh /etc/init.d/4011_dji_f450
|
||||
fi
|
||||
|
||||
if param compare SYS_AUTOSTART 4020
|
||||
then
|
||||
sh /etc/init.d/4020_hk_micro_pcb
|
||||
fi
|
||||
|
||||
#
|
||||
# Quad +
|
||||
#
|
||||
|
||||
@@ -33,7 +33,6 @@ MODULES += drivers/hil
|
||||
MODULES += drivers/hott/hott_telemetry
|
||||
MODULES += drivers/hott/hott_sensors
|
||||
MODULES += drivers/blinkm
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += drivers/airspeed
|
||||
MODULES += drivers/ets_airspeed
|
||||
MODULES += drivers/meas_airspeed
|
||||
|
||||
@@ -25,6 +25,7 @@ MODULES += drivers/l3gd20
|
||||
MODULES += drivers/hmc5883
|
||||
MODULES += drivers/ms5611
|
||||
MODULES += drivers/pca8574
|
||||
MODULES += drivers/roboclaw
|
||||
MODULES += systemcmds/perf
|
||||
MODULES += systemcmds/reboot
|
||||
MODULES += systemcmds/tests
|
||||
|
||||
@@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) :
|
||||
safety_sub_fd(-1),
|
||||
num_of_cells(0),
|
||||
detected_cells_runcount(0),
|
||||
t_led_color({0}),
|
||||
t_led_color{0},
|
||||
t_led_blink(0),
|
||||
led_thread_runcount(0),
|
||||
led_interval(1000),
|
||||
|
||||
+11
-13
@@ -294,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
||||
// calculate the position of the start and end points. We should not be doing this often
|
||||
// as this function generally will not be called repeatedly when we are out of the sector.
|
||||
|
||||
// TO DO - this is messed up and won't compile
|
||||
float start_disp_x = radius * sinf(arc_start_bearing);
|
||||
float start_disp_y = radius * cosf(arc_start_bearing);
|
||||
float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep));
|
||||
float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep));
|
||||
float lon_start = lon_now + start_disp_x / 111111.0f;
|
||||
float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f;
|
||||
float lon_end = lon_now + end_disp_x / 111111.0f;
|
||||
float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f;
|
||||
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
double start_disp_x = (double)radius * sin(arc_start_bearing);
|
||||
double start_disp_y = (double)radius * cos(arc_start_bearing);
|
||||
double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep)));
|
||||
double lon_start = lon_now + start_disp_x / 111111.0;
|
||||
double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0;
|
||||
double lon_end = lon_now + end_disp_x / 111111.0;
|
||||
double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0;
|
||||
double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
|
||||
|
||||
|
||||
if (dist_to_start < dist_to_end) {
|
||||
crosstrack_error->distance = dist_to_start;
|
||||
crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
|
||||
|
||||
} else {
|
||||
crosstrack_error->past_end = true;
|
||||
crosstrack_error->distance = dist_to_end;
|
||||
@@ -319,7 +317,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do
|
||||
|
||||
}
|
||||
|
||||
crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
|
||||
crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing);
|
||||
return_value = OK;
|
||||
return return_value;
|
||||
}
|
||||
|
||||
@@ -474,7 +474,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
||||
// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
|
||||
// We use an float epsilon delta to test float equality.
|
||||
if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
|
||||
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
|
||||
mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1);
|
||||
|
||||
} else {
|
||||
|
||||
@@ -636,7 +636,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
/* welcome user */
|
||||
warnx("starting");
|
||||
|
||||
char *main_states_str[MAIN_STATE_MAX];
|
||||
const char *main_states_str[MAIN_STATE_MAX];
|
||||
main_states_str[MAIN_STATE_MANUAL] = "MANUAL";
|
||||
main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL";
|
||||
main_states_str[MAIN_STATE_POSCTL] = "POSCTL";
|
||||
@@ -645,7 +645,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
|
||||
main_states_str[MAIN_STATE_ACRO] = "ACRO";
|
||||
|
||||
char *arming_states_str[ARMING_STATE_MAX];
|
||||
const char *arming_states_str[ARMING_STATE_MAX];
|
||||
arming_states_str[ARMING_STATE_INIT] = "INIT";
|
||||
arming_states_str[ARMING_STATE_STANDBY] = "STANDBY";
|
||||
arming_states_str[ARMING_STATE_ARMED] = "ARMED";
|
||||
@@ -654,7 +654,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
arming_states_str[ARMING_STATE_REBOOT] = "REBOOT";
|
||||
arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE";
|
||||
|
||||
char *nav_states_str[NAVIGATION_STATE_MAX];
|
||||
const char *nav_states_str[NAVIGATION_STATE_MAX];
|
||||
nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL";
|
||||
nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL";
|
||||
nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL";
|
||||
@@ -797,7 +797,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
|
||||
bool updated = false;
|
||||
|
||||
bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
rc_calibration_check(mavlink_fd);
|
||||
|
||||
/* Subscribe to safety topic */
|
||||
int safety_sub = orb_subscribe(ORB_ID(safety));
|
||||
@@ -820,10 +820,16 @@ int commander_thread_main(int argc, char *argv[])
|
||||
struct offboard_control_setpoint_s sp_offboard;
|
||||
memset(&sp_offboard, 0, sizeof(sp_offboard));
|
||||
|
||||
/* Subscribe to telemetry status */
|
||||
int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
struct telemetry_status_s telemetry;
|
||||
memset(&telemetry, 0, sizeof(telemetry));
|
||||
/* Subscribe to telemetry status topics */
|
||||
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
|
||||
telemetry_last_heartbeat[i] = 0;
|
||||
telemetry_lost[i] = true;
|
||||
}
|
||||
|
||||
/* Subscribe to global position */
|
||||
int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
|
||||
@@ -905,7 +911,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
bool arming_state_changed = false;
|
||||
bool main_state_changed = false;
|
||||
bool failsafe_old = false;
|
||||
bool system_checked = false;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
@@ -953,7 +958,7 @@ int commander_thread_main(int argc, char *argv[])
|
||||
status_changed = true;
|
||||
|
||||
/* re-check RC calibration */
|
||||
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
|
||||
rc_calibration_check(mavlink_fd);
|
||||
}
|
||||
|
||||
/* navigation parameters */
|
||||
@@ -962,15 +967,6 @@ int commander_thread_main(int argc, char *argv[])
|
||||
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
|
||||
}
|
||||
|
||||
/* Perform system checks (again) once params are loaded and MAVLink is up. */
|
||||
if (!system_checked && mavlink_fd &&
|
||||
(telemetry.heartbeat_time > 0) &&
|
||||
(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
|
||||
|
||||
(void)rc_calibration_check(mavlink_fd);
|
||||
system_checked = true;
|
||||
}
|
||||
|
||||
orb_check(sp_man_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
@@ -983,10 +979,26 @@ int commander_thread_main(int argc, char *argv[])
|
||||
orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
|
||||
}
|
||||
|
||||
orb_check(telemetry_sub, &updated);
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
orb_check(telemetry_subs[i], &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry);
|
||||
if (updated) {
|
||||
struct telemetry_status_s telemetry;
|
||||
memset(&telemetry, 0, sizeof(telemetry));
|
||||
|
||||
orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry);
|
||||
|
||||
/* perform system checks when new telemetry link connected */
|
||||
if (mavlink_fd &&
|
||||
telemetry_last_heartbeat[i] == 0 &&
|
||||
telemetry.heartbeat_time > 0 &&
|
||||
hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) {
|
||||
|
||||
(void)rc_calibration_check(mavlink_fd);
|
||||
}
|
||||
|
||||
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
|
||||
}
|
||||
}
|
||||
|
||||
orb_check(sensor_sub, &updated);
|
||||
@@ -1390,18 +1402,35 @@ int commander_thread_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
/* data link check */
|
||||
if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) {
|
||||
/* data links check */
|
||||
bool have_link = false;
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) {
|
||||
/* handle the case where data link was regained */
|
||||
if (telemetry_lost[i]) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i);
|
||||
telemetry_lost[i] = false;
|
||||
}
|
||||
have_link = true;
|
||||
|
||||
} else {
|
||||
if (!telemetry_lost[i]) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i);
|
||||
telemetry_lost[i] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (have_link) {
|
||||
/* handle the case where data link was regained */
|
||||
if (status.data_link_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: data link regained");
|
||||
status.data_link_lost = false;
|
||||
status_changed = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.data_link_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST");
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST");
|
||||
status.data_link_lost = true;
|
||||
status_changed = true;
|
||||
}
|
||||
@@ -1743,6 +1772,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
|
||||
}
|
||||
|
||||
print_reject_mode(status, "AUTO_MISSION");
|
||||
|
||||
// fallback to LOITER if home position not set
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO_LOITER);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
}
|
||||
|
||||
// fallback to POSCTL
|
||||
|
||||
@@ -271,7 +271,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
|
||||
}
|
||||
break;
|
||||
|
||||
case MAIN_STATE_AUTO_MISSION:
|
||||
case MAIN_STATE_AUTO_LOITER:
|
||||
/* need global position estimate */
|
||||
if (status->condition_global_position_valid) {
|
||||
@@ -279,6 +278,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
|
||||
}
|
||||
break;
|
||||
|
||||
case MAIN_STATE_AUTO_MISSION:
|
||||
case MAIN_STATE_AUTO_RTL:
|
||||
/* need global position and home position */
|
||||
if (status->condition_global_position_valid && status->condition_home_position_valid) {
|
||||
|
||||
@@ -1533,7 +1533,7 @@ FixedwingEstimator::start()
|
||||
_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 40,
|
||||
6000,
|
||||
5000,
|
||||
(main_t)&FixedwingEstimator::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
||||
@@ -25,6 +25,7 @@ AttPosEKF::AttPosEKF()
|
||||
useAirspeed = true;
|
||||
useCompass = true;
|
||||
useRangeFinder = true;
|
||||
useOpticalFlow = true;
|
||||
numericalProtection = true;
|
||||
refSet = false;
|
||||
storeIndex = 0;
|
||||
@@ -227,10 +228,22 @@ void AttPosEKF::CovariancePrediction(float dt)
|
||||
// scale gyro bias noise when on ground to allow for faster bias estimation
|
||||
for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma;
|
||||
processNoise[13] = dVelBiasSigma;
|
||||
for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
|
||||
for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
|
||||
for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma;
|
||||
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
|
||||
if (!inhibitWindStates) {
|
||||
for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma;
|
||||
} else {
|
||||
for (uint8_t i=14; i<=15; i++) processNoise[i] = 0;
|
||||
}
|
||||
if (!inhibitMagStates) {
|
||||
for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma;
|
||||
for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma;
|
||||
} else {
|
||||
for (uint8_t i=16; i<=21; i++) processNoise[i] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma;
|
||||
} else {
|
||||
processNoise[22] = 0;
|
||||
}
|
||||
|
||||
// square all sigmas
|
||||
for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]);
|
||||
@@ -842,30 +855,6 @@ void AttPosEKF::CovariancePrediction(float dt)
|
||||
nextP[i][i] = nextP[i][i] + processNoise[i];
|
||||
}
|
||||
|
||||
// If on ground or no magnetometer fitted, inhibit magnetometer bias updates by
|
||||
// setting the coresponding covariance terms to zero.
|
||||
if (onGround || !useCompass)
|
||||
{
|
||||
zeroRows(nextP,16,21);
|
||||
zeroCols(nextP,16,21);
|
||||
}
|
||||
|
||||
// If on ground or not using airspeed sensing, inhibit wind velocity
|
||||
// covariance growth.
|
||||
if (onGround || !useAirspeed)
|
||||
{
|
||||
zeroRows(nextP,14,15);
|
||||
zeroCols(nextP,14,15);
|
||||
}
|
||||
|
||||
// If on ground, inhibit terrain offset updates by
|
||||
// setting the coresponding covariance terms to zero.
|
||||
if (onGround)
|
||||
{
|
||||
zeroRows(nextP,22,22);
|
||||
zeroCols(nextP,22,22);
|
||||
}
|
||||
|
||||
// If the total position variance exceds 1E6 (1000m), then stop covariance
|
||||
// growth by setting the predicted to the previous values
|
||||
// This prevent an ill conditioned matrix from occurring for long periods
|
||||
@@ -882,48 +871,22 @@ void AttPosEKF::CovariancePrediction(float dt)
|
||||
}
|
||||
}
|
||||
|
||||
if (onGround || staticMode) {
|
||||
// copy the portion of the variances we want to
|
||||
// propagate
|
||||
for (unsigned i = 0; i <= 13; i++) {
|
||||
P[i][i] = nextP[i][i];
|
||||
}
|
||||
|
||||
// force symmetry for observable states
|
||||
// force zero for non-observable states
|
||||
for (unsigned i = 1; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < i; j++)
|
||||
{
|
||||
if ((i > 13) || (j > 13)) {
|
||||
P[i][j] = 0.0f;
|
||||
} else {
|
||||
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
|
||||
}
|
||||
P[j][i] = P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
|
||||
// Copy covariance
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
P[i][i] = nextP[i][i];
|
||||
}
|
||||
|
||||
// force symmetry for observable states
|
||||
for (unsigned i = 1; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < i; j++)
|
||||
{
|
||||
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
|
||||
P[j][i] = P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
// Copy covariance
|
||||
for (unsigned i = 0; i < n_states; i++) {
|
||||
P[i][i] = nextP[i][i];
|
||||
}
|
||||
|
||||
ConstrainVariances();
|
||||
// force symmetry for observable states
|
||||
for (unsigned i = 1; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < i; j++)
|
||||
{
|
||||
P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]);
|
||||
P[j][i] = P[i][j];
|
||||
}
|
||||
}
|
||||
|
||||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::FuseVelposNED()
|
||||
@@ -944,7 +907,7 @@ void AttPosEKF::FuseVelposNED()
|
||||
bool fuseData[6] = {false,false,false,false,false,false};
|
||||
uint8_t stateIndex;
|
||||
uint8_t obsIndex;
|
||||
uint8_t indexLimit;
|
||||
uint8_t indexLimit = 22;
|
||||
|
||||
// declare variables used by state and covariance update calculations
|
||||
float velErr;
|
||||
@@ -981,11 +944,6 @@ void AttPosEKF::FuseVelposNED()
|
||||
R_OBS[4] = R_OBS[3];
|
||||
R_OBS[5] = sq(posDSigma) + sq(posErr);
|
||||
|
||||
// Set innovation variances to zero default
|
||||
for (uint8_t i = 0; i<=5; i++)
|
||||
{
|
||||
varInnovVelPos[i] = 0.0f;
|
||||
}
|
||||
// calculate innovations and check GPS data validity using an innovation consistency check
|
||||
if (fuseVelData)
|
||||
{
|
||||
@@ -1071,15 +1029,6 @@ void AttPosEKF::FuseVelposNED()
|
||||
{
|
||||
fuseData[5] = true;
|
||||
}
|
||||
// Limit range of states modified when on ground
|
||||
if(!onGround)
|
||||
{
|
||||
indexLimit = 22;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexLimit = 13;
|
||||
}
|
||||
// Fuse measurements sequentially
|
||||
for (obsIndex=0; obsIndex<=5; obsIndex++)
|
||||
{
|
||||
@@ -1113,6 +1062,22 @@ void AttPosEKF::FuseVelposNED()
|
||||
if (obsIndex != 5) {
|
||||
Kfusion[13] = 0;
|
||||
}
|
||||
// Don't update wind states if inhibited
|
||||
if (inhibitWindStates) {
|
||||
Kfusion[14] = 0;
|
||||
Kfusion[15] = 0;
|
||||
}
|
||||
// Don't update magnetic field states if inhibited
|
||||
if (inhibitMagStates) {
|
||||
for (uint8_t i = 16; i<=21; i++)
|
||||
{
|
||||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
// Don't update terrain state if inhibited
|
||||
if (inhibitGndHgtState) {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
|
||||
// Calculate state corrections and re-normalise the quaternions
|
||||
for (uint8_t i = 0; i<=indexLimit; i++)
|
||||
@@ -1179,7 +1144,6 @@ void AttPosEKF::FuseMagnetometer()
|
||||
for (uint8_t i = 0; i < n_states; i++) {
|
||||
H_MAG[i] = 0.0f;
|
||||
}
|
||||
unsigned indexLimit;
|
||||
|
||||
// Perform sequential fusion of Magnetometer measurements.
|
||||
// This assumes that the errors in the different components are
|
||||
@@ -1189,19 +1153,6 @@ void AttPosEKF::FuseMagnetometer()
|
||||
// associated with sequential fusion
|
||||
if (useCompass && fuseMagData && (obsIndex < 3))
|
||||
{
|
||||
// Limit range of states modified when on ground
|
||||
if(!onGround)
|
||||
{
|
||||
indexLimit = n_states;
|
||||
}
|
||||
else
|
||||
{
|
||||
indexLimit = 13 + 1;
|
||||
}
|
||||
|
||||
// Sequential fusion of XYZ components to spread processing load across
|
||||
// three prediction time steps.
|
||||
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (obsIndex == 0)
|
||||
{
|
||||
@@ -1287,15 +1238,31 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]);
|
||||
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
|
||||
Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]);
|
||||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
|
||||
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
|
||||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
|
||||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
|
||||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
|
||||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
|
||||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
|
||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
|
||||
// Estimation of selected states is inhibited by setting their Kalman gains to zero
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]);
|
||||
Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]);
|
||||
} else {
|
||||
Kfusion[14] = 0;
|
||||
Kfusion[15] = 0;
|
||||
}
|
||||
if (!inhibitMagStates) {
|
||||
Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]);
|
||||
Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]);
|
||||
Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]);
|
||||
Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]);
|
||||
Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]);
|
||||
Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]);
|
||||
} else {
|
||||
for (uint8_t i=16; i <= 21; i++) {
|
||||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[0] = 1.0f/SK_MX[0];
|
||||
innovMag[0] = MagPred[0] - magData.x;
|
||||
}
|
||||
@@ -1342,15 +1309,34 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[12] = SK_MY[0]*(P[12][20] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][17]*SK_MY[1] - P[12][16]*SK_MY[3] + P[12][18]*SK_MY[4]);
|
||||
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
|
||||
Kfusion[13] = 0.0f;//SK_MY[0]*(P[13][20] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][17]*SK_MY[1] - P[13][16]*SK_MY[3] + P[13][18]*SK_MY[4]);
|
||||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
|
||||
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
|
||||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
|
||||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
|
||||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
|
||||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
|
||||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
|
||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
// Estimation of selected states is inhibited by setting their Kalman gains to zero
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[14] = SK_MY[0]*(P[14][20] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][17]*SK_MY[1] - P[14][16]*SK_MY[3] + P[14][18]*SK_MY[4]);
|
||||
Kfusion[15] = SK_MY[0]*(P[15][20] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][17]*SK_MY[1] - P[15][16]*SK_MY[3] + P[15][18]*SK_MY[4]);
|
||||
} else {
|
||||
Kfusion[14] = 0;
|
||||
Kfusion[15] = 0;
|
||||
}
|
||||
if (!inhibitMagStates) {
|
||||
Kfusion[16] = SK_MY[0]*(P[16][20] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][17]*SK_MY[1] - P[16][16]*SK_MY[3] + P[16][18]*SK_MY[4]);
|
||||
Kfusion[17] = SK_MY[0]*(P[17][20] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][17]*SK_MY[1] - P[17][16]*SK_MY[3] + P[17][18]*SK_MY[4]);
|
||||
Kfusion[18] = SK_MY[0]*(P[18][20] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][17]*SK_MY[1] - P[18][16]*SK_MY[3] + P[18][18]*SK_MY[4]);
|
||||
Kfusion[19] = SK_MY[0]*(P[19][20] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][17]*SK_MY[1] - P[19][16]*SK_MY[3] + P[19][18]*SK_MY[4]);
|
||||
Kfusion[20] = SK_MY[0]*(P[20][20] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][17]*SK_MY[1] - P[20][16]*SK_MY[3] + P[20][18]*SK_MY[4]);
|
||||
Kfusion[21] = SK_MY[0]*(P[21][20] + P[21][0]*SH_MAG[2] + P[21][1]*SH_MAG[1] + P[21][2]*SH_MAG[0] - P[21][3]*SK_MY[2] - P[21][17]*SK_MY[1] - P[21][16]*SK_MY[3] + P[21][18]*SK_MY[4]);
|
||||
} else {
|
||||
Kfusion[16] = 0;
|
||||
Kfusion[17] = 0;
|
||||
Kfusion[18] = 0;
|
||||
Kfusion[19] = 0;
|
||||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MY[0]*(P[22][20] + P[22][0]*SH_MAG[2] + P[22][1]*SH_MAG[1] + P[22][2]*SH_MAG[0] - P[22][3]*SK_MY[2] - P[22][17]*SK_MY[1] - P[22][16]*SK_MY[3] + P[22][18]*SK_MY[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[1] = 1.0f/SK_MY[0];
|
||||
innovMag[1] = MagPred[1] - magData.y;
|
||||
}
|
||||
@@ -1398,15 +1384,34 @@ void AttPosEKF::FuseMagnetometer()
|
||||
Kfusion[12] = SK_MZ[0]*(P[12][21] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][18]*SK_MZ[1] + P[12][16]*SK_MZ[5] - P[12][17]*SK_MZ[4]);
|
||||
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
|
||||
Kfusion[13] = 0.0f;//SK_MZ[0]*(P[13][21] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][18]*SK_MZ[1] + P[13][16]*SK_MZ[5] - P[13][17]*SK_MZ[4]);
|
||||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
|
||||
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
|
||||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
|
||||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
|
||||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
|
||||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
|
||||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
|
||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
|
||||
// Estimation of selected states is inhibited by setting their Kalman gains to zero
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[14] = SK_MZ[0]*(P[14][21] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][18]*SK_MZ[1] + P[14][16]*SK_MZ[5] - P[14][17]*SK_MZ[4]);
|
||||
Kfusion[15] = SK_MZ[0]*(P[15][21] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][18]*SK_MZ[1] + P[15][16]*SK_MZ[5] - P[15][17]*SK_MZ[4]);
|
||||
} else {
|
||||
Kfusion[14] = 0;
|
||||
Kfusion[15] = 0;
|
||||
}
|
||||
if (!inhibitMagStates) {
|
||||
Kfusion[16] = SK_MZ[0]*(P[16][21] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][18]*SK_MZ[1] + P[16][16]*SK_MZ[5] - P[16][17]*SK_MZ[4]);
|
||||
Kfusion[17] = SK_MZ[0]*(P[17][21] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][18]*SK_MZ[1] + P[17][16]*SK_MZ[5] - P[17][17]*SK_MZ[4]);
|
||||
Kfusion[18] = SK_MZ[0]*(P[18][21] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][18]*SK_MZ[1] + P[18][16]*SK_MZ[5] - P[18][17]*SK_MZ[4]);
|
||||
Kfusion[19] = SK_MZ[0]*(P[19][21] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][18]*SK_MZ[1] + P[19][16]*SK_MZ[5] - P[19][17]*SK_MZ[4]);
|
||||
Kfusion[20] = SK_MZ[0]*(P[20][21] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][18]*SK_MZ[1] + P[20][16]*SK_MZ[5] - P[20][17]*SK_MZ[4]);
|
||||
Kfusion[21] = SK_MZ[0]*(P[21][21] + P[21][0]*SH_MAG[1] + P[21][3]*SH_MAG[0] - P[21][1]*SK_MZ[2] + P[21][2]*SK_MZ[3] + P[21][18]*SK_MZ[1] + P[21][16]*SK_MZ[5] - P[21][17]*SK_MZ[4]);
|
||||
} else {
|
||||
Kfusion[16] = 0;
|
||||
Kfusion[17] = 0;
|
||||
Kfusion[18] = 0;
|
||||
Kfusion[19] = 0;
|
||||
Kfusion[20] = 0;
|
||||
Kfusion[21] = 0;
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_MZ[0]*(P[22][21] + P[22][0]*SH_MAG[1] + P[22][3]*SH_MAG[0] - P[22][1]*SK_MZ[2] + P[22][2]*SK_MZ[3] + P[22][18]*SK_MZ[1] + P[22][16]*SK_MZ[5] - P[22][17]*SK_MZ[4]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovMag[2] = 1.0f/SK_MZ[0];
|
||||
innovMag[2] = MagPred[2] - magData.z;
|
||||
|
||||
@@ -1416,7 +1421,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||
if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j= 0; j < indexLimit; j++)
|
||||
for (uint8_t j= 0; j < n_states; j++)
|
||||
{
|
||||
states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
|
||||
}
|
||||
@@ -1433,7 +1438,7 @@ void AttPosEKF::FuseMagnetometer()
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (uint8_t i = 0; i < indexLimit; i++)
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j <= 3; j++)
|
||||
{
|
||||
@@ -1455,9 +1460,9 @@ void AttPosEKF::FuseMagnetometer()
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < indexLimit; i++)
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < indexLimit; j++)
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
KHP[i][j] = 0.0f;
|
||||
for (uint8_t k = 0; k <= 3; k++)
|
||||
@@ -1474,9 +1479,9 @@ void AttPosEKF::FuseMagnetometer()
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < indexLimit; i++)
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < indexLimit; j++)
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
@@ -1552,15 +1557,31 @@ void AttPosEKF::FuseAirspeed()
|
||||
Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][14]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][15]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]);
|
||||
// Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate
|
||||
Kfusion[13] = 0.0f;//SK_TAS*(P[13][4]*SH_TAS[2] - P[13][14]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][15]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]);
|
||||
Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
|
||||
Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
|
||||
Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
|
||||
Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
|
||||
Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
|
||||
Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
|
||||
Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
|
||||
Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
|
||||
Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
// Estimation of selected states is inhibited by setting their Kalman gains to zero
|
||||
if (!inhibitWindStates) {
|
||||
Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][14]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][15]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]);
|
||||
Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][14]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][15]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]);
|
||||
} else {
|
||||
Kfusion[14] = 0;
|
||||
Kfusion[15] = 0;
|
||||
}
|
||||
if (!inhibitMagStates) {
|
||||
Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][14]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][15]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]);
|
||||
Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][14]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][15]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]);
|
||||
Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][14]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][15]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]);
|
||||
Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][14]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][15]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]);
|
||||
Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][14]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][15]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]);
|
||||
Kfusion[21] = SK_TAS*(P[21][4]*SH_TAS[2] - P[21][14]*SH_TAS[2] + P[21][5]*SH_TAS[1] - P[21][15]*SH_TAS[1] + P[21][6]*vd*SH_TAS[0]);
|
||||
} else {
|
||||
for (uint8_t i=16; i <= 21; i++) {
|
||||
Kfusion[i] = 0;
|
||||
}
|
||||
}
|
||||
if (!inhibitGndHgtState) {
|
||||
Kfusion[22] = SK_TAS*(P[22][4]*SH_TAS[2] - P[22][14]*SH_TAS[2] + P[22][5]*SH_TAS[1] - P[22][15]*SH_TAS[1] + P[22][6]*vd*SH_TAS[0]);
|
||||
} else {
|
||||
Kfusion[22] = 0;
|
||||
}
|
||||
varInnovVtas = 1.0f/SK_TAS;
|
||||
|
||||
// Calculate the measurement innovation
|
||||
@@ -1662,9 +1683,9 @@ void AttPosEKF::FuseRangeFinder()
|
||||
float ptd = statesAtRngTime[22];
|
||||
|
||||
// Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data
|
||||
SH_RNG[4] = sin(rngFinderPitch);
|
||||
SH_RNG[4] = sinf(rngFinderPitch);
|
||||
cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch);
|
||||
if (useRangeFinder && cosRngTilt > 0.87f)
|
||||
if (useRangeFinder && fuseRngData && cosRngTilt > 0.87f)
|
||||
{
|
||||
// Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset
|
||||
// This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations
|
||||
@@ -1685,10 +1706,12 @@ void AttPosEKF::FuseRangeFinder()
|
||||
SK_RNG[5] = SH_RNG[2];
|
||||
Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]);
|
||||
|
||||
// Calculate the innovation variance for data logging
|
||||
varInnovRng = 1.0f/SK_RNG[0];
|
||||
|
||||
// Calculate the measurement innovation
|
||||
rngPred = (ptd - pd)/cosRngTilt;
|
||||
innovRng = rngPred - rngMea;
|
||||
//printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd);
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 5Sigma
|
||||
if ((innovRng*innovRng*SK_RNG[0]) < 25)
|
||||
@@ -1704,6 +1727,293 @@ void AttPosEKF::FuseRangeFinder()
|
||||
|
||||
}
|
||||
|
||||
void AttPosEKF::FuseOptFlow()
|
||||
{
|
||||
static uint8_t obsIndex;
|
||||
static float SH_LOS[13];
|
||||
static float SKK_LOS[15];
|
||||
static float SK_LOS[2];
|
||||
static float q0 = 0.0f;
|
||||
static float q1 = 0.0f;
|
||||
static float q2 = 0.0f;
|
||||
static float q3 = 1.0f;
|
||||
static float vn = 0.0f;
|
||||
static float ve = 0.0f;
|
||||
static float vd = 0.0f;
|
||||
static float pd = 0.0f;
|
||||
static float ptd = 0.0f;
|
||||
static Vector3f delAng;
|
||||
static float R_LOS = 0.01f;
|
||||
static float losPred[2];
|
||||
|
||||
// Transformation matrix from nav to body axes
|
||||
Mat3f Tnb_local;
|
||||
// Transformation matrix from body to sensor axes
|
||||
// assume camera is aligned with Z body axis plus a misalignment
|
||||
// defined by 3 small angles about X, Y and Z body axis
|
||||
Mat3f Tbs;
|
||||
Tbs.x.y = a3;
|
||||
Tbs.y.x = -a3;
|
||||
Tbs.x.z = -a2;
|
||||
Tbs.z.x = a2;
|
||||
Tbs.y.z = a1;
|
||||
Tbs.z.y = -a1;
|
||||
// Transformation matrix from navigation to sensor axes
|
||||
Mat3f Tns;
|
||||
float H_LOS[n_states];
|
||||
for (uint8_t i = 0; i < n_states; i++) {
|
||||
H_LOS[i] = 0.0f;
|
||||
}
|
||||
Vector3f velNED_local;
|
||||
Vector3f relVelSensor;
|
||||
|
||||
// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg.
|
||||
if (useOpticalFlow && (fuseOptFlowData || obsIndex == 1) && !onGround && Tbs.z.z > 0.866f && rngMea > 5.0f && rngMea < 39.0f)
|
||||
{
|
||||
// Sequential fusion of XY components to spread processing load across
|
||||
// two prediction time steps.
|
||||
|
||||
// Calculate observation jacobians and Kalman gains
|
||||
if (fuseOptFlowData)
|
||||
{
|
||||
// Copy required states to local variable names
|
||||
q0 = statesAtOptFlowTime[0];
|
||||
q1 = statesAtOptFlowTime[1];
|
||||
q2 = statesAtOptFlowTime[2];
|
||||
q3 = statesAtOptFlowTime[3];
|
||||
vn = statesAtOptFlowTime[4];
|
||||
ve = statesAtOptFlowTime[5];
|
||||
vd = statesAtOptFlowTime[6];
|
||||
pd = statesAtOptFlowTime[9];
|
||||
ptd = statesAtOptFlowTime[22];
|
||||
velNED_local.x = vn;
|
||||
velNED_local.y = ve;
|
||||
velNED_local.z = vd;
|
||||
|
||||
// calculate rotation from NED to body axes
|
||||
float q00 = sq(q0);
|
||||
float q11 = sq(q1);
|
||||
float q22 = sq(q2);
|
||||
float q33 = sq(q3);
|
||||
float q01 = q0 * q1;
|
||||
float q02 = q0 * q2;
|
||||
float q03 = q0 * q3;
|
||||
float q12 = q1 * q2;
|
||||
float q13 = q1 * q3;
|
||||
float q23 = q2 * q3;
|
||||
Tnb_local.x.x = q00 + q11 - q22 - q33;
|
||||
Tnb_local.y.y = q00 - q11 + q22 - q33;
|
||||
Tnb_local.z.z = q00 - q11 - q22 + q33;
|
||||
Tnb_local.y.x = 2*(q12 - q03);
|
||||
Tnb_local.z.x = 2*(q13 + q02);
|
||||
Tnb_local.x.y = 2*(q12 + q03);
|
||||
Tnb_local.z.y = 2*(q23 - q01);
|
||||
Tnb_local.x.z = 2*(q13 - q02);
|
||||
Tnb_local.y.z = 2*(q23 + q01);
|
||||
|
||||
// calculate transformation from NED to sensor axes
|
||||
Tns = Tbs*Tnb_local;
|
||||
|
||||
// calculate range from ground plain to centre of sensor fov assuming flat earth
|
||||
float range = ConstrainFloat(((ptd - pd)/Tns.z.z),0.5f,100.0f);
|
||||
|
||||
// calculate relative velocity in sensor frame
|
||||
relVelSensor = Tns*velNED_local;
|
||||
|
||||
// calculate delta angles in sensor axes
|
||||
Vector3f delAngRel = Tbs*delAng;
|
||||
|
||||
// divide velocity by range and include angular rate effects to get predicted angular LOS rates relative to X and Y axes
|
||||
losPred[0] = relVelSensor.y/range;
|
||||
losPred[1] = -relVelSensor.x/range;
|
||||
|
||||
//printf("relVelSensor.x=%5.1f, relVelSensor.y=%5.1f\n", relVelSensor.x, relVelSensor.y);
|
||||
//printf("Xpred=%5.2f, Xmea=%5.2f, Ypred=%5.2f, Ymea=%5.2f, delAng.x=%4.4f, delAng.y=%4.4f\n", losPred[0], losData[0], losPred[1], losData[1], delAng.x, delAng.y);
|
||||
//printf("omegaX=%5.2f, omegaY=%5.2f, velY=%5.1f velX=%5.1f\n, range=%5.1f\n", delAngRel.x/dt, delAngRel.y/dt, relVelSensor.y, relVelSensor.x, range);
|
||||
|
||||
// Calculate observation jacobians
|
||||
SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3);
|
||||
SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
SH_LOS[3] = 1/(pd - ptd);
|
||||
SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2;
|
||||
SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1;
|
||||
SH_LOS[7] = 1/sq(pd - ptd);
|
||||
SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1;
|
||||
SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0;
|
||||
SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3;
|
||||
SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0;
|
||||
SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2;
|
||||
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3);
|
||||
H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3));
|
||||
H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gain
|
||||
SKK_LOS[0] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3);
|
||||
SKK_LOS[1] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3);
|
||||
SKK_LOS[2] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3);
|
||||
SKK_LOS[3] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3);
|
||||
SKK_LOS[4] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3);
|
||||
SKK_LOS[5] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3);
|
||||
SKK_LOS[6] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]);
|
||||
SKK_LOS[7] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]);
|
||||
SKK_LOS[8] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]);
|
||||
SKK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]);
|
||||
SKK_LOS[10] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
SKK_LOS[11] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
SKK_LOS[12] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
SKK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]);
|
||||
SKK_LOS[14] = SH_LOS[0];
|
||||
|
||||
SK_LOS[0] = 1/(R_LOS + SKK_LOS[8]*(P[0][0]*SKK_LOS[8] + P[1][0]*SKK_LOS[7] + P[2][0]*SKK_LOS[9] - P[3][0]*SKK_LOS[6] - P[9][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][0]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][0]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[7]*(P[0][1]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] + P[2][1]*SKK_LOS[9] - P[3][1]*SKK_LOS[6] - P[9][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][1]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][1]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SKK_LOS[9]*(P[0][2]*SKK_LOS[8] + P[1][2]*SKK_LOS[7] + P[2][2]*SKK_LOS[9] - P[3][2]*SKK_LOS[6] - P[9][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][2]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][2]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SKK_LOS[6]*(P[0][3]*SKK_LOS[8] + P[1][3]*SKK_LOS[7] + P[2][3]*SKK_LOS[9] - P[3][3]*SKK_LOS[6] - P[9][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][3]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][3]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[8] + P[1][9]*SKK_LOS[7] + P[2][9]*SKK_LOS[9] - P[3][9]*SKK_LOS[6] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][9]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][9]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[2]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[8] + P[1][22]*SKK_LOS[7] + P[2][22]*SKK_LOS[9] - P[3][22]*SKK_LOS[6] - P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][22]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][22]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14]*(P[0][4]*SKK_LOS[8] + P[1][4]*SKK_LOS[7] + P[2][4]*SKK_LOS[9] - P[3][4]*SKK_LOS[6] - P[9][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][4]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14]*(P[0][5]*SKK_LOS[8] + P[1][5]*SKK_LOS[7] + P[2][5]*SKK_LOS[9] - P[3][5]*SKK_LOS[6] - P[9][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]*(P[0][6]*SKK_LOS[8] + P[1][6]*SKK_LOS[7] + P[2][6]*SKK_LOS[9] - P[3][6]*SKK_LOS[6] - P[9][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]));
|
||||
Kfusion[0] = -SK_LOS[0]*(P[0][0]*SKK_LOS[8] + P[0][1]*SKK_LOS[7] - P[0][3]*SKK_LOS[6] + P[0][2]*SKK_LOS[9] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[0][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[0][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[1] = -SK_LOS[0]*(P[1][0]*SKK_LOS[8] + P[1][1]*SKK_LOS[7] - P[1][3]*SKK_LOS[6] + P[1][2]*SKK_LOS[9] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[1][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[1][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[2] = -SK_LOS[0]*(P[2][0]*SKK_LOS[8] + P[2][1]*SKK_LOS[7] - P[2][3]*SKK_LOS[6] + P[2][2]*SKK_LOS[9] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[2][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[2][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[3] = -SK_LOS[0]*(P[3][0]*SKK_LOS[8] + P[3][1]*SKK_LOS[7] - P[3][3]*SKK_LOS[6] + P[3][2]*SKK_LOS[9] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[3][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[3][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[4] = -SK_LOS[0]*(P[4][0]*SKK_LOS[8] + P[4][1]*SKK_LOS[7] - P[4][3]*SKK_LOS[6] + P[4][2]*SKK_LOS[9] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[4][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[5] = -SK_LOS[0]*(P[5][0]*SKK_LOS[8] + P[5][1]*SKK_LOS[7] - P[5][3]*SKK_LOS[6] + P[5][2]*SKK_LOS[9] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[5][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[6] = -SK_LOS[0]*(P[6][0]*SKK_LOS[8] + P[6][1]*SKK_LOS[7] - P[6][3]*SKK_LOS[6] + P[6][2]*SKK_LOS[9] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[6][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[7] = -SK_LOS[0]*(P[7][0]*SKK_LOS[8] + P[7][1]*SKK_LOS[7] - P[7][3]*SKK_LOS[6] + P[7][2]*SKK_LOS[9] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[7][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[7][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[8] = -SK_LOS[0]*(P[8][0]*SKK_LOS[8] + P[8][1]*SKK_LOS[7] - P[8][3]*SKK_LOS[6] + P[8][2]*SKK_LOS[9] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[8][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[8][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[9] = -SK_LOS[0]*(P[9][0]*SKK_LOS[8] + P[9][1]*SKK_LOS[7] - P[9][3]*SKK_LOS[6] + P[9][2]*SKK_LOS[9] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[9][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[9][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[10] = -SK_LOS[0]*(P[10][0]*SKK_LOS[8] + P[10][1]*SKK_LOS[7] - P[10][3]*SKK_LOS[6] + P[10][2]*SKK_LOS[9] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[10][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[10][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[11] = -SK_LOS[0]*(P[11][0]*SKK_LOS[8] + P[11][1]*SKK_LOS[7] - P[11][3]*SKK_LOS[6] + P[11][2]*SKK_LOS[9] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[11][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[11][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[12] = -SK_LOS[0]*(P[12][0]*SKK_LOS[8] + P[12][1]*SKK_LOS[7] - P[12][3]*SKK_LOS[6] + P[12][2]*SKK_LOS[9] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[12][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[12][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//-SK_LOS[0]*(P[13][0]*SKK_LOS[8] + P[13][1]*SKK_LOS[7] - P[13][3]*SKK_LOS[6] + P[13][2]*SKK_LOS[9] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[13][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[13][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[14] = -SK_LOS[0]*(P[14][0]*SKK_LOS[8] + P[14][1]*SKK_LOS[7] - P[14][3]*SKK_LOS[6] + P[14][2]*SKK_LOS[9] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[14][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[14][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[15] = -SK_LOS[0]*(P[15][0]*SKK_LOS[8] + P[15][1]*SKK_LOS[7] - P[15][3]*SKK_LOS[6] + P[15][2]*SKK_LOS[9] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[15][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[15][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[16] = -SK_LOS[0]*(P[16][0]*SKK_LOS[8] + P[16][1]*SKK_LOS[7] - P[16][3]*SKK_LOS[6] + P[16][2]*SKK_LOS[9] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[16][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[16][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[17] = -SK_LOS[0]*(P[17][0]*SKK_LOS[8] + P[17][1]*SKK_LOS[7] - P[17][3]*SKK_LOS[6] + P[17][2]*SKK_LOS[9] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[17][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[17][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[18] = -SK_LOS[0]*(P[18][0]*SKK_LOS[8] + P[18][1]*SKK_LOS[7] - P[18][3]*SKK_LOS[6] + P[18][2]*SKK_LOS[9] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[18][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[18][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[19] = -SK_LOS[0]*(P[19][0]*SKK_LOS[8] + P[19][1]*SKK_LOS[7] - P[19][3]*SKK_LOS[6] + P[19][2]*SKK_LOS[9] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[19][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[19][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[20] = -SK_LOS[0]*(P[20][0]*SKK_LOS[8] + P[20][1]*SKK_LOS[7] - P[20][3]*SKK_LOS[6] + P[20][2]*SKK_LOS[9] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[20][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[20][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[21] = -SK_LOS[0]*(P[21][0]*SKK_LOS[8] + P[21][1]*SKK_LOS[7] - P[21][3]*SKK_LOS[6] + P[21][2]*SKK_LOS[9] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[21][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[21][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
Kfusion[22] = -SK_LOS[0]*(P[22][0]*SKK_LOS[8] + P[22][1]*SKK_LOS[7] - P[22][3]*SKK_LOS[6] + P[22][2]*SKK_LOS[9] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[5]*SKK_LOS[14] + P[22][5]*SH_LOS[3]*SKK_LOS[4]*SKK_LOS[14] - P[22][6]*SH_LOS[3]*SKK_LOS[3]*SKK_LOS[14]);
|
||||
varInnovOptFlow[0] = 1.0f/SK_LOS[0];
|
||||
innovOptFlow[0] = losPred[0] - losData[0];
|
||||
|
||||
// reset the observation index to 0 (we start by fusing the X
|
||||
// measurement)
|
||||
obsIndex = 0;
|
||||
fuseOptFlowData = false;
|
||||
}
|
||||
else if (obsIndex == 1) // we are now fusing the Y measurement
|
||||
{
|
||||
// Calculate observation jacobians
|
||||
for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0;
|
||||
H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]);
|
||||
H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]);
|
||||
H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]);
|
||||
H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1);
|
||||
H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3));
|
||||
H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3));
|
||||
H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3));
|
||||
H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7];
|
||||
|
||||
// Calculate Kalman gains
|
||||
SK_LOS[1] = 1/(R_LOS + SKK_LOS[12]*(P[0][2]*SKK_LOS[10] + P[1][2]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[3][2]*SKK_LOS[13] - P[9][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][2]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][2]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][2]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][2]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SKK_LOS[13]*(P[0][3]*SKK_LOS[10] + P[1][3]*SKK_LOS[11] + P[2][3]*SKK_LOS[12] - P[3][3]*SKK_LOS[13] - P[9][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][3]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][3]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][3]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][3]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[10]*(P[0][0]*SKK_LOS[10] + P[1][0]*SKK_LOS[11] + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][0]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][0]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][0]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][0]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SKK_LOS[11]*(P[0][1]*SKK_LOS[10] + P[1][1]*SKK_LOS[11] + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[3]*SKK_LOS[14]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[3]*SKK_LOS[14]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][1]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][1]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][1]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][1]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][9]*SKK_LOS[10] + P[1][9]*SKK_LOS[11] + P[2][9]*SKK_LOS[12] - P[3][9]*SKK_LOS[13] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][9]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][9]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][9]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[1]*SH_LOS[7]*SKK_LOS[14]*(P[0][22]*SKK_LOS[10] + P[1][22]*SKK_LOS[11] + P[2][22]*SKK_LOS[12] - P[3][22]*SKK_LOS[13] - P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][22]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][22]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14]*(P[0][4]*SKK_LOS[10] + P[1][4]*SKK_LOS[11] + P[2][4]*SKK_LOS[12] - P[3][4]*SKK_LOS[13] - P[9][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][4]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][4]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) - SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]*(P[0][5]*SKK_LOS[10] + P[1][5]*SKK_LOS[11] + P[2][5]*SKK_LOS[12] - P[3][5]*SKK_LOS[13] - P[9][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][5]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][5]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][5]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]) + SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]*(P[0][6]*SKK_LOS[10] + P[1][6]*SKK_LOS[11] + P[2][6]*SKK_LOS[12] - P[3][6]*SKK_LOS[13] - P[9][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][6]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14]));
|
||||
Kfusion[0] = SK_LOS[1]*(P[0][0]*SKK_LOS[10] - P[0][3]*SKK_LOS[13] + P[0][1]*SKK_LOS[11] + P[0][2]*SKK_LOS[12] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[0][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[0][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[0][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[1] = SK_LOS[1]*(P[1][0]*SKK_LOS[10] - P[1][3]*SKK_LOS[13] + P[1][1]*SKK_LOS[11] + P[1][2]*SKK_LOS[12] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[1][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[1][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[1][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[2] = SK_LOS[1]*(P[2][0]*SKK_LOS[10] - P[2][3]*SKK_LOS[13] + P[2][1]*SKK_LOS[11] + P[2][2]*SKK_LOS[12] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[2][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[2][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[2][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[3] = SK_LOS[1]*(P[3][0]*SKK_LOS[10] - P[3][3]*SKK_LOS[13] + P[3][1]*SKK_LOS[11] + P[3][2]*SKK_LOS[12] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[3][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[3][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[3][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[4] = SK_LOS[1]*(P[4][0]*SKK_LOS[10] - P[4][3]*SKK_LOS[13] + P[4][1]*SKK_LOS[11] + P[4][2]*SKK_LOS[12] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[4][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[4][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[4][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[5] = SK_LOS[1]*(P[5][0]*SKK_LOS[10] - P[5][3]*SKK_LOS[13] + P[5][1]*SKK_LOS[11] + P[5][2]*SKK_LOS[12] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[5][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[5][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[5][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[6] = SK_LOS[1]*(P[6][0]*SKK_LOS[10] - P[6][3]*SKK_LOS[13] + P[6][1]*SKK_LOS[11] + P[6][2]*SKK_LOS[12] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[6][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[6][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[6][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[7] = SK_LOS[1]*(P[7][0]*SKK_LOS[10] - P[7][3]*SKK_LOS[13] + P[7][1]*SKK_LOS[11] + P[7][2]*SKK_LOS[12] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[7][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[7][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[7][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[8] = SK_LOS[1]*(P[8][0]*SKK_LOS[10] - P[8][3]*SKK_LOS[13] + P[8][1]*SKK_LOS[11] + P[8][2]*SKK_LOS[12] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[8][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[8][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[8][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[9] = SK_LOS[1]*(P[9][0]*SKK_LOS[10] - P[9][3]*SKK_LOS[13] + P[9][1]*SKK_LOS[11] + P[9][2]*SKK_LOS[12] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[9][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[9][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[9][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[10] = SK_LOS[1]*(P[10][0]*SKK_LOS[10] - P[10][3]*SKK_LOS[13] + P[10][1]*SKK_LOS[11] + P[10][2]*SKK_LOS[12] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[10][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[10][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[10][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[11] = SK_LOS[1]*(P[11][0]*SKK_LOS[10] - P[11][3]*SKK_LOS[13] + P[11][1]*SKK_LOS[11] + P[11][2]*SKK_LOS[12] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[11][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[11][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[11][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[12] = SK_LOS[1]*(P[12][0]*SKK_LOS[10] - P[12][3]*SKK_LOS[13] + P[12][1]*SKK_LOS[11] + P[12][2]*SKK_LOS[12] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[12][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[12][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[12][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[13] = 0.0f;//SK_LOS[1]*(P[13][0]*SKK_LOS[10] - P[13][3]*SKK_LOS[13] + P[13][1]*SKK_LOS[11] + P[13][2]*SKK_LOS[12] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[13][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[13][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[13][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[14] = SK_LOS[1]*(P[14][0]*SKK_LOS[10] - P[14][3]*SKK_LOS[13] + P[14][1]*SKK_LOS[11] + P[14][2]*SKK_LOS[12] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[14][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[14][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[14][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[15] = SK_LOS[1]*(P[15][0]*SKK_LOS[10] - P[15][3]*SKK_LOS[13] + P[15][1]*SKK_LOS[11] + P[15][2]*SKK_LOS[12] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[15][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[15][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[15][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[16] = SK_LOS[1]*(P[16][0]*SKK_LOS[10] - P[16][3]*SKK_LOS[13] + P[16][1]*SKK_LOS[11] + P[16][2]*SKK_LOS[12] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[16][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[16][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[16][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[17] = SK_LOS[1]*(P[17][0]*SKK_LOS[10] - P[17][3]*SKK_LOS[13] + P[17][1]*SKK_LOS[11] + P[17][2]*SKK_LOS[12] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[17][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[17][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[17][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[18] = SK_LOS[1]*(P[18][0]*SKK_LOS[10] - P[18][3]*SKK_LOS[13] + P[18][1]*SKK_LOS[11] + P[18][2]*SKK_LOS[12] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[18][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[18][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[18][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[19] = SK_LOS[1]*(P[19][0]*SKK_LOS[10] - P[19][3]*SKK_LOS[13] + P[19][1]*SKK_LOS[11] + P[19][2]*SKK_LOS[12] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[19][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[19][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[19][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[20] = SK_LOS[1]*(P[20][0]*SKK_LOS[10] - P[20][3]*SKK_LOS[13] + P[20][1]*SKK_LOS[11] + P[20][2]*SKK_LOS[12] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[20][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[20][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[20][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[21] = SK_LOS[1]*(P[21][0]*SKK_LOS[10] - P[21][3]*SKK_LOS[13] + P[21][1]*SKK_LOS[11] + P[21][2]*SKK_LOS[12] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[21][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[21][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[21][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
Kfusion[22] = SK_LOS[1]*(P[22][0]*SKK_LOS[10] - P[22][3]*SKK_LOS[13] + P[22][1]*SKK_LOS[11] + P[22][2]*SKK_LOS[12] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SKK_LOS[14] + P[22][6]*SH_LOS[3]*SKK_LOS[0]*SKK_LOS[14] + P[22][4]*SH_LOS[3]*SKK_LOS[2]*SKK_LOS[14] - P[22][5]*SH_LOS[3]*SKK_LOS[1]*SKK_LOS[14]);
|
||||
varInnovOptFlow[1] = 1.0f/SK_LOS[1];
|
||||
innovOptFlow[1] = losPred[1] - losData[1];
|
||||
}
|
||||
|
||||
// Check the innovation for consistency and don't fuse if > 3Sigma
|
||||
if ((innovOptFlow[obsIndex]*innovOptFlow[obsIndex]/varInnovOptFlow[obsIndex]) < 9.0f)
|
||||
{
|
||||
// correct the state vector
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
|
||||
}
|
||||
// normalise the quaternion states
|
||||
float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]);
|
||||
if (quatMag > 1e-12)
|
||||
{
|
||||
for (uint8_t j= 0; j<=3; j++)
|
||||
{
|
||||
float quatMagInv = 1.0f/quatMag;
|
||||
states[j] = states[j] * quatMagInv;
|
||||
}
|
||||
}
|
||||
// correct the covariance P = (I - K*H)*P
|
||||
// take advantage of the empty columns in KH to reduce the
|
||||
// number of operations
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j <= 6; j++)
|
||||
{
|
||||
KH[i][j] = Kfusion[i] * H_LOS[j];
|
||||
}
|
||||
for (uint8_t j = 7; j <= 8; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][9] = Kfusion[i] * H_LOS[9];
|
||||
for (uint8_t j = 10; j <= 21; j++)
|
||||
{
|
||||
KH[i][j] = 0.0f;
|
||||
}
|
||||
KH[i][22] = Kfusion[i] * H_LOS[22];
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
KHP[i][j] = 0.0f;
|
||||
for (uint8_t k = 0; k <= 6; k++)
|
||||
{
|
||||
KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j];
|
||||
}
|
||||
KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j];
|
||||
KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
for (uint8_t i = 0; i < n_states; i++)
|
||||
{
|
||||
for (uint8_t j = 0; j < n_states; j++)
|
||||
{
|
||||
P[i][j] = P[i][j] - KHP[i][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
obsIndex = obsIndex + 1;
|
||||
ForceSymmetry();
|
||||
ConstrainVariances();
|
||||
}
|
||||
|
||||
void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last)
|
||||
{
|
||||
uint8_t row;
|
||||
@@ -1904,6 +2214,24 @@ void AttPosEKF::OnGroundCheck()
|
||||
if (staticMode) {
|
||||
staticMode = (!refSet || (GPSstatus < GPS_FIX_3D));
|
||||
}
|
||||
// don't update wind states if there is no airspeed measurement
|
||||
if (onGround || !useAirspeed) {
|
||||
inhibitWindStates = true;
|
||||
} else {
|
||||
inhibitWindStates =false;
|
||||
}
|
||||
// don't update magnetic field states if on ground or not using compass
|
||||
if (onGround || !useCompass) {
|
||||
inhibitMagStates = true;
|
||||
} else {
|
||||
inhibitMagStates = false;
|
||||
}
|
||||
// don't update terrain offset state if on ground
|
||||
if (onGround) {
|
||||
inhibitGndHgtState = true;
|
||||
} else {
|
||||
inhibitGndHgtState = false;
|
||||
}
|
||||
}
|
||||
|
||||
void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude)
|
||||
@@ -1931,8 +2259,8 @@ void AttPosEKF::CovarianceInit()
|
||||
P[11][11] = P[10][10];
|
||||
P[12][12] = P[10][10];
|
||||
P[13][13] = sq(0.2f*dtIMU);
|
||||
P[14][14] = sq(8.0f);
|
||||
P[15][14] = P[14][14];
|
||||
P[14][14] = sq(0.0f);
|
||||
P[15][15] = P[14][14];
|
||||
P[16][16] = sq(0.02f);
|
||||
P[17][17] = P[16][16];
|
||||
P[18][18] = P[16][16];
|
||||
|
||||
@@ -29,6 +29,10 @@ public:
|
||||
float covDelAngMax; // maximum delta angle between covariance predictions
|
||||
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
|
||||
float a1; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
float a2; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
float a3; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
float yawVarScale;
|
||||
float windVelSigma;
|
||||
float dAngBiasSigma;
|
||||
@@ -55,6 +59,9 @@ public:
|
||||
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
|
||||
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
|
||||
EAS2TAS = 1.0f;
|
||||
a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
|
||||
a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
|
||||
a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
|
||||
|
||||
yawVarScale = 1.0f;
|
||||
windVelSigma = 0.1f;
|
||||
@@ -115,6 +122,7 @@ public:
|
||||
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
|
||||
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtRngTime[n_states]; // filter states at the effective measurement time
|
||||
float statesAtOptFlowTime[n_states]; // States at the effective optical flow measurement time
|
||||
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
|
||||
@@ -147,9 +155,13 @@ public:
|
||||
float innovMag[3]; // innovation output
|
||||
float varInnovMag[3]; // innovation variance output
|
||||
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
|
||||
float losData[2]; // optical flow LOS rate measurements (rad/sec)
|
||||
float innovVtas; // innovation output
|
||||
float innovRng; ///< Range finder innovation
|
||||
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
|
||||
float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
|
||||
float varInnovVtas; // innovation variance output
|
||||
float varInnovRng; // range finder innovation variance
|
||||
float VtasMeas; // true airspeed measurement (m/s)
|
||||
float magDeclination; ///< magnetic declination
|
||||
double latRef; // WGS-84 latitude of reference point (rad)
|
||||
@@ -178,12 +190,18 @@ public:
|
||||
bool fuseMagData; // boolean true when magnetometer data is to be fused
|
||||
bool fuseVtasData; // boolean true when airspeed data is to be fused
|
||||
bool fuseRngData; ///< true when range data is fused
|
||||
bool fuseOptFlowData; // true when optical flow data is fused
|
||||
|
||||
bool inhibitWindStates; // true when wind states and covariances are to remain constant
|
||||
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
|
||||
bool inhibitGndHgtState; // true when the terrain ground height offset state and covariances are to remain constant
|
||||
|
||||
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
|
||||
bool staticMode; ///< boolean true if no position feedback is fused
|
||||
bool useAirspeed; ///< boolean true if airspeed data is being used
|
||||
bool useCompass; ///< boolean true if magnetometer data is being used
|
||||
bool useRangeFinder; ///< true when rangefinder is being used
|
||||
bool useOpticalFlow; // true when optical flow data is being used
|
||||
|
||||
bool ekfDiverged;
|
||||
uint64_t lastReset;
|
||||
@@ -208,7 +226,7 @@ void FuseAirspeed();
|
||||
|
||||
void FuseRangeFinder();
|
||||
|
||||
void FuseOpticalFlow();
|
||||
void FuseOptFlow();
|
||||
|
||||
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
|
||||
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
|
||||
#ifdef EKF_DEBUG
|
||||
#include <stdio.h>
|
||||
#include <stdarg.h>
|
||||
|
||||
static void
|
||||
ekf_debug_print(const char *fmt, va_list args)
|
||||
@@ -101,6 +102,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn)
|
||||
return vecOut;
|
||||
}
|
||||
|
||||
// overload * operator to provide a matrix product
|
||||
Mat3f operator*( Mat3f matIn1, Mat3f matIn2)
|
||||
{
|
||||
Mat3f matOut;
|
||||
matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x;
|
||||
matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y;
|
||||
matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z;
|
||||
|
||||
matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x;
|
||||
matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y;
|
||||
matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z;
|
||||
|
||||
matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x;
|
||||
matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y;
|
||||
matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z;
|
||||
|
||||
return matOut;
|
||||
}
|
||||
|
||||
// overload % operator to provide a vector cross product
|
||||
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2)
|
||||
{
|
||||
|
||||
@@ -41,6 +41,7 @@ Vector3f operator*(float sclIn1, Vector3f vecIn1);
|
||||
Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator*( Mat3f matIn, Vector3f vecIn);
|
||||
Mat3f operator*( Mat3f matIn1, Mat3f matIn2);
|
||||
Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2);
|
||||
Vector3f operator*(Vector3f vecIn1, float sclIn1);
|
||||
|
||||
@@ -79,4 +80,4 @@ struct ekf_status_report {
|
||||
bool velOffsetExcessive;
|
||||
};
|
||||
|
||||
void ekf_debug(const char *fmt, ...);
|
||||
void ekf_debug(const char *fmt, ...);
|
||||
|
||||
@@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
_attitude_sp_pub(-1),
|
||||
_nav_capabilities_pub(-1),
|
||||
|
||||
_att(),
|
||||
_att_sp(),
|
||||
_nav_capabilities(),
|
||||
_manual(),
|
||||
_airspeed(),
|
||||
_control_mode(),
|
||||
_global_pos(),
|
||||
_pos_sp_triplet(),
|
||||
_sensor_combined(),
|
||||
_range_finder(),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
|
||||
|
||||
@@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
|
||||
_airspeed_valid(false),
|
||||
_groundspeed_undershoot(0.0f),
|
||||
_global_pos_valid(false),
|
||||
_att(),
|
||||
_att_sp(),
|
||||
_nav_capabilities(),
|
||||
_manual(),
|
||||
_airspeed(),
|
||||
_control_mode(),
|
||||
_global_pos(),
|
||||
_pos_sp_triplet(),
|
||||
_sensor_combined(),
|
||||
_mTecs(),
|
||||
_was_pos_control_mode(false),
|
||||
_range_finder()
|
||||
_was_pos_control_mode(false)
|
||||
{
|
||||
_nav_capabilities.turn_distance = 0.0f;
|
||||
|
||||
@@ -806,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
|
||||
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
|
||||
|
||||
// XXX re-visit
|
||||
float baro_altitude = _global_pos.alt;
|
||||
|
||||
/* filter speed and altitude for controller */
|
||||
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
|
||||
math::Vector<3> accel_earth = _R_nb * accel_body;
|
||||
|
||||
float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
|
||||
|
||||
@@ -944,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi
|
||||
float airspeed_land = 1.3f * _parameters.airspeed_min;
|
||||
float airspeed_approach = 1.3f * _parameters.airspeed_min;
|
||||
|
||||
/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
|
||||
float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
|
||||
/* Calculate altitude of last ordinary waypoint L */
|
||||
float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
|
||||
|
||||
float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
|
||||
@@ -1449,7 +1445,7 @@ FixedwingPositionControl::start()
|
||||
_control_task = task_spawn_cmd("fw_pos_control_l1",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 5,
|
||||
4048,
|
||||
3500,
|
||||
(main_t)&FixedwingPositionControl::task_main_trampoline,
|
||||
nullptr);
|
||||
|
||||
|
||||
@@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
|
||||
/* Apply overrride given by the limitOverride argument (this is used for limits which are not given by
|
||||
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
|
||||
* is running) */
|
||||
bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
||||
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
||||
|
||||
/* Write part of the status message */
|
||||
_status.flightPathAngleSp = flightPathAngleSp;
|
||||
|
||||
@@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock
|
||||
{
|
||||
public:
|
||||
// methods
|
||||
BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
|
||||
BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) :
|
||||
SuperBlock(parent, name),
|
||||
_isAngularLimit(isAngularLimit),
|
||||
_isAngularLimit(fAngularLimit),
|
||||
_min(this, "MIN"),
|
||||
_max(this, "MAX")
|
||||
{};
|
||||
|
||||
@@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req)
|
||||
MavlinkFTP::ErrorCode
|
||||
MavlinkFTP::_workRemove(Request *req)
|
||||
{
|
||||
auto hdr = req->header();
|
||||
//auto hdr = req->header();
|
||||
|
||||
// for now, send error reply
|
||||
return kErrPerm;
|
||||
|
||||
@@ -420,32 +420,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message
|
||||
void
|
||||
MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_radio_status_t rstatus;
|
||||
mavlink_msg_radio_status_decode(msg, &rstatus);
|
||||
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
|
||||
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
|
||||
mavlink_radio_status_t rstatus;
|
||||
mavlink_msg_radio_status_decode(msg, &rstatus);
|
||||
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
|
||||
tstatus.timestamp = hrt_absolute_time();
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
|
||||
tstatus.rssi = rstatus.rssi;
|
||||
tstatus.remote_rssi = rstatus.remrssi;
|
||||
tstatus.txbuf = rstatus.txbuf;
|
||||
tstatus.noise = rstatus.noise;
|
||||
tstatus.remote_noise = rstatus.remnoise;
|
||||
tstatus.rxerrors = rstatus.rxerrors;
|
||||
tstatus.fixed = rstatus.fixed;
|
||||
tstatus.timestamp = hrt_absolute_time();
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
|
||||
tstatus.rssi = rstatus.rssi;
|
||||
tstatus.remote_rssi = rstatus.remrssi;
|
||||
tstatus.txbuf = rstatus.txbuf;
|
||||
tstatus.noise = rstatus.noise;
|
||||
tstatus.remote_noise = rstatus.remnoise;
|
||||
tstatus.rxerrors = rstatus.rxerrors;
|
||||
tstatus.fixed = rstatus.fixed;
|
||||
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
|
||||
} else {
|
||||
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
|
||||
}
|
||||
|
||||
/* this means that heartbeats alone won't be published to the radio status no more */
|
||||
_radio_status_available = true;
|
||||
}
|
||||
|
||||
/* this means that heartbeats alone won't be published to the radio status no more */
|
||||
_radio_status_available = true;
|
||||
}
|
||||
|
||||
void
|
||||
@@ -474,28 +477,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
|
||||
void
|
||||
MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
|
||||
{
|
||||
mavlink_heartbeat_t hb;
|
||||
mavlink_msg_heartbeat_decode(msg, &hb);
|
||||
/* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */
|
||||
if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) {
|
||||
mavlink_heartbeat_t hb;
|
||||
mavlink_msg_heartbeat_decode(msg, &hb);
|
||||
|
||||
/* ignore own heartbeats, accept only heartbeats from GCS */
|
||||
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
|
||||
_telemetry_heartbeat_time = hrt_absolute_time();
|
||||
/* ignore own heartbeats, accept only heartbeats from GCS */
|
||||
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
|
||||
_telemetry_heartbeat_time = hrt_absolute_time();
|
||||
|
||||
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
|
||||
if (!_radio_status_available) {
|
||||
/* if no radio status messages arrive, lets at least publish that heartbeats were received */
|
||||
if (!_radio_status_available) {
|
||||
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
struct telemetry_status_s tstatus;
|
||||
memset(&tstatus, 0, sizeof(tstatus));
|
||||
|
||||
tstatus.timestamp = _telemetry_heartbeat_time;
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
|
||||
tstatus.timestamp = _telemetry_heartbeat_time;
|
||||
tstatus.heartbeat_time = _telemetry_heartbeat_time;
|
||||
tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
|
||||
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
|
||||
if (_telemetry_status_pub < 0) {
|
||||
_telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus);
|
||||
|
||||
} else {
|
||||
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
|
||||
} else {
|
||||
orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -64,12 +64,15 @@
|
||||
#define rCCR REG(STM32_I2C_CCR_OFFSET)
|
||||
#define rTRISE REG(STM32_I2C_TRISE_OFFSET)
|
||||
|
||||
void i2c_reset(void);
|
||||
static int i2c_interrupt(int irq, void *context);
|
||||
static void i2c_rx_setup(void);
|
||||
static void i2c_tx_setup(void);
|
||||
static void i2c_rx_complete(void);
|
||||
static void i2c_tx_complete(void);
|
||||
#ifdef DEBUG
|
||||
static void i2c_dump(void);
|
||||
#endif
|
||||
|
||||
static DMA_HANDLE rx_dma;
|
||||
static DMA_HANDLE tx_dma;
|
||||
|
||||
@@ -219,8 +219,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values);
|
||||
extern void dsm_bind(uint16_t cmd, int pulses);
|
||||
extern int sbus_init(const char *device);
|
||||
extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
|
||||
extern bool sbus1_output(uint16_t *values, uint16_t num_values);
|
||||
extern bool sbus2_output(uint16_t *values, uint16_t num_values);
|
||||
extern void sbus1_output(uint16_t *values, uint16_t num_values);
|
||||
extern void sbus2_output(uint16_t *values, uint16_t num_values);
|
||||
|
||||
/** global debug level for isr_debug() */
|
||||
extern volatile uint8_t debug_level;
|
||||
|
||||
@@ -711,7 +711,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
|
||||
{
|
||||
#define SELECT_PAGE(_page_name) \
|
||||
do { \
|
||||
*values = &_page_name[0]; \
|
||||
*values = (uint16_t *)&_page_name[0]; \
|
||||
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
|
||||
} while(0)
|
||||
|
||||
|
||||
@@ -116,14 +116,14 @@ sbus_init(const char *device)
|
||||
return sbus_fd;
|
||||
}
|
||||
|
||||
bool
|
||||
void
|
||||
sbus1_output(uint16_t *values, uint16_t num_values)
|
||||
{
|
||||
char a = 'A';
|
||||
write(sbus_fd, &a, 1);
|
||||
}
|
||||
|
||||
bool
|
||||
void
|
||||
sbus2_output(uint16_t *values, uint16_t num_values)
|
||||
{
|
||||
char b = 'B';
|
||||
|
||||
+18
-13
@@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
struct log_GVSP_s log_GVSP;
|
||||
struct log_BATT_s log_BATT;
|
||||
struct log_DIST_s log_DIST;
|
||||
struct log_TELE_s log_TELE;
|
||||
struct log_TEL_s log_TEL;
|
||||
struct log_EST0_s log_EST0;
|
||||
struct log_EST1_s log_EST1;
|
||||
struct log_PWR_s log_PWR;
|
||||
@@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
int esc_sub;
|
||||
int global_vel_sp_sub;
|
||||
int battery_sub;
|
||||
int telemetry_sub;
|
||||
int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM];
|
||||
int range_finder_sub;
|
||||
int estimator_status_sub;
|
||||
int tecs_status_sub;
|
||||
@@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
|
||||
subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
|
||||
subs.battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status));
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]);
|
||||
}
|
||||
subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
|
||||
subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status));
|
||||
subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
|
||||
@@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[])
|
||||
}
|
||||
|
||||
/* --- TELEMETRY --- */
|
||||
if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) {
|
||||
log_msg.msg_type = LOG_TELE_MSG;
|
||||
log_msg.body.log_TELE.rssi = buf.telemetry.rssi;
|
||||
log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi;
|
||||
log_msg.body.log_TELE.noise = buf.telemetry.noise;
|
||||
log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise;
|
||||
log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors;
|
||||
log_msg.body.log_TELE.fixed = buf.telemetry.fixed;
|
||||
log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf;
|
||||
LOGBUFFER_WRITE_AND_COUNT(TELE);
|
||||
for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) {
|
||||
if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) {
|
||||
log_msg.msg_type = LOG_TEL0_MSG + i;
|
||||
log_msg.body.log_TEL.rssi = buf.telemetry.rssi;
|
||||
log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi;
|
||||
log_msg.body.log_TEL.noise = buf.telemetry.noise;
|
||||
log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise;
|
||||
log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors;
|
||||
log_msg.body.log_TEL.fixed = buf.telemetry.fixed;
|
||||
log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf;
|
||||
log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time;
|
||||
LOGBUFFER_WRITE_AND_COUNT(TEL);
|
||||
}
|
||||
}
|
||||
|
||||
/* --- BOTTOM DISTANCE --- */
|
||||
|
||||
@@ -91,6 +91,14 @@ struct log_format_s {
|
||||
.labels = _labels \
|
||||
}
|
||||
|
||||
#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \
|
||||
.type = LOG_##_name##_MSG, \
|
||||
.length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \
|
||||
.name = #_name, \
|
||||
.format = _format, \
|
||||
.labels = _labels \
|
||||
}
|
||||
|
||||
#define LOG_FORMAT_MSG 0x80
|
||||
|
||||
#define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s)
|
||||
|
||||
@@ -276,18 +276,7 @@ struct log_DIST_s {
|
||||
uint8_t flags;
|
||||
};
|
||||
|
||||
/* --- TELE - TELEMETRY STATUS --- */
|
||||
#define LOG_TELE_MSG 22
|
||||
struct log_TELE_s {
|
||||
uint8_t rssi;
|
||||
uint8_t remote_rssi;
|
||||
uint8_t noise;
|
||||
uint8_t remote_noise;
|
||||
uint16_t rxerrors;
|
||||
uint16_t fixed;
|
||||
uint8_t txbuf;
|
||||
};
|
||||
|
||||
// ID 22 available
|
||||
// ID 23 available
|
||||
|
||||
/* --- PWR - ONBOARD POWER SYSTEM --- */
|
||||
@@ -385,6 +374,23 @@ struct log_EST1_s {
|
||||
float s[16];
|
||||
};
|
||||
|
||||
/* --- TEL0..3 - TELEMETRY STATUS --- */
|
||||
#define LOG_TEL0_MSG 34
|
||||
#define LOG_TEL1_MSG 35
|
||||
#define LOG_TEL2_MSG 36
|
||||
#define LOG_TEL3_MSG 37
|
||||
struct log_TEL_s {
|
||||
uint8_t rssi;
|
||||
uint8_t remote_rssi;
|
||||
uint8_t noise;
|
||||
uint8_t remote_noise;
|
||||
uint16_t rxerrors;
|
||||
uint16_t fixed;
|
||||
uint8_t txbuf;
|
||||
uint64_t heartbeat_time;
|
||||
};
|
||||
|
||||
|
||||
/********** SYSTEM MESSAGES, ID > 0x80 **********/
|
||||
|
||||
/* --- TIME - TIME STAMP --- */
|
||||
@@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = {
|
||||
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
|
||||
LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
|
||||
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
|
||||
LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
|
||||
LOG_FORMAT_S(TEL0, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT_S(TEL1, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT_S(TEL2, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"),
|
||||
LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"),
|
||||
LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"),
|
||||
LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
|
||||
|
||||
@@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT
|
||||
|
||||
__EXPORT struct system_load_s system_load;
|
||||
|
||||
extern FAR struct _TCB *sched_gettcb(pid_t pid);
|
||||
extern FAR struct tcb_s *sched_gettcb(pid_t pid);
|
||||
|
||||
void cpuload_initialize_once()
|
||||
{
|
||||
|
||||
@@ -54,6 +54,9 @@
|
||||
|
||||
#include "systemlib.h"
|
||||
|
||||
// Didn't seem right to include up_internal.h, so direct extern instead.
|
||||
extern void up_systemreset(void) noreturn_function;
|
||||
|
||||
void
|
||||
systemreset(bool to_bootloader)
|
||||
{
|
||||
|
||||
@@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
|
||||
ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
|
||||
|
||||
#include "topics/telemetry_status.h"
|
||||
ORB_DEFINE(telemetry_status, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_0, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_1, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_2, struct telemetry_status_s);
|
||||
ORB_DEFINE(telemetry_status_3, struct telemetry_status_s);
|
||||
|
||||
#include "topics/debug_key_value.h"
|
||||
ORB_DEFINE(debug_key_value, struct debug_key_value_s);
|
||||
|
||||
@@ -72,6 +72,18 @@ struct telemetry_status_s {
|
||||
* @}
|
||||
*/
|
||||
|
||||
ORB_DECLARE(telemetry_status);
|
||||
ORB_DECLARE(telemetry_status_0);
|
||||
ORB_DECLARE(telemetry_status_1);
|
||||
ORB_DECLARE(telemetry_status_2);
|
||||
ORB_DECLARE(telemetry_status_3);
|
||||
|
||||
#define TELEMETRY_STATUS_ORB_ID_NUM 4
|
||||
|
||||
static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = {
|
||||
ORB_ID(telemetry_status_0),
|
||||
ORB_ID(telemetry_status_1),
|
||||
ORB_ID(telemetry_status_2),
|
||||
ORB_ID(telemetry_status_3),
|
||||
};
|
||||
|
||||
#endif /* TOPIC_TELEMETRY_STATUS_H */
|
||||
|
||||
@@ -46,6 +46,7 @@
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <math.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
@@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param)
|
||||
if (!(arg == NULL)) {
|
||||
|
||||
/* start search */
|
||||
char *ss = search_string;
|
||||
char *pp = p_name;
|
||||
const char *ss = search_string;
|
||||
const char *pp = p_name;
|
||||
bool mismatch = false;
|
||||
|
||||
/* XXX this comparison is only ok for trailing wildcards */
|
||||
|
||||
@@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[])
|
||||
TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
|
||||
TEST_OP("Vector<3> length", v1.length());
|
||||
TEST_OP("Vector<3> length squared", v1.length_squared());
|
||||
#pragma GCC diagnostic push
|
||||
#pragma GCC diagnostic ignored "-Wunused-variable"
|
||||
TEST_OP("Vector<3> element read", volatile float a = v1(0));
|
||||
TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
|
||||
#pragma GCC diagnostic pop
|
||||
TEST_OP("Vector<3> element write", v1(0) = 1.0f);
|
||||
TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user