void
MulticopterPositionControl::update_ref()
{
if (_local_pos.ref_timestamp != _ref_timestamp) {
double lat_sp, lon_sp;
float alt_sp;
if (_ref_timestamp != 0) {
/* calculate current position setpoint in global frame */
map_projection_reproject(&_ref_pos, _pos_sp(0), _pos_sp(1), &lat_sp, &lon_sp);
alt_sp = _ref_alt - _pos_sp(2);
}
/* update local projection reference */
map_projection_init(&_ref_pos, _local_pos.ref_lat, _local_pos.ref_lon);
_ref_alt = _local_pos.ref_alt;
if (_ref_timestamp != 0) {
/* reproject position setpoint to new reference */
map_projection_project(&_ref_pos, lat_sp, lon_sp, &_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(alt_sp - _ref_alt);
}
_ref_timestamp = _local_pos.ref_timestamp;
}
}
float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
const vehicle_global_position_s &global_position)
{
if (!map_projection_initialized(&_projection_reference)) {
map_projection_init(&_projection_reference, global_position.lat, global_position.lon);
}
float x1, y1, x2, y2;
map_projection_project(&_projection_reference, lat, lon, &x1, &y1);
map_projection_project(&_projection_reference, global_position.lat, global_position.lon, &x2, &y2);
float dx = x1 - x2, dy = y1 - y2;
float target_distance = sqrtf(dx * dx + dy * dy);
float z = altitude - global_position.alt;
return atan2f(z, target_distance);
}
void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
{
if (!_initialised) {
return;
}
// limit data rate to prevent data being lost
bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps) {
gpsSample gps_sample_new = {};
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
_time_last_gps = time_usec;
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
_gps_speed_valid = gps->vel_ned_valid;
gps_sample_new.sacc = gps->sacc;
gps_sample_new.hacc = gps->eph;
gps_sample_new.vacc = gps->epv;
gps_sample_new.hgt = (float)gps->alt * 1e-3f;
// Only calculate the relative position if the WGS-84 location of the origin is set
if (collect_gps(time_usec, gps)) {
float lpos_x = 0.0f;
float lpos_y = 0.0f;
map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
gps_sample_new.pos(0) = lpos_x;
gps_sample_new.pos(1) = lpos_y;
} else {
gps_sample_new.pos(0) = 0.0f;
gps_sample_new.pos(1) = 0.0f;
}
_gps_buffer.push(gps_sample_new);
}
}
void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps)
{
if (!_gps_initialised) {
initialiseGPS(gps);
return;
}
if (time_usec - _time_last_gps > 70000 && gps_is_good(gps)) {
gpsSample gps_sample_new = {};
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERRIOD_MS * 1000 / 2;
_time_last_gps = time_usec;
_last_valid_gps_time_us = hrt_absolute_time();
gps_sample_new.time_us = math::max(gps_sample_new.time_us, _imu_sample_delayed.time_us);
memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data));
_gps_speed_valid = gps->vel_ned_valid;
float lpos_x = 0.0f;
float lpos_y = 0.0f;
map_projection_project(&_posRef, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y);
gps_sample_new.pos(0) = lpos_x;
gps_sample_new.pos(1) = lpos_y;
gps_sample_new.hgt = gps->alt / 1e3f;
_gps_buffer.push(gps_sample_new);
}
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
int mavlink_fd;
mavlink_fd = px4_open(MAVLINK_LOG_DEVICE, 0);
float x_est[2] = { 0.0f, 0.0f }; // pos, vel
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
float z_est[2] = { 0.0f, 0.0f }; // pos, vel
float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer
float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer
float R_gps[3][3]; // rotation matrix for GPS correction moment
memset(est_buf, 0, sizeof(est_buf));
memset(R_buf, 0, sizeof(R_buf));
memset(R_gps, 0, sizeof(R_gps));
int buf_ptr = 0;
static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation
float eph = max_eph_epv;
float epv = 1.0f;
float eph_flow = 1.0f;
float eph_vision = 0.2f;
float epv_vision = 0.2f;
float eph_mocap = 0.05f;
float epv_mocap = 0.05f;
float x_est_prev[2], y_est_prev[2], z_est_prev[2];
memset(x_est_prev, 0, sizeof(x_est_prev));
memset(y_est_prev, 0, sizeof(y_est_prev));
memset(z_est_prev, 0, sizeof(z_est_prev));
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
float surface_offset = 0.0f; // ground level offset from reference altitude
float surface_offset_rate = 0.0f; // surface offset change rate
hrt_abstime accel_timestamp = 0;
hrt_abstime baro_timestamp = 0;
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
struct map_projection_reference_s ref;
memset(&ref, 0, sizeof(ref));
hrt_abstime home_timestamp = 0;
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
uint16_t vision_updates = 0;
uint16_t mocap_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
hrt_abstime pub_last = hrt_absolute_time();
hrt_abstime t_prev = 0;
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float corr_baro = 0.0f; // D
float corr_gps[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_vision[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float corr_mocap[3][1] = {
{ 0.0f }, // N (pos)
{ 0.0f }, // E (pos)
{ 0.0f }, // D (pos)
};
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
float sonar_prev = 0.0f;
//hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered)
bool gps_valid = false; // GPS is valid
bool sonar_valid = false; // sonar is valid
bool flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
bool vision_valid = false; // vision is valid
bool mocap_valid = false; // mocap is valid
/* declare and safely initialize all structs */
struct actuator_controls_s actuator;
memset(&actuator, 0, sizeof(actuator));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct home_position_s home;
memset(&home, 0, sizeof(home));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
struct vision_position_estimate_s vision;
memset(&vision, 0, sizeof(vision));
struct att_pos_mocap_s mocap;
memset(&mocap, 0, sizeof(mocap));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
int home_position_sub = orb_subscribe(ORB_ID(home_position));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t vehicle_global_position_pub = NULL;
struct position_estimator_inav_params params;
struct position_estimator_inav_param_handles pos_inav_param_handles;
/* initialize parameter handles */
inav_parameters_init(&pos_inav_param_handles);
/* first parameters read at start up */
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */
/* first parameters update */
inav_parameters_update(&pos_inav_param_handles, ¶ms);
px4_pollfd_struct_t fds_init[1] = {
{ .fd = sensor_combined_sub, .events = POLLIN },
};
/* wait for initial baro value */
bool wait_baro = true;
thread_running = true;
while (wait_baro && !thread_should_exit) {
int ret = px4_poll(fds_init, 1, 1000);
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
} else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_timestamp != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp;
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
if (PX4_ISFINITE(sensor.baro_alt_meter)) {
baro_offset += sensor.baro_alt_meter;
baro_init_cnt++;
}
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
warnx("baro offset: %d m", (int)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offset: %d m", (int)baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
}
}
}
}
/* main loop */
px4_pollfd_struct_t fds[1] = {
{ .fd = vehicle_attitude_sub, .events = POLLIN },
};
while (!thread_should_exit) {
int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
/* poll error */
mavlink_log_info(mavlink_fd, "[inav] poll error on init");
continue;
} else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
inav_parameters_update(&pos_inav_param_handles, ¶ms);
}
/* actuator */
orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* sensor combined */
orb_check(sensor_combined_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_timestamp != accel_timestamp) {
if (att.R_valid) {
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
sensor.accelerometer_m_s2[2] -= acc_bias[2];
/* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) {
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
}
}
acc[2] += CONSTANTS_ONE_G;
} else {
memset(acc, 0, sizeof(acc));
}
accel_timestamp = sensor.accelerometer_timestamp;
accel_updates++;
}
if (sensor.baro_timestamp != baro_timestamp) {
corr_baro = baro_offset - sensor.baro_alt_meter - z_est[0];
baro_timestamp = sensor.baro_timestamp;
baro_updates++;
}
}
/* optical flow */
orb_check(optical_flow_sub, &updated);
if (updated) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
/* calculate time from previous update */
// float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
// flow_prev = flow.flow_timestamp;
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
(PX4_R(att.R, 2, 2) > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
if (fabsf(corr_sonar) > params.sonar_err) {
/* correction is too large: spike or new ground level? */
if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
/* spike detected, ignore */
corr_sonar = 0.0f;
sonar_valid = false;
} else {
/* new ground level */
surface_offset -= corr_sonar;
surface_offset_rate = 0.0f;
corr_sonar = 0.0f;
corr_sonar_filtered = 0.0f;
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
}
} else {
/* correction is ok, use it */
sonar_valid_time = t;
sonar_valid = true;
}
}
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
/* distance to surface */
float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
//todo check direction of x und y axis
flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt;
flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt;
/* flow measurements vector */
float flow_m[3];
flow_m[0] = -flow_ang[0] * flow_dist;
flow_m[1] = -flow_ang[1] * flow_dist;
flow_m[2] = z_est[1];
/* velocity in NED */
float flow_v[2] = { 0.0f, 0.0f };
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
}
}
/* velocity correction */
corr_flow[0] = flow_v[0] - x_est[1];
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate) {
w_flow *= 0.05f;
}
/* under ideal conditions, on 1m distance assume EPH = 10cm */
eph_flow = 0.1f / w_flow;
flow_valid = true;
} else {
w_flow = 0.0f;
flow_valid = false;
}
flow_updates++;
}
/* home position */
orb_check(home_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(home_position), home_position_sub, &home);
if (home.timestamp != home_timestamp) {
home_timestamp = home.timestamp;
double est_lat, est_lon;
float est_alt;
if (ref_inited) {
/* calculate current estimated position in global frame */
est_alt = local_pos.ref_alt - local_pos.z;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
}
/* update reference */
map_projection_init(&ref, home.lat, home.lon);
/* update baro offset */
baro_offset += home.alt - local_pos.ref_alt;
local_pos.ref_lat = home.lat;
local_pos.ref_lon = home.lon;
local_pos.ref_alt = home.alt;
local_pos.ref_timestamp = home.timestamp;
if (ref_inited) {
/* reproject position estimate with new reference */
map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
z_est[0] = -(est_alt - local_pos.ref_alt);
}
ref_inited = true;
}
}
/* check no vision circuit breaker is set */
if (params.no_vision != CBRK_NO_VISION_KEY) {
/* vehicle vision position */
orb_check(vision_position_estimate_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
static float last_vision_x = 0.0f;
static float last_vision_y = 0.0f;
static float last_vision_z = 0.0f;
/* reset position estimate on first vision update */
if (!vision_valid) {
x_est[0] = vision.x;
x_est[1] = vision.vx;
y_est[0] = vision.y;
y_est[1] = vision.vy;
/* only reset the z estimate if the z weight parameter is not zero */
if (params.w_z_vision_p > MIN_VALID_W)
{
z_est[0] = vision.z;
z_est[1] = vision.vz;
}
vision_valid = true;
last_vision_x = vision.x;
last_vision_y = vision.y;
last_vision_z = vision.z;
warnx("VISION estimate valid");
mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
}
/* calculate correction for position */
corr_vision[0][0] = vision.x - x_est[0];
corr_vision[1][0] = vision.y - y_est[0];
corr_vision[2][0] = vision.z - z_est[0];
static hrt_abstime last_vision_time = 0;
float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
last_vision_time = vision.timestamp_boot;
if (vision_dt > 0.000001f && vision_dt < 0.2f) {
vision.vx = (vision.x - last_vision_x) / vision_dt;
vision.vy = (vision.y - last_vision_y) / vision_dt;
vision.vz = (vision.z - last_vision_z) / vision_dt;
last_vision_x = vision.x;
last_vision_y = vision.y;
last_vision_z = vision.z;
/* calculate correction for velocity */
corr_vision[0][1] = vision.vx - x_est[1];
corr_vision[1][1] = vision.vy - y_est[1];
corr_vision[2][1] = vision.vz - z_est[1];
} else {
/* assume zero motion */
corr_vision[0][1] = 0.0f - x_est[1];
corr_vision[1][1] = 0.0f - y_est[1];
corr_vision[2][1] = 0.0f - z_est[1];
}
vision_updates++;
}
}
/* vehicle mocap position */
orb_check(att_pos_mocap_sub, &updated);
if (updated) {
orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
/* reset position estimate on first mocap update */
if (!mocap_valid) {
x_est[0] = mocap.x;
y_est[0] = mocap.y;
z_est[0] = mocap.z;
mocap_valid = true;
warnx("MOCAP data valid");
mavlink_log_info(mavlink_fd, "[inav] MOCAP data valid");
}
/* calculate correction for position */
corr_mocap[0][0] = mocap.x - x_est[0];
corr_mocap[1][0] = mocap.y - y_est[0];
corr_mocap[2][0] = mocap.z - z_est[0];
mocap_updates++;
}
/* vehicle GPS position */
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
bool reset_est = false;
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
}
if (gps_valid) {
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
/* initialize reference position if needed */
if (!ref_inited) {
if (ref_init_start == 0) {
ref_init_start = t;
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s;
y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s;
local_pos.ref_lat = lat;
local_pos.ref_lon = lon;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(&ref, lat, lon);
// XXX replace this print
warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
}
}
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
/* reset position estimate when GPS becomes good */
if (reset_est) {
x_est[0] = gps_proj[0];
x_est[1] = gps.vel_n_m_s;
y_est[0] = gps_proj[1];
y_est[1] = gps.vel_e_m_s;
}
/* calculate index of estimated values in buffer */
int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
if (est_i < 0) {
est_i += EST_BUF_SIZE;
}
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
} else {
corr_gps[0][1] = 0.0f;
corr_gps[1][1] = 0.0f;
corr_gps[2][1] = 0.0f;
}
/* save rotation matrix at this moment */
memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
}
} else {
/* no GPS lock */
memset(corr_gps, 0, sizeof(corr_gps));
ref_init_start = 0;
}
gps_updates++;
}
}
/* check for timeout on FLOW topic */
if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) {
flow_valid = false;
sonar_valid = false;
warnx("FLOW timeout");
mavlink_log_info(mavlink_fd, "[inav] FLOW timeout");
}
/* check for timeout on GPS topic */
if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(mavlink_fd, "[inav] GPS timeout");
}
/* check for timeout on vision topic */
if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
vision_valid = false;
warnx("VISION timeout");
mavlink_log_info(mavlink_fd, "[inav] VISION timeout");
}
/* check for timeout on mocap topic */
if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
mocap_valid = false;
warnx("MOCAP timeout");
mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout");
}
/* check for sonar measurement timeout */
if (sonar_valid && (t > (sonar_time + sonar_timeout))) {
corr_sonar = 0.0f;
sonar_valid = false;
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms
t_prev = t;
/* increase EPH/EPV on each step */
if (eph < max_eph_epv) {
eph *= 1.0f + dt;
}
if (epv < max_eph_epv) {
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
/* use GPS if it's valid and reference position initialized */
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
/* use VISION if it's valid and has a valid weight parameter */
bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
/* use MOCAP if it's valid and has a valid weight parameter */
bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W;
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
if (dist_bottom_valid) {
/* surface distance prediction */
surface_offset += surface_offset_rate * dt;
/* surface distance correction */
if (sonar_valid) {
surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
surface_offset -= corr_sonar * params.w_z_sonar * dt;
}
}
float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
float w_z_gps_v = params.w_z_gps_v * w_gps_z;
float w_xy_vision_p = params.w_xy_vision_p;
float w_xy_vision_v = params.w_xy_vision_v;
float w_z_vision_p = params.w_z_vision_p;
float w_mocap_p = params.w_mocap_p;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_xy_gps_p *= params.w_gps_flow;
w_xy_gps_v *= params.w_gps_flow;
}
/* baro offset correction */
if (use_gps_z) {
float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
baro_offset += offs_corr;
corr_baro += offs_corr;
}
/* accelerometer bias correction for GPS (use buffered rotation matrix) */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps_xy) {
accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
}
if (use_gps_z) {
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
}
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += R_gps[j][i] * accel_bias_corr[j];
}
if (isfinite(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* accelerometer bias correction for VISION (use buffered rotation matrix) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_vision_xy) {
accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
}
if (use_vision_z) {
accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
}
/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_mocap) {
accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
}
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* accelerometer bias correction for flow and baro (assume that there is no delay) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_flow) {
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
}
accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est, acc[2]);
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
}
/* inertial filter correction for altitude */
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
if (use_gps_z) {
epv = fminf(epv, gps.epv);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
}
if (use_vision_z) {
epv = fminf(epv, epv_vision);
inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
}
if (use_mocap) {
epv = fminf(epv, epv_mocap);
inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
}
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
memset(corr_mocap, 0, sizeof(corr_mocap));
corr_baro = 0;
} else {
memcpy(z_est_prev, z_est, sizeof(z_est));
}
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est, acc[0]);
inertial_filter_predict(dt, y_est, acc[1]);
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
}
/* inertial filter correction for position */
if (use_flow) {
eph = fminf(eph, eph_flow);
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
}
if (use_gps_xy) {
eph = fminf(eph, gps.eph);
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
}
}
if (use_vision_xy) {
eph = fminf(eph, eph_vision);
inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);
if (w_xy_vision_v > MIN_VALID_W) {
inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
}
}
if (use_mocap) {
eph = fminf(eph, eph_mocap);
inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);
inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
}
if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
memset(corr_mocap, 0, sizeof(corr_mocap));
memset(corr_flow, 0, sizeof(corr_flow));
} else {
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
}
} else {
/* gradually reset xy velocity estimates */
inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
}
if (inav_verbose_mode) {
/* print updates rate */
if (t > updates_counter_start + updates_counter_len) {
float updates_dt = (t - updates_counter_start) * 0.000001f;
warnx(
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",
(double)(accel_updates / updates_dt),
(double)(baro_updates / updates_dt),
(double)(gps_updates / updates_dt),
(double)(attitude_updates / updates_dt),
(double)(flow_updates / updates_dt),
(double)(vision_updates / updates_dt),
(double)(mocap_updates / updates_dt));
updates_counter_start = t;
accel_updates = 0;
baro_updates = 0;
gps_updates = 0;
attitude_updates = 0;
flow_updates = 0;
vision_updates = 0;
mocap_updates = 0;
}
}
if (t > pub_last + PUB_INTERVAL) {
pub_last = t;
/* push current estimate to buffer */
est_buf[buf_ptr][0][0] = x_est[0];
est_buf[buf_ptr][0][1] = x_est[1];
est_buf[buf_ptr][1][0] = y_est[0];
est_buf[buf_ptr][1][1] = y_est[1];
est_buf[buf_ptr][2][0] = z_est[0];
est_buf[buf_ptr][2][1] = z_est[1];
/* push current rotation matrix to buffer */
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
buf_ptr++;
if (buf_ptr >= EST_BUF_SIZE) {
buf_ptr = 0;
}
/* publish local position */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;
local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
local_pos.z_global = local_pos.z_valid && use_gps_z;
local_pos.x = x_est[0];
local_pos.vx = x_est[1];
local_pos.y = y_est[0];
local_pos.vy = y_est[1];
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.yaw = att.yaw;
local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph;
local_pos.epv = epv;
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = -z_est[0] - surface_offset;
local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate;
}
local_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
if (local_pos.xy_global && local_pos.z_global) {
/* publish global position */
global_pos.timestamp = t;
global_pos.time_utc_usec = gps.time_utc_usec;
double est_lat, est_lon;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = est_lat;
global_pos.lon = est_lon;
global_pos.alt = local_pos.ref_alt - local_pos.z;
global_pos.vel_n = local_pos.vx;
global_pos.vel_e = local_pos.vy;
global_pos.vel_d = local_pos.vz;
global_pos.yaw = local_pos.yaw;
global_pos.eph = eph;
global_pos.epv = epv;
if (vehicle_global_position_pub == NULL) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
}
}
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[inav] stopped");
thread_running = false;
return 0;
}
void
MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
{
mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
uint64_t timestamp = hrt_absolute_time();
/* airspeed */
{
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
airspeed.timestamp = timestamp;
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
if (_airspeed_pub < 0) {
_airspeed_pub = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), _airspeed_pub, &airspeed);
}
}
/* attitude */
struct vehicle_attitude_s hil_attitude;
{
memset(&hil_attitude, 0, sizeof(hil_attitude));
math::Quaternion q(hil_state.attitude_quaternion);
math::Matrix<3, 3> C_nb = q.to_dcm();
math::Vector<3> euler = C_nb.to_euler();
hil_attitude.timestamp = timestamp;
memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
hil_attitude.R_valid = true;
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
hil_attitude.q[2] = q(2);
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler(0);
hil_attitude.pitch = euler(1);
hil_attitude.yaw = euler(2);
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
if (_attitude_pub < 0) {
_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
} else {
orb_publish(ORB_ID(vehicle_attitude), _attitude_pub, &hil_attitude);
}
}
/* global position */
{
struct vehicle_global_position_s hil_global_pos;
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
hil_global_pos.timestamp = timestamp;
hil_global_pos.global_valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
if (_global_pos_pub < 0) {
_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos);
}
}
/* local position */
{
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
_hil_local_alt0 = hil_state.alt / 1000.0f;
map_projection_init(hil_state.lat, hil_state.lon);
hil_local_pos.ref_timestamp = timestamp;
hil_local_pos.ref_lat = hil_state.lat;
hil_local_pos.ref_lon = hil_state.lon;
hil_local_pos.ref_alt = _hil_local_alt0;
}
float x;
float y;
map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
hil_local_pos.v_xy_valid = true;
hil_local_pos.v_z_valid = true;
hil_local_pos.x = x;
hil_local_pos.y = y;
hil_local_pos.z = _hil_local_alt0 - hil_state.alt / 1000.0f;
hil_local_pos.vx = hil_state.vx / 100.0f;
hil_local_pos.vy = hil_state.vy / 100.0f;
hil_local_pos.vz = hil_state.vz / 100.0f;
hil_local_pos.yaw = hil_attitude.yaw;
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
hil_local_pos.landed = landed;
if (_local_pos_pub < 0) {
_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
} else {
orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &hil_local_pos);
}
}
/* accelerometer */
{
struct accel_report accel;
memset(&accel, 0, sizeof(accel));
accel.timestamp = timestamp;
accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
accel.x = hil_state.xacc;
accel.y = hil_state.yacc;
accel.z = hil_state.zacc;
accel.temperature = 25.0f;
if (_accel_pub < 0) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
}
}
/* battery status */
{
struct battery_status_s hil_battery_status;
memset(&hil_battery_status, 0, sizeof(hil_battery_status));
hil_battery_status.timestamp = timestamp;
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.voltage_filtered_v = 11.1f;
hil_battery_status.current_a = 10.0f;
hil_battery_status.discharged_mah = -1.0f;
if (_battery_pub < 0) {
_battery_pub = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
} else {
orb_publish(ORB_ID(battery_status), _battery_pub, &hil_battery_status);
}
}
}
static void
handle_message(mavlink_message_t *msg)
{
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
mavlink_command_long_t cmd_mavlink;
mavlink_msg_command_long_decode(msg, &cmd_mavlink);
if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid)
|| (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
//check for MAVLINK terminate command
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
/* This is the link shutdown command, terminate mavlink */
printf("[mavlink] Terminating .. \n");
fflush(stdout);
usleep(50000);
/* terminate other threads and this thread */
thread_should_exit = true;
} else {
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = cmd_mavlink.param1;
vcmd.param2 = cmd_mavlink.param2;
vcmd.param3 = cmd_mavlink.param3;
vcmd.param4 = cmd_mavlink.param4;
vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
// XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = cmd_mavlink.confirmation;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* publish */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
}
}
if (msg->msgid == MAVLINK_MSG_ID_OPTICAL_FLOW) {
mavlink_optical_flow_t flow;
mavlink_msg_optical_flow_decode(msg, &flow);
struct optical_flow_s f;
f.timestamp = flow.time_usec;
f.flow_raw_x = flow.flow_x;
f.flow_raw_y = flow.flow_y;
f.flow_comp_x_m = flow.flow_comp_m_x;
f.flow_comp_y_m = flow.flow_comp_m_y;
f.ground_distance_m = flow.ground_distance;
f.quality = flow.quality;
f.sensor_id = flow.sensor_id;
/* check if topic is advertised */
if (flow_pub <= 0) {
flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
} else {
/* publish */
orb_publish(ORB_ID(optical_flow), flow_pub, &f);
}
}
if (msg->msgid == MAVLINK_MSG_ID_SET_MODE) {
/* Set mode on request */
mavlink_set_mode_t new_mode;
mavlink_msg_set_mode_decode(msg, &new_mode);
union px4_custom_mode custom_mode;
custom_mode.data = new_mode.custom_mode;
/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
vcmd.param1 = new_mode.base_mode;
vcmd.param2 = custom_mode.main_mode;
vcmd.param3 = 0;
vcmd.param4 = 0;
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = VEHICLE_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
vcmd.source_component = msg->compid;
vcmd.confirmation = 1;
/* check if topic is advertised */
if (cmd_pub <= 0) {
cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
} else {
/* create command */
orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
}
}
/* Handle Vicon position estimates */
if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
mavlink_vicon_position_estimate_t pos;
mavlink_msg_vicon_position_estimate_decode(msg, &pos);
vicon_position.timestamp = hrt_absolute_time();
vicon_position.x = pos.x;
vicon_position.y = pos.y;
vicon_position.z = pos.z;
vicon_position.roll = pos.roll;
vicon_position.pitch = pos.pitch;
vicon_position.yaw = pos.yaw;
if (vicon_position_pub <= 0) {
vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
} else {
orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position);
}
}
/* Handle quadrotor motor setpoints */
if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
if (mavlink_system.sysid < 4) {
/* switch to a receiving link mode */
gcs_link = false;
/*
* rate control mode - defined by MAVLink
*/
uint8_t ml_mode = 0;
bool ml_armed = false;
switch (quad_motors_setpoint.mode) {
case 0:
ml_armed = false;
break;
case 1:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
ml_armed = true;
break;
case 2:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
ml_armed = true;
break;
case 3:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
break;
case 4:
ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION;
break;
}
offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX;
offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX;
if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) {
ml_armed = false;
}
offboard_control_sp.armed = ml_armed;
offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
offboard_control_sp.timestamp = hrt_absolute_time();
/* check if topic has to be advertised */
if (offboard_control_sp_pub <= 0) {
offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
} else {
/* Publish */
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
}
}
}
/* handle status updates of the radio */
if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
struct telemetry_status_s tstatus;
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
/* publish telemetry status topic */
tstatus.timestamp = hrt_absolute_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);
} else {
orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
}
}
/*
* Only decode hil messages in HIL mode.
*
* The HIL mode is enabled by the HIL bit flag
* in the system mode. Either send a set mode
* COMMAND_LONG message or a SET_MODE message
*/
if (mavlink_hil_enabled) {
uint64_t timestamp = hrt_absolute_time();
if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
/* sensors general */
hil_sensors.timestamp = hrt_absolute_time();
/* hil gyro */
static const float mrad2rad = 1.0e-3f;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.gyro_raw[0] = imu.xgyro / mrad2rad;
hil_sensors.gyro_raw[1] = imu.ygyro / mrad2rad;
hil_sensors.gyro_raw[2] = imu.zgyro / mrad2rad;
hil_sensors.gyro_rad_s[0] = imu.xgyro;
hil_sensors.gyro_rad_s[1] = imu.ygyro;
hil_sensors.gyro_rad_s[2] = imu.zgyro;
/* accelerometer */
hil_sensors.accelerometer_counter = hil_counter;
static const float mg2ms2 = 9.8f / 1000.0f;
hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
hil_sensors.accelerometer_m_s2[0] = imu.xacc;
hil_sensors.accelerometer_m_s2[1] = imu.yacc;
hil_sensors.accelerometer_m_s2[2] = imu.zacc;
hil_sensors.accelerometer_mode = 0; // TODO what is this?
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
/* adc */
hil_sensors.adc_voltage_v[0] = 0.0f;
hil_sensors.adc_voltage_v[1] = 0.0f;
hil_sensors.adc_voltage_v[2] = 0.0f;
/* magnetometer */
float mga2ga = 1.0e-3f;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.magnetometer_raw[0] = imu.xmag / mga2ga;
hil_sensors.magnetometer_raw[1] = imu.ymag / mga2ga;
hil_sensors.magnetometer_raw[2] = imu.zmag / mga2ga;
hil_sensors.magnetometer_ga[0] = imu.xmag;
hil_sensors.magnetometer_ga[1] = imu.ymag;
hil_sensors.magnetometer_ga[2] = imu.zmag;
hil_sensors.magnetometer_range_ga = 32.7f; // int16
hil_sensors.magnetometer_mode = 0; // TODO what is this
hil_sensors.magnetometer_cuttoff_freq_hz = 50.0f;
/* baro */
hil_sensors.baro_pres_mbar = imu.abs_pressure;
hil_sensors.baro_alt_meter = imu.pressure_alt;
hil_sensors.baro_temp_celcius = imu.temperature;
hil_sensors.gyro_counter = hil_counter;
hil_sensors.magnetometer_counter = hil_counter;
hil_sensors.accelerometer_counter = hil_counter;
/* differential pressure */
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
hil_sensors.differential_pressure_counter = hil_counter;
/* airspeed from differential pressure, ambient pressure and temp */
struct airspeed_s airspeed;
float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa);
// XXX need to fix this
float tas = ias;
airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = ias;
airspeed.true_airspeed_m_s = tas;
if (pub_hil_airspeed < 0) {
pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
/* individual sensor publications */
struct gyro_report gyro;
gyro.x_raw = imu.xgyro / mrad2rad;
gyro.y_raw = imu.ygyro / mrad2rad;
gyro.z_raw = imu.zgyro / mrad2rad;
gyro.x = imu.xgyro;
gyro.y = imu.ygyro;
gyro.z = imu.zgyro;
gyro.temperature = imu.temperature;
gyro.timestamp = hrt_absolute_time();
if (pub_hil_gyro < 0) {
pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
} else {
orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
}
struct accel_report accel;
accel.x_raw = imu.xacc / mg2ms2;
accel.y_raw = imu.yacc / mg2ms2;
accel.z_raw = imu.zacc / mg2ms2;
accel.x = imu.xacc;
accel.y = imu.yacc;
accel.z = imu.zacc;
accel.temperature = imu.temperature;
accel.timestamp = hrt_absolute_time();
if (pub_hil_accel < 0) {
pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
}
struct mag_report mag;
mag.x_raw = imu.xmag / mga2ga;
mag.y_raw = imu.ymag / mga2ga;
mag.z_raw = imu.zmag / mga2ga;
mag.x = imu.xmag;
mag.y = imu.ymag;
mag.z = imu.zmag;
mag.timestamp = hrt_absolute_time();
if (pub_hil_mag < 0) {
pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
} else {
orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
}
struct baro_report baro;
baro.pressure = imu.abs_pressure;
baro.altitude = imu.pressure_alt;
baro.temperature = imu.temperature;
baro.timestamp = hrt_absolute_time();
if (pub_hil_baro < 0) {
pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
} else {
orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
}
/* publish combined sensor topic */
if (pub_hil_sensors > 0) {
orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
} else {
pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
}
/* fill in HIL battery status */
hil_battery_status.timestamp = hrt_absolute_time();
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.current_a = 10.0f;
/* lazily publish the battery voltage */
if (pub_hil_battery > 0) {
orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
} else {
pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
}
// increment counters
hil_counter++;
hil_frames++;
// output
if ((timestamp - old_timestamp) > 10000000) {
printf("receiving hil sensor at %d hz\n", hil_frames / 10);
old_timestamp = timestamp;
hil_frames = 0;
}
}
if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
mavlink_hil_gps_t gps;
mavlink_msg_hil_gps_decode(msg, &gps);
/* gps */
hil_gps.timestamp_position = gps.time_usec;
hil_gps.time_gps_usec = gps.time_usec;
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
hil_gps.s_variance_m_s = 5.0f;
hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
/* go back to -PI..PI */
if (heading_rad > M_PI_F)
heading_rad -= 2.0f * M_PI_F;
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
hil_gps.vel_ned_valid = true;
/* COG (course over ground) is spec'ed as -PI..+PI */
hil_gps.cog_rad = heading_rad;
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_visible = gps.satellites_visible;
/* publish GPS measurement data */
if (pub_hil_gps > 0) {
orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
} else {
pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
}
}
if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
struct airspeed_s airspeed;
airspeed.timestamp = hrt_absolute_time();
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
if (pub_hil_airspeed < 0) {
pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
} else {
orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
}
uint64_t timestamp = hrt_absolute_time();
// publish global position
if (pub_hil_global_pos > 0) {
orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
// global position packet
hil_global_pos.timestamp = timestamp;
hil_global_pos.valid = true;
hil_global_pos.lat = hil_state.lat;
hil_global_pos.lon = hil_state.lon;
hil_global_pos.alt = hil_state.alt / 1000.0f;
hil_global_pos.vx = hil_state.vx / 100.0f;
hil_global_pos.vy = hil_state.vy / 100.0f;
hil_global_pos.vz = hil_state.vz / 100.0f;
} else {
pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
}
// publish local position
if (pub_hil_local_pos > 0) {
float x;
float y;
bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
double lat = hil_state.lat*1e-7;
double lon = hil_state.lon*1e-7;
map_projection_project(lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
hil_local_pos.z_valid = true;
hil_local_pos.v_xy_valid = true;
hil_local_pos.v_z_valid = true;
hil_local_pos.x = x;
hil_local_pos.y = y;
hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
hil_local_pos.vx = hil_state.vx/100.0f;
hil_local_pos.vy = hil_state.vy/100.0f;
hil_local_pos.vz = hil_state.vz/100.0f;
hil_local_pos.yaw = hil_attitude.yaw;
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;
hil_local_pos.ref_timestamp = timestamp;
hil_local_pos.ref_lat = hil_state.lat;
hil_local_pos.ref_lon = hil_state.lon;
hil_local_pos.ref_alt = alt0;
hil_local_pos.landed = landed;
orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
} else {
pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
lat0 = hil_state.lat;
lon0 = hil_state.lon;
alt0 = hil_state.alt / 1000.0f;
map_projection_init(hil_state.lat, hil_state.lon);
}
/* Calculate Rotation Matrix */
math::Quaternion q(hil_state.attitude_quaternion);
math::Dcm C_nb(q);
math::EulerAngles euler(C_nb);
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
hil_attitude.R[i][j] = C_nb(i, j);
hil_attitude.R_valid = true;
/* set quaternion */
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
hil_attitude.q[2] = q(2);
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler.getPhi();
hil_attitude.pitch = euler.getTheta();
hil_attitude.yaw = euler.getPsi();
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
hil_attitude.yawspeed = hil_state.yawspeed;
/* set timestamp and notify processes (broadcast) */
hil_attitude.timestamp = hrt_absolute_time();
if (pub_hil_attitude > 0) {
orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
} else {
pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
}
struct accel_report accel;
accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
accel.x = hil_state.xacc;
accel.y = hil_state.yacc;
accel.z = hil_state.zacc;
accel.temperature = 25.0f;
accel.timestamp = hrt_absolute_time();
if (pub_hil_accel < 0) {
pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
} else {
orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
}
/* fill in HIL battery status */
hil_battery_status.timestamp = hrt_absolute_time();
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.current_a = 10.0f;
/* lazily publish the battery voltage */
if (pub_hil_battery > 0) {
orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
} else {
pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
}
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t man;
mavlink_msg_manual_control_decode(msg, &man);
struct rc_channels_s rc_hil;
memset(&rc_hil, 0, sizeof(rc_hil));
static orb_advert_t rc_pub = 0;
rc_hil.timestamp = hrt_absolute_time();
rc_hil.chan_count = 4;
rc_hil.chan[0].scaled = man.x / 1000.0f;
rc_hil.chan[1].scaled = man.y / 1000.0f;
rc_hil.chan[2].scaled = man.r / 1000.0f;
rc_hil.chan[3].scaled = man.z / 1000.0f;
struct manual_control_setpoint_s mc;
static orb_advert_t mc_pub = 0;
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
/* get a copy first, to prevent altering values that are not sent by the mavlink command */
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &mc);
mc.timestamp = rc_hil.timestamp;
mc.roll = man.x / 1000.0f;
mc.pitch = man.y / 1000.0f;
mc.yaw = man.r / 1000.0f;
mc.throttle = man.z / 1000.0f;
/* fake RC channels with manual control input from simulator */
if (rc_pub == 0) {
rc_pub = orb_advertise(ORB_ID(rc_channels), &rc_hil);
} else {
orb_publish(ORB_ID(rc_channels), rc_pub, &rc_hil);
}
if (mc_pub == 0) {
mc_pub = orb_advertise(ORB_ID(manual_control_setpoint), &mc);
} else {
orb_publish(ORB_ID(manual_control_setpoint), mc_pub, &mc);
}
}
}
}
void
MulticopterPositionControl::task_main()
{
warnx("started");
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[mpc] started");
/*
* do subscriptions
*/
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
parameters_update(true);
/* initialize values of critical structs until first regular update */
_arming.armed = false;
/* get an initial update for all sensor and status data */
poll_subscriptions();
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
math::Vector<3> sp_move_rate;
sp_move_rate.zero();
math::Vector<3> thrust_int;
thrust_int.zero();
math::Matrix<3, 3> R;
R.identity();
/* wakeup source */
struct pollfd fds[1];
fds[0].fd = _local_pos_sub;
fds[0].events = POLLIN;
while (!_task_should_exit) {
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 500);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
poll_subscriptions();
parameters_update(false);
hrt_abstime t = hrt_absolute_time();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.0f;
t_prev = t;
if (_control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
_reset_pos_sp = true;
_reset_alt_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = _control_mode.flag_armed;
update_ref();
if (_control_mode.flag_control_altitude_enabled ||
_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_climb_rate_enabled ||
_control_mode.flag_control_velocity_enabled) {
_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
_pos(2) = _local_pos.z;
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
_vel(2) = _local_pos.vz;
sp_move_rate.zero();
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();
/* move altitude setpoint with throttle stick */
sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
}
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();
/* move position setpoint with roll/pitch stick */
sp_move_rate(0) = _manual.x;
sp_move_rate(1) = _manual.y;
}
/* limit setpoint move rate */
float sp_move_norm = sp_move_rate.length();
if (sp_move_norm > 1.0f) {
sp_move_rate /= sp_move_norm;
}
/* scale to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
/* move position setpoint */
_pos_sp += sp_move_rate * dt;
/* check if position setpoint is too far from actual position */
math::Vector<3> pos_sp_offs;
pos_sp_offs.zero();
if (_control_mode.flag_control_position_enabled) {
pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
}
if (_control_mode.flag_control_altitude_enabled) {
pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
}
float pos_sp_offs_norm = pos_sp_offs.length();
if (pos_sp_offs_norm > 1.0f) {
pos_sp_offs /= pos_sp_offs_norm;
_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
}
} else {
/* AUTO */
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&_pos_sp.data[0], &_pos_sp.data[1]);
_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, loiter, reset position setpoint if needed */
reset_pos_sp();
reset_alt_sp();
}
}
/* fill local position setpoint */
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;
/* publish local position setpoint */
if (_local_pos_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
} else {
_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
}
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
_att_sp.yaw_body = _att.yaw;
_att_sp.thrust = 0.0f;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} else {
/* run position & altitude controllers, calculate velocity setpoint */
math::Vector<3> pos_err = _pos_sp - _pos;
_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
}
if (!_control_mode.flag_control_position_enabled) {
_reset_pos_sp = true;
_vel_sp(0) = 0.0f;
_vel_sp(1) = 0.0f;
}
/* use constant descend rate when landing, ignore altitude setpoint */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
_vel_sp(2) = _params.land_speed;
}
if (!_control_mode.flag_control_manual_enabled) {
/* limit 3D speed only in non-manual modes */
float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
if (vel_sp_norm > 1.0f) {
_vel_sp /= vel_sp_norm;
}
}
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
/* publish velocity setpoint */
if (_global_vel_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}
if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
/* reset integrals if needed */
if (_control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = _params.thr_min;
if (reset_int_z_manual) {
i = _manual.z;
if (i < _params.thr_min) {
i = _params.thr_min;
} else if (i > _params.thr_max) {
i = _params.thr_max;
}
}
thrust_int(2) = -i;
}
} else {
reset_int_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
if (reset_int_xy) {
reset_int_xy = false;
thrust_int(0) = 0.0f;
thrust_int(1) = 0.0f;
}
} else {
reset_int_xy = true;
}
/* velocity error */
math::Vector<3> vel_err = _vel_sp - _vel;
/* derivative of velocity error, not includes setpoint acceleration */
math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
_vel_prev = _vel;
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
if (!_control_mode.flag_control_velocity_enabled) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
}
if (!_control_mode.flag_control_climb_rate_enabled) {
thrust_sp(2) = 0.0f;
}
/* limit thrust vector and check for saturation */
bool saturation_xy = false;
bool saturation_z = false;
/* limit min lift */
float thr_min = _params.thr_min;
if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
/* don't allow downside thrust direction in manual attitude mode */
thr_min = 0.0f;
}
float tilt_max = _params.tilt_max_air;
/* adjust limits for landing mode */
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
/* limit max tilt and min lift when landing */
tilt_max = _params.tilt_max_land;
if (thr_min < 0.0f) {
thr_min = 0.0f;
}
}
/* limit min lift */
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
if (_control_mode.flag_control_velocity_enabled) {
/* limit max tilt */
if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) {
/* absolute horizontal thrust */
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
/* max horizontal thrust for given vertical thrust*/
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
} else {
/* thrust compensation for altitude only control mode */
float att_comp;
if (_att.R[2][2] > TILT_COS_MAX) {
att_comp = 1.0f / _att.R[2][2];
} else if (_att.R[2][2] > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
}
/* limit max thrust */
float thrust_abs = thrust_sp.length();
if (thrust_abs > _params.thr_max) {
if (thrust_sp(2) < 0.0f) {
if (-thrust_sp(2) > _params.thr_max) {
/* thrust Z component is too large, limit it */
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -_params.thr_max;
saturation_xy = true;
saturation_z = true;
} else {
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
} else {
/* Z component is negative, going down, simply limit thrust vector */
float k = _params.thr_max / thrust_abs;
thrust_sp *= k;
saturation_xy = true;
saturation_z = true;
}
thrust_abs = _params.thr_max;
}
/* update integrals */
if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
}
if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
/* protection against flipping on ground when landing */
if (thrust_int(2) > 0.0f) {
thrust_int(2) = 0.0f;
}
}
/* calculate attitude setpoint from thrust vector */
if (_control_mode.flag_control_velocity_enabled) {
/* desired body_z axis = -normalize(thrust_vector) */
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
/* no thrust, set Z axis to safe value */
body_z.zero();
body_z(2) = 1.0f;
}
/* vector of desired yaw direction in XY plane, rotated by PI/2 */
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
/* desired body_x axis, orthogonal to body_z */
body_x = y_C % body_z;
/* keep nose to front while inverted upside down */
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
} else {
/* desired thrust is in XY plane, set X downside to construct correct matrix,
* but yaw component will not be used actually */
body_x.zero();
body_x(2) = 1.0f;
}
/* desired body_y axis */
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
} else if (!_control_mode.flag_control_manual_enabled) {
/* autonomous altitude control without position control (failsafe landing),
* force level attitude, don't change yaw */
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
}
_att_sp.thrust = thrust_abs;
_att_sp.timestamp = hrt_absolute_time();
/* publish attitude setpoint */
if (_att_sp_pub > 0) {
orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
} else {
_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
}
} else {
reset_int_z = true;
}
}
} else {
/* position controller disabled, reset setpoints */
_reset_alt_sp = true;
_reset_pos_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = _control_mode.flag_armed && _control_mode.flag_control_manual_enabled && !_control_mode.flag_control_climb_rate_enabled;
}
warnx("stopped");
mavlink_log_info(_mavlink_fd, "[mpc] stopped");
_control_task = -1;
_exit(0);
}
void BlockLocalPositionEstimator::gpsCorrect()
{
// measure
Vector<double, n_y_gps> y_global;
if (gpsMeasure(y_global) != OK) { return; }
// gps measurement in local frame
double lat = y_global(0);
double lon = y_global(1);
float alt = y_global(2);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltHome);
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, 6> y;
y.setZero();
y(0) = px;
y(1) = py;
y(2) = pz;
y(3) = y_global(3);
y(4) = y_global(4);
y(5) = y_global(5);
// gps measurement matrix, measures position and velocity
Matrix<float, n_y_gps, n_x> C;
C.setZero();
C(Y_gps_x, X_x) = 1;
C(Y_gps_y, X_y) = 1;
C(Y_gps_z, X_z) = 1;
C(Y_gps_vx, X_vx) = 1;
C(Y_gps_vy, X_vy) = 1;
C(Y_gps_vz, X_vz) = 1;
// gps covariance matrix
Matrix<float, n_y_gps, n_y_gps> R;
R.setZero();
// default to parameter, use gps cov if provided
float var_xy = _gps_xy_stddev.get() * _gps_xy_stddev.get();
float var_z = _gps_z_stddev.get() * _gps_z_stddev.get();
float var_vxy = _gps_vxy_stddev.get() * _gps_vxy_stddev.get();
float var_vz = _gps_vz_stddev.get() * _gps_vz_stddev.get();
// if field is not zero, set it to the value provided
if (_sub_gps.get().eph > 1e-3f) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > 1e-3f) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;
R(3, 3) = var_vxy;
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
// get delayed x and P
float t_delay = 0;
int i = 0;
for (i = 1; i < HIST_LEN; i++) {
t_delay = 1.0e-6f * (_timeStamp - _tDelay.get(i)(0, 0));
if (t_delay > _gps_delay.get()) {
break;
}
}
// if you are 3 steps past the delay you wanted, this
// data is probably too old to use
if (t_delay > GPS_DELAY_MAX) {
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps delayed data too old: %8.4f", double(t_delay));
return;
}
Vector<float, n_x> x0 = _xDelay.get(i);
// residual
Vector<float, n_y_gps> r = y - C * x0;
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, 6>(C * _P * C.transpose() + R);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
if (beta > BETA_TABLE[n_y_gps]) {
if (_gpsFault < FAULT_MINOR) {
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] gps fault, beta: %5.2f", double(beta));
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] r: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
//double(r(0)), double(r(1)), double(r(2)),
//double(r(3)), double(r(4)), double(r(5)));
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] S_I: %5.2f %5.2f %5.2f %5.2f %5.2f %5.2f",
//double(S_I(0, 0)), double(S_I(1, 1)), double(S_I(2, 2)),
//double(S_I(3, 3)), double(S_I(4, 4)), double(S_I(5, 5)));
_gpsFault = FAULT_MINOR;
}
} else if (_gpsFault) {
_gpsFault = FAULT_NONE;
//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
// kalman filter correction if no hard fault
if (_gpsFault < fault_lvl_disable) {
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
_x += K * r;
_P -= K * C * _P;
}
}
void
BottleDrop::task_main()
{
_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
bool updated = false;
float z_0; // ground properties
float turn_radius; // turn radius of the UAV
float precision; // Expected precision of the UAV
float ground_distance = _alt_clearance; // Replace by closer estimate in loop
// constant
float g = CONSTANTS_ONE_G; // constant of gravity [m/s^2]
float m = 0.5f; // mass of bottle [kg]
float rho = 1.2f; // air density [kg/m^3]
float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
float dt_freefall_prediction = 0.01f; // step size of the free fall prediction [s]
// Has to be estimated by experiment
float cd = 0.86f; // Drag coefficient for a cylinder with a d/l ratio of 1/3 []
float t_signal =
0.084f; // Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
float t_door =
0.7f; // The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]
// Definition
float h_0; // height over target
float az; // acceleration in z direction[m/s^2]
float vz; // velocity in z direction [m/s]
float z; // fallen distance [m]
float h; // height over target [m]
float ax; // acceleration in x direction [m/s^2]
float vx; // ground speed in x direction [m/s]
float x; // traveled distance in x direction [m]
float vw; // wind speed [m/s]
float vrx; // relative velocity in x direction [m/s]
float v; // relative speed vector [m/s]
float Fd; // Drag force [N]
float Fdx; // Drag force in x direction [N]
float Fdz; // Drag force in z direction [N]
float x_drop, y_drop; // coordinates of the drop point in reference to the target (projection of NED)
float x_t, y_t; // coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
float x_l, y_l; // local position in projected coordinates
float x_f, y_f; // to-be position of the UAV after dt_runs seconds in projected coordinates
double x_f_NED, y_f_NED; // to-be position of the UAV after dt_runs seconds in NED
float distance_open_door; // The distance the UAV travels during its doors open [m]
float approach_error = 0.0f; // The error in radians between current ground vector and desired ground vector
float distance_real = 0; // The distance between the UAVs position and the drop point [m]
float future_distance = 0; // The distance between the UAVs to-be position and the drop point [m]
unsigned counter = 0;
param_t param_gproperties = param_find("BD_GPROPERTIES");
param_t param_turn_radius = param_find("BD_TURNRADIUS");
param_t param_precision = param_find("BD_PRECISION");
param_t param_cd = param_find("BD_OBJ_CD");
param_t param_mass = param_find("BD_OBJ_MASS");
param_t param_surface = param_find("BD_OBJ_SURFACE");
param_get(param_precision, &precision);
param_get(param_turn_radius, &turn_radius);
param_get(param_gproperties, &z_0);
param_get(param_cd, &cd);
param_get(param_mass, &m);
param_get(param_surface, &A);
int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct parameter_update_s update;
memset(&update, 0, sizeof(update));
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
struct mission_item_s flight_vector_s {};
struct mission_item_s flight_vector_e {};
flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_s.acceptance_radius = 50; // TODO: make parameter
flight_vector_s.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
flight_vector_e.acceptance_radius = 50; // TODO: make parameter
flight_vector_e.autocontinue = true;
flight_vector_s.altitude_is_relative = false;
struct wind_estimate_s wind;
// wakeup source(s)
struct pollfd fds[1];
// Setup of loop
fds[0].fd = _command_sub;
fds[0].events = POLLIN;
// Whatever state the bay is in, we want it closed on startup
lock_release();
close_bay();
while (!_task_should_exit) {
/* wait for up to 100ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
warn("poll error %d, %d", pret, errno);
continue;
}
/* vehicle commands updated */
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
/* copy global position */
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
if (_global_pos.timestamp == 0) {
continue;
}
const unsigned sleeptime_us = 9500;
hrt_abstime last_run = hrt_absolute_time();
float dt_runs = sleeptime_us / 1e6f;
// switch to faster updates during the drop
while (_drop_state > DROP_STATE_INIT) {
// Get wind estimate
orb_check(_wind_estimate_sub, &updated);
if (updated) {
orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
}
// Get vehicle position
orb_check(vehicle_global_position_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
}
// Get parameter updates
orb_check(parameter_update_sub, &updated);
if (updated) {
// copy global position
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
// update all parameters
param_get(param_gproperties, &z_0);
param_get(param_turn_radius, &turn_radius);
param_get(param_precision, &precision);
}
orb_check(_command_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
handle_command(&_command);
}
float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
ground_distance = _global_pos.alt - _target_position.alt;
// Distance to drop position and angle error to approach vector
// are relevant in all states greater than target valid (which calculates these positions)
if (_drop_state > DROP_STATE_TARGET_VALID) {
distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));
float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n);
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
approach_error = _wrap_pi(ground_direction - approach_direction);
if (counter % 90 == 0) {
mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
}
}
switch (_drop_state) {
case DROP_STATE_INIT:
// do nothing
break;
case DROP_STATE_TARGET_VALID:
{
az = g; // acceleration in z direction[m/s^2]
vz = 0; // velocity in z direction [m/s]
z = 0; // fallen distance [m]
h_0 = _global_pos.alt - _target_position.alt; // height over target at start[m]
h = h_0; // height over target [m]
ax = 0; // acceleration in x direction [m/s^2]
vx = groundspeed_body;// XXX project // ground speed in x direction [m/s]
x = 0; // traveled distance in x direction [m]
vw = 0; // wind speed [m/s]
vrx = 0; // relative velocity in x direction [m/s]
v = groundspeed_body; // relative speed vector [m/s]
Fd = 0; // Drag force [N]
Fdx = 0; // Drag force in x direction [N]
Fdz = 0; // Drag force in z direction [N]
// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
while (h > 0.05f) {
// z-direction
vz = vz + az * dt_freefall_prediction;
z = z + vz * dt_freefall_prediction;
h = h_0 - z;
// x-direction
vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
vx = vx + ax * dt_freefall_prediction;
x = x + vx * dt_freefall_prediction;
vrx = vx + vw;
// drag force
v = sqrtf(vz * vz + vrx * vrx);
Fd = 0.5f * rho * A * cd * (v * v);
Fdx = Fd * vrx / v;
Fdz = Fd * vz / v;
// acceleration
az = g - Fdz / m;
ax = -Fdx / m;
}
// compute drop vector
x = groundspeed_body * t_signal + x;
x_t = 0.0f;
y_t = 0.0f;
float wind_direction_n, wind_direction_e;
if (windspeed_norm < 0.5f) { // If there is no wind, an arbitrarily direction is chosen
wind_direction_n = 1.0f;
wind_direction_e = 0.0f;
} else {
wind_direction_n = wind.windspeed_north / windspeed_norm;
wind_direction_e = wind.windspeed_east / windspeed_norm;
}
x_drop = x_t + x * wind_direction_n;
y_drop = y_t + x * wind_direction_e;
map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
_drop_position.alt = _target_position.alt + _alt_clearance;
// Compute flight vector
map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
&(flight_vector_s.lat), &(flight_vector_s.lon));
flight_vector_s.altitude = _drop_position.alt;
map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
&flight_vector_e.lat, &flight_vector_e.lon);
flight_vector_e.altitude = _drop_position.alt;
// Save WPs in datamanager
const ssize_t len = sizeof(struct mission_item_s);
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
warnx("ERROR: could not save onboard WP");
}
if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
warnx("ERROR: could not save onboard WP");
}
_onboard_mission.count = 2;
_onboard_mission.current_seq = 0;
if (_onboard_mission_pub > 0) {
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
}
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F));
_drop_state = DROP_STATE_TARGET_SET;
}
break;
case DROP_STATE_TARGET_SET:
{
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
} else {
// We're close enough - open the bay
distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body));
if (isfinite(distance_real) && distance_real < distance_open_door &&
fabsf(approach_error) < math::radians(20.0f)) {
open_bay();
_drop_state = DROP_STATE_BAY_OPEN;
mavlink_log_info(_mavlink_fd, "#audio: opening bay");
}
}
}
break;
case DROP_STATE_BAY_OPEN:
{
if (_drop_approval) {
map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
x_f = x_l + _global_pos.vel_n * dt_runs;
y_f = y_l + _global_pos.vel_e * dt_runs;
map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
if (isfinite(distance_real) &&
(distance_real < precision) && ((distance_real < future_distance))) {
drop();
_drop_state = DROP_STATE_DROPPED;
mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
} else {
float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
if (distance_wp2 < distance_real) {
_onboard_mission.current_seq = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
}
}
}
break;
case DROP_STATE_DROPPED:
/* 2s after drop, reset and close everything again */
if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
_drop_state = DROP_STATE_INIT;
_drop_approval = false;
lock_release();
close_bay();
mavlink_log_info(_mavlink_fd, "#audio: closing bay");
// remove onboard mission
_onboard_mission.current_seq = -1;
_onboard_mission.count = 0;
orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
}
break;
case DROP_STATE_BAY_CLOSED:
// do nothing
break;
}
counter++;
// update_actuators();
// run at roughly 100 Hz
usleep(sleeptime_us);
dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
last_run = hrt_absolute_time();
}
}
warnx("exiting.");
_main_task = -1;
_exit(0);
}
void MulticopterPositionControl::control_auto(float dt)
{
if (!_mode_auto) {
_mode_auto = true;
/* reset position setpoint on AUTO mode activation */
reset_pos_sp();
reset_alt_sp();
}
//Poll position setpoint
bool updated;
orb_check(_pos_sp_triplet_sub, &updated);
if (updated) {
orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
//Make sure that the position setpoint is valid
if (!isfinite(_pos_sp_triplet.current.lat) ||
!isfinite(_pos_sp_triplet.current.lon) ||
!isfinite(_pos_sp_triplet.current.alt)) {
_pos_sp_triplet.current.valid = false;
}
}
if (_pos_sp_triplet.current.valid) {
/* in case of interrupted mission don't go to waypoint but stay at current position */
_reset_pos_sp = true;
_reset_alt_sp = true;
/* project setpoint to local frame */
math::Vector<3> curr_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
&curr_sp.data[0], &curr_sp.data[1]);
curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here
/* convert current setpoint to scaled space */
math::Vector<3> curr_sp_s = curr_sp.emult(scale);
/* by default use current setpoint as is */
math::Vector<3> pos_sp_s = curr_sp_s;
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
/* follow "previous - current" line */
math::Vector<3> prev_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
&prev_sp.data[0], &prev_sp.data[1]);
prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
if ((curr_sp - prev_sp).length() > MIN_DIST) {
/* find X - cross point of L1 sphere and trajectory */
math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();
if (curr_pos_s_len < 1.0f) {
/* copter is closer to waypoint than L1 radius */
/* check next waypoint and use it to avoid slowing down when passing via waypoint */
if (_pos_sp_triplet.next.valid) {
math::Vector<3> next_sp;
map_projection_project(&_ref_pos,
_pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
&next_sp.data[0], &next_sp.data[1]);
next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
if ((next_sp - curr_sp).length() > MIN_DIST) {
math::Vector<3> next_sp_s = next_sp.emult(scale);
/* calculate angle prev - curr - next */
math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
/* cos(a) * curr_next, a = angle between current and next trajectory segments */
float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
/* cos(b), b = angle pos - curr_sp - prev_sp */
float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
float curr_next_s_len = curr_next_s.length();
/* if curr - next distance is larger than L1 radius, limit it */
if (curr_next_s_len > 1.0f) {
cos_a_curr_next /= curr_next_s_len;
}
/* feed forward position setpoint offset */
math::Vector<3> pos_ff = prev_curr_s_norm *
cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
pos_sp_s += pos_ff;
}
}
}
} else {
bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
if (near) {
/* L1 sphere crosses trajectory */
} else {
/* copter is too far from trajectory */
/* if copter is behind prev waypoint, go directly to prev waypoint */
if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
pos_sp_s = prev_sp_s;
}
/* if copter is in front of curr waypoint, go directly to curr waypoint */
if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
pos_sp_s = curr_sp_s;
}
pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
}
}
}
}
/* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
/* difference between current and desired position setpoints, 1 = max speed */
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();
if (d_pos_m_len > dt) {
pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}
/* scale result back to normal space */
_pos_sp = pos_sp_s.edivide(scale);
/* update yaw setpoint if needed */
if (isfinite(_pos_sp_triplet.current.yaw)) {
_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
}
} else {
/* no waypoint, do nothing, setpoint was already reset */
}
}
/****************************************************************************
* main
****************************************************************************/
int position_estimator_inav_thread_main(int argc, char *argv[])
{
orb_advert_t mavlink_log_pub = nullptr;
float x_est[2] = { 0.0f, 0.0f }; // pos, vel
float y_est[2] = { 0.0f, 0.0f }; // pos, vel
float z_est[2] = { 0.0f, 0.0f }; // pos, vel
float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer
float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer
float R_gps[3][3]; // rotation matrix for GPS correction moment
memset(est_buf, 0, sizeof(est_buf));
memset(R_buf, 0, sizeof(R_buf));
memset(R_gps, 0, sizeof(R_gps));
int buf_ptr = 0;
static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation
float eph = max_eph_epv;
float epv = 1.0f;
float eph_flow = 1.0f;
float eph_vision = 0.2f;
float epv_vision = 0.2f;
float eph_mocap = 0.05f;
float epv_mocap = 0.05f;
float x_est_prev[2], y_est_prev[2], z_est_prev[2];
memset(x_est_prev, 0, sizeof(x_est_prev));
memset(y_est_prev, 0, sizeof(y_est_prev));
memset(z_est_prev, 0, sizeof(z_est_prev));
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
hrt_abstime accel_timestamp = 0;
hrt_abstime baro_timestamp = 0;
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
struct map_projection_reference_s ref;
memset(&ref, 0, sizeof(ref));
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
uint16_t flow_updates = 0;
uint16_t vision_updates = 0;
uint16_t mocap_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
hrt_abstime pub_last = hrt_absolute_time();
hrt_abstime t_prev = 0;
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float corr_baro = 0.0f; // D
float corr_gps[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_vision[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float corr_mocap[3][1] = {
{ 0.0f }, // N (pos)
{ 0.0f }, // E (pos)
{ 0.0f }, // D (pos)
};
const int mocap_heading = 2;
float dist_ground = 0.0f; //variables for lidar altitude estimation
float corr_lidar = 0.0f;
float lidar_offset = 0.0f;
int lidar_offset_count = 0;
bool lidar_first = true;
bool use_lidar = false;
bool use_lidar_prev = false;
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered)
hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered)
int n_flow = 0;
float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f };
float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f };
float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f };
float yaw_comp[] = { 0.0f, 0.0f };
hrt_abstime flow_time = 0;
float flow_min_dist = 0.2f;
bool gps_valid = false; // GPS is valid
bool lidar_valid = false; // lidar is valid
bool flow_valid = false; // flow is valid
bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false)
bool vision_valid = false; // vision is valid
bool mocap_valid = false; // mocap is valid
/* declare and safely initialize all structs */
struct actuator_controls_s actuator;
memset(&actuator, 0, sizeof(actuator));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
struct sensor_combined_s sensor;
memset(&sensor, 0, sizeof(sensor));
struct vehicle_gps_position_s gps;
memset(&gps, 0, sizeof(gps));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));
struct vision_position_estimate_s vision;
memset(&vision, 0, sizeof(vision));
struct att_pos_mocap_s mocap;
memset(&mocap, 0, sizeof(mocap));
struct vehicle_global_position_s global_pos;
memset(&global_pos, 0, sizeof(global_pos));
struct distance_sensor_s lidar;
memset(&lidar, 0, sizeof(lidar));
struct vehicle_rates_setpoint_s rates_setpoint;
memset(&rates_setpoint, 0, sizeof(rates_setpoint));
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
int vehicle_rate_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
/* advertise */
orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
orb_advert_t vehicle_global_position_pub = NULL;
struct position_estimator_inav_params params;
memset(¶ms, 0, sizeof(params));
struct position_estimator_inav_param_handles pos_inav_param_handles;
/* initialize parameter handles */
inav_parameters_init(&pos_inav_param_handles);
/* first parameters read at start up */
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub,
¶m_update); /* read from param topic to clear updated flag */
/* first parameters update */
inav_parameters_update(&pos_inav_param_handles, ¶ms);
px4_pollfd_struct_t fds_init[1] = {};
fds_init[0].fd = sensor_combined_sub;
fds_init[0].events = POLLIN;
/* wait for initial baro value */
bool wait_baro = true;
TerrainEstimator *terrain_estimator = new TerrainEstimator();
thread_running = true;
hrt_abstime baro_wait_for_sample_time = hrt_absolute_time();
while (wait_baro && !thread_should_exit) {
int ret = px4_poll(&fds_init[0], 1, 1000);
if (ret < 0) {
/* poll error */
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
wait_baro = false;
mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");
}
else if (ret > 0) {
if (fds_init[0].revents & POLLIN) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (wait_baro && sensor.baro_timestamp[0] != baro_timestamp) {
baro_timestamp = sensor.baro_timestamp[0];
baro_wait_for_sample_time = hrt_absolute_time();
/* mean calculation over several measurements */
if (baro_init_cnt < baro_init_num) {
if (PX4_ISFINITE(sensor.baro_alt_meter[0])) {
baro_offset += sensor.baro_alt_meter[0];
baro_init_cnt++;
}
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
local_pos.z_valid = true;
local_pos.v_z_valid = true;
}
}
}
} else {
PX4_WARN("INAV poll timeout");
}
}
/* main loop */
px4_pollfd_struct_t fds[1];
fds[0].fd = vehicle_attitude_sub;
fds[0].events = POLLIN;
while (!thread_should_exit) {
int ret = px4_poll(fds, 1, 20); // wait maximal 20 ms = 50 Hz minimum rate
hrt_abstime t = hrt_absolute_time();
if (ret < 0) {
/* poll error */
mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
continue;
} else if (ret > 0) {
/* act on attitude updates */
/* vehicle attitude */
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
bool updated;
/* parameter update */
orb_check(parameter_update_sub, &updated);
if (updated) {
struct parameter_update_s update;
orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
inav_parameters_update(&pos_inav_param_handles, ¶ms);
}
/* actuator */
orb_check(actuator_sub, &updated);
if (updated) {
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
orb_check(armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
}
/* sensor combined */
orb_check(sensor_combined_sub, &updated);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.accelerometer_timestamp[0] != accel_timestamp) {
if (att.R_valid) {
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
sensor.accelerometer_m_s2[2] -= acc_bias[2];
/* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) {
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
}
}
acc[2] += CONSTANTS_ONE_G;
} else {
memset(acc, 0, sizeof(acc));
}
accel_timestamp = sensor.accelerometer_timestamp[0];
accel_updates++;
}
if (sensor.baro_timestamp[0] != baro_timestamp) {
corr_baro = baro_offset - sensor.baro_alt_meter[0] - z_est[0];
baro_timestamp = sensor.baro_timestamp[0];
baro_updates++;
}
}
/* lidar alt estimation */
orb_check(distance_sensor_sub, &updated);
/* update lidar separately, needed by terrain estimator */
if (updated) {
orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar);
lidar.current_distance += params.lidar_calibration_offset;
}
if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
&& (PX4_R(att.R, 2, 2) > 0.7f)) {
if (!use_lidar_prev && use_lidar) {
lidar_first = true;
}
use_lidar_prev = use_lidar;
lidar_time = t;
dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
if (lidar_first) {
lidar_first = false;
lidar_offset = dist_ground + z_est[0];
mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR: new ground offset");
warnx("[inav] LIDAR: new ground offset");
}
corr_lidar = lidar_offset - dist_ground - z_est[0];
if (fabsf(corr_lidar) > params.lidar_err) { //check for spike
corr_lidar = 0;
lidar_valid = false;
lidar_offset_count++;
if (lidar_offset_count > 3) { //if consecutive bigger/smaller measurements -> new ground offset -> reinit
lidar_first = true;
lidar_offset_count = 0;
}
} else {
corr_lidar = lidar_offset - dist_ground - z_est[0];
lidar_valid = true;
lidar_offset_count = 0;
lidar_valid_time = t;
}
} else {
lidar_valid = false;
}
}
/* optical flow */
orb_check(optical_flow_sub, &updated);
if (updated && lidar_valid) {
orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
flow_time = t;
float flow_q = flow.quality / 255.0f;
float dist_bottom = lidar.current_distance;
if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
/* distance to surface */
//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
float flow_dist = dist_bottom; //use this if using lidar
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&
fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;
/*calculate offset of flow-gyro using already calibrated gyro from autopilot*/
flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;
flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;
flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;
//moving average
if (n_flow >= 100) {
gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];
gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];
n_flow = 0;
flow_gyrospeed_filtered[0] = 0.0f;
flow_gyrospeed_filtered[1] = 0.0f;
flow_gyrospeed_filtered[2] = 0.0f;
att_gyrospeed_filtered[0] = 0.0f;
att_gyrospeed_filtered[1] = 0.0f;
att_gyrospeed_filtered[2] = 0.0f;
} else {
flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);
flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);
flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);
att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);
att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);
att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
n_flow++;
}
/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
/* convert raw flow to angular flow (rad/s) */
float flow_ang[2];
/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */
orb_check(vehicle_rate_sp_sub, &updated);
if (updated)
orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
double rate_threshold = 0.15f;
if (fabs(rates_setpoint.pitch) < rate_threshold) {
//warnx("[inav] test ohne comp");
flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
}
else {
//warnx("[inav] test mit comp");
//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
+ gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)
}
if (fabs(rates_setpoint.roll) < rate_threshold) {
flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
}
else {
//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
+ gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
}
/* flow measurements vector */
float flow_m[3];
if (fabs(rates_setpoint.yaw) < rate_threshold) {
flow_m[0] = -flow_ang[0] * flow_dist;
flow_m[1] = -flow_ang[1] * flow_dist;
} else {
flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;
flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;
}
flow_m[2] = z_est[1];
/* velocity in NED */
float flow_v[2] = { 0.0f, 0.0f };
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
}
}
/* velocity correction */
corr_flow[0] = flow_v[0] - x_est[1];
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate) {
w_flow *= 0.05f;
}
/* under ideal conditions, on 1m distance assume EPH = 10cm */
eph_flow = 0.1f / w_flow;
flow_valid = true;
} else {
w_flow = 0.0f;
flow_valid = false;
}
flow_updates++;
}
/* check no vision circuit breaker is set */
if (params.no_vision != CBRK_NO_VISION_KEY) {
/* vehicle vision position */
orb_check(vision_position_estimate_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
static float last_vision_x = 0.0f;
static float last_vision_y = 0.0f;
static float last_vision_z = 0.0f;
/* reset position estimate on first vision update */
if (!vision_valid) {
x_est[0] = vision.x;
x_est[1] = vision.vx;
y_est[0] = vision.y;
y_est[1] = vision.vy;
/* only reset the z estimate if the z weight parameter is not zero */
if (params.w_z_vision_p > MIN_VALID_W) {
z_est[0] = vision.z;
z_est[1] = vision.vz;
}
vision_valid = true;
last_vision_x = vision.x;
last_vision_y = vision.y;
last_vision_z = vision.z;
warnx("VISION estimate valid");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION estimate valid");
}
/* calculate correction for position */
corr_vision[0][0] = vision.x - x_est[0];
corr_vision[1][0] = vision.y - y_est[0];
corr_vision[2][0] = vision.z - z_est[0];
static hrt_abstime last_vision_time = 0;
float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
last_vision_time = vision.timestamp_boot;
if (vision_dt > 0.000001f && vision_dt < 0.2f) {
vision.vx = (vision.x - last_vision_x) / vision_dt;
vision.vy = (vision.y - last_vision_y) / vision_dt;
vision.vz = (vision.z - last_vision_z) / vision_dt;
last_vision_x = vision.x;
last_vision_y = vision.y;
last_vision_z = vision.z;
/* calculate correction for velocity */
corr_vision[0][1] = vision.vx - x_est[1];
corr_vision[1][1] = vision.vy - y_est[1];
corr_vision[2][1] = vision.vz - z_est[1];
} else {
/* assume zero motion */
corr_vision[0][1] = 0.0f - x_est[1];
corr_vision[1][1] = 0.0f - y_est[1];
corr_vision[2][1] = 0.0f - z_est[1];
}
vision_updates++;
}
}
/* vehicle mocap position */
orb_check(att_pos_mocap_sub, &updated);
if (updated) {
orb_copy(ORB_ID(att_pos_mocap), att_pos_mocap_sub, &mocap);
if (!params.disable_mocap) {
/* reset position estimate on first mocap update */
if (!mocap_valid) {
x_est[0] = mocap.x;
y_est[0] = mocap.y;
z_est[0] = mocap.z;
mocap_valid = true;
warnx("MOCAP data valid");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP data valid");
}
/* calculate correction for position */
corr_mocap[0][0] = mocap.x - x_est[0];
corr_mocap[1][0] = mocap.y - y_est[0];
corr_mocap[2][0] = mocap.z - z_est[0];
mocap_updates++;
}
}
/* vehicle GPS position */
orb_check(vehicle_gps_position_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
bool reset_est = false;
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) {
gps_valid = false;
mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");
warnx("[inav] GPS signal lost");
}
} else {
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) {
gps_valid = true;
reset_est = true;
mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");
warnx("[inav] GPS signal found");
}
}
if (gps_valid) {
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
/* initialize reference position if needed */
if (!ref_inited) {
if (ref_init_start == 0) {
ref_init_start = t;
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s;
y_est[0] = 0.0f;
y_est[1] = gps.vel_e_m_s;
local_pos.ref_lat = lat;
local_pos.ref_lon = lon;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(&ref, lat, lon);
// XXX replace this print
warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
}
}
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(&ref, lat, lon, &(gps_proj[0]), &(gps_proj[1]));
/* reset position estimate when GPS becomes good */
if (reset_est) {
x_est[0] = gps_proj[0];
x_est[1] = gps.vel_n_m_s;
y_est[0] = gps_proj[1];
y_est[1] = gps.vel_e_m_s;
}
/* calculate index of estimated values in buffer */
int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
if (est_i < 0) {
est_i += EST_BUF_SIZE;
}
/* calculate correction for position */
corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
} else {
corr_gps[0][1] = 0.0f;
corr_gps[1][1] = 0.0f;
corr_gps[2][1] = 0.0f;
}
/* save rotation matrix at this moment */
memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
}
} else {
/* no GPS lock */
memset(corr_gps, 0, sizeof(corr_gps));
ref_init_start = 0;
}
gps_updates++;
}
}
/* check for timeout on FLOW topic */
if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
flow_valid = false;
warnx("FLOW timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] FLOW timeout");
}
/* check for timeout on GPS topic */
if (gps_valid && (t > (gps.timestamp_position + gps_topic_timeout))) {
gps_valid = false;
warnx("GPS timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] GPS timeout");
}
/* check for timeout on vision topic */
if (vision_valid && (t > (vision.timestamp_boot + vision_topic_timeout))) {
vision_valid = false;
warnx("VISION timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] VISION timeout");
}
/* check for timeout on mocap topic */
if (mocap_valid && (t > (mocap.timestamp_boot + mocap_topic_timeout))) {
mocap_valid = false;
warnx("MOCAP timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] MOCAP timeout");
}
/* check for lidar measurement timeout */
if (lidar_valid && (t > (lidar_time + lidar_timeout))) {
lidar_valid = false;
warnx("LIDAR timeout");
mavlink_log_info(&mavlink_log_pub, "[inav] LIDAR timeout");
}
float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms
t_prev = t;
/* increase EPH/EPV on each step */
if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0
eph = 0.001;
}
if (eph < max_eph_epv) {
eph *= 1.0f + dt;
}
if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
epv = 0.001;
}
if (epv < max_eph_epv) {
epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift)
}
/* use GPS if it's valid and reference position initialized */
//bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
//bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
bool use_gps_xy = false;
bool use_gps_z = false;
/* use VISION if it's valid and has a valid weight parameter */
bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
/* use MOCAP if it's valid and has a valid weight parameter */
bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap
if (params.disable_mocap) { //disable mocap if fake gps is used
use_mocap = false;
}
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* use LIDAR if it's valid and lidar altitude estimation is enabled */
use_lidar = lidar_valid && params.enable_lidar_alt_est;
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
float w_z_gps_v = params.w_z_gps_v * w_gps_z;
float w_xy_vision_p = params.w_xy_vision_p;
float w_xy_vision_v = params.w_xy_vision_v;
float w_z_vision_p = params.w_z_vision_p;
float w_mocap_p = params.w_mocap_p;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_xy_gps_p *= params.w_gps_flow;
w_xy_gps_v *= params.w_gps_flow;
}
/* baro offset correction */
if (use_gps_z) {
float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
baro_offset += offs_corr;
corr_baro += offs_corr;
}
/* accelerometer bias correction for GPS (use buffered rotation matrix) */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps_xy) {
accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
}
if (use_gps_z) {
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
}
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += R_gps[j][i] * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* accelerometer bias correction for VISION (use buffered rotation matrix) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_vision_xy) {
accel_bias_corr[0] -= corr_vision[0][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[0] -= corr_vision[0][1] * w_xy_vision_v;
accel_bias_corr[1] -= corr_vision[1][0] * w_xy_vision_p * w_xy_vision_p;
accel_bias_corr[1] -= corr_vision[1][1] * w_xy_vision_v;
}
if (use_vision_z) {
accel_bias_corr[2] -= corr_vision[2][0] * w_z_vision_p * w_z_vision_p;
}
/* accelerometer bias correction for MOCAP (use buffered rotation matrix) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_mocap) {
accel_bias_corr[0] -= corr_mocap[0][0] * w_mocap_p * w_mocap_p;
accel_bias_corr[1] -= corr_mocap[1][0] * w_mocap_p * w_mocap_p;
accel_bias_corr[2] -= corr_mocap[2][0] * w_mocap_p * w_mocap_p;
}
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* accelerometer bias correction for flow and baro (assume that there is no delay) */
accel_bias_corr[0] = 0.0f;
accel_bias_corr[1] = 0.0f;
accel_bias_corr[2] = 0.0f;
if (use_flow) {
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
}
if (use_lidar) {
accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;
} else {
accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
}
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
acc_bias[i] += c * params.w_acc_bias * dt;
}
}
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est, acc[2]);
if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
}
/* inertial filter correction for altitude */
if (use_lidar) {
inertial_filter_correct(corr_lidar, dt, z_est, 0, params.w_z_lidar);
} else {
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
}
if (use_gps_z) {
epv = fminf(epv, gps.epv);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
}
if (use_vision_z) {
epv = fminf(epv, epv_vision);
inertial_filter_correct(corr_vision[2][0], dt, z_est, 0, w_z_vision_p);
}
if (use_mocap) {
epv = fminf(epv, epv_mocap);
inertial_filter_correct(corr_mocap[2][0], dt, z_est, 0, w_mocap_p);
}
if (!(PX4_ISFINITE(z_est[0]) && PX4_ISFINITE(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
memset(corr_mocap, 0, sizeof(corr_mocap));
corr_baro = 0;
} else {
memcpy(z_est_prev, z_est, sizeof(z_est));
}
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est, acc[0]);
inertial_filter_predict(dt, y_est, acc[1]);
if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
}
/* inertial filter correction for position */
if (use_flow) {
eph = fminf(eph, eph_flow);
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
}
if (use_gps_xy) {
eph = fminf(eph, gps.eph);
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
}
}
if (use_vision_xy) {
eph = fminf(eph, eph_vision);
inertial_filter_correct(corr_vision[0][0], dt, x_est, 0, w_xy_vision_p);
inertial_filter_correct(corr_vision[1][0], dt, y_est, 0, w_xy_vision_p);
if (w_xy_vision_v > MIN_VALID_W) {
inertial_filter_correct(corr_vision[0][1], dt, x_est, 1, w_xy_vision_v);
inertial_filter_correct(corr_vision[1][1], dt, y_est, 1, w_xy_vision_v);
}
}
if (use_mocap) {
eph = fminf(eph, eph_mocap);
inertial_filter_correct(corr_mocap[0][0], dt, x_est, 0, w_mocap_p);
inertial_filter_correct(corr_mocap[1][0], dt, y_est, 0, w_mocap_p);
}
if (!(PX4_ISFINITE(x_est[0]) && PX4_ISFINITE(x_est[1]) && PX4_ISFINITE(y_est[0]) && PX4_ISFINITE(y_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
memset(corr_mocap, 0, sizeof(corr_mocap));
memset(corr_flow, 0, sizeof(corr_flow));
} else {
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
}
} else {
/* gradually reset xy velocity estimates */
inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
}
/* run terrain estimator */
terrain_estimator->predict(dt, &att, &sensor, &lidar);
terrain_estimator->measurement_update(hrt_absolute_time(), &gps, &lidar, &att);
if (inav_verbose_mode) {
/* print updates rate */
if (t > updates_counter_start + updates_counter_len) {
float updates_dt = (t - updates_counter_start) * 0.000001f;
warnx(
"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s, vision = %.1f/s, mocap = %.1f/s",
(double)(accel_updates / updates_dt),
(double)(baro_updates / updates_dt),
(double)(gps_updates / updates_dt),
(double)(attitude_updates / updates_dt),
(double)(flow_updates / updates_dt),
(double)(vision_updates / updates_dt),
(double)(mocap_updates / updates_dt));
updates_counter_start = t;
accel_updates = 0;
baro_updates = 0;
gps_updates = 0;
attitude_updates = 0;
flow_updates = 0;
vision_updates = 0;
mocap_updates = 0;
}
}
if (t > pub_last + PUB_INTERVAL) {
pub_last = t;
/* push current estimate to buffer */
est_buf[buf_ptr][0][0] = x_est[0];
est_buf[buf_ptr][0][1] = x_est[1];
est_buf[buf_ptr][1][0] = y_est[0];
est_buf[buf_ptr][1][1] = y_est[1];
est_buf[buf_ptr][2][0] = z_est[0];
est_buf[buf_ptr][2][1] = z_est[1];
/* push current rotation matrix to buffer */
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
buf_ptr++;
if (buf_ptr >= EST_BUF_SIZE) {
buf_ptr = 0;
}
/* publish local position */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;
//local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
//local_pos.z_global = local_pos.z_valid && use_gps_z;
local_pos.xy_global = true;
local_pos.z_global = true;
local_pos.x = x_est[0];
local_pos.vx = x_est[1];
local_pos.y = y_est[0];
local_pos.vy = y_est[1];
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.yaw = att.yaw;
local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph;
local_pos.epv = epv;
if (local_pos.dist_bottom_valid) {
local_pos.dist_bottom = dist_ground;
local_pos.dist_bottom_rate = - z_est[1];
}
local_pos.timestamp = t;
orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
if (local_pos.xy_global && local_pos.z_global) {
/* publish global position */
global_pos.timestamp = t;
global_pos.time_utc_usec = gps.time_utc_usec;
double est_lat, est_lon;
map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
global_pos.lat = est_lat;
global_pos.lon = est_lon;
global_pos.alt = local_pos.ref_alt - local_pos.z;
global_pos.vel_n = local_pos.vx;
global_pos.vel_e = local_pos.vy;
global_pos.vel_d = local_pos.vz;
global_pos.yaw = local_pos.yaw;
global_pos.eph = eph;
global_pos.epv = epv;
if (terrain_estimator->is_valid()) {
global_pos.terrain_alt = global_pos.alt - terrain_estimator->get_distance_to_ground();
global_pos.terrain_alt_valid = true;
} else {
global_pos.terrain_alt_valid = false;
}
global_pos.pressure_alt = sensor.baro_alt_meter[0];
if (vehicle_global_position_pub == NULL) {
vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
} else {
orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
}
}
}
}
warnx("stopped");
mavlink_log_info(&mavlink_log_pub, "[inav] stopped");
thread_running = false;
return 0;
}
void *multirotor_position_control_thread_main()
{
/* welcome user */
fprintf (stdout, "Multirotor position controller started\n");
fflush(stdout);
int i;
bool_t updated;
bool_t reset_mission_sp = 0 /* false */;
bool_t global_pos_sp_valid = 0 /* false */;
bool_t reset_man_sp_z = 1 /* true */;
bool_t reset_man_sp_xy = 1 /* true */;
bool_t reset_int_z = 1 /* true */;
bool_t reset_int_z_manual = 0 /* false */;
bool_t reset_int_xy = 1 /* true */;
bool_t was_armed = 0 /* false */;
bool_t reset_auto_sp_xy = 1 /* true */;
bool_t reset_auto_sp_z = 1 /* true */;
bool_t reset_takeoff_sp = 1 /* true */;
absolute_time t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float i_limit; /* use integral_limit_out = tilt_max / 2 */
float ref_alt = 0.0f;
absolute_time ref_alt_t = 0;
absolute_time local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
/* structures */
struct parameter_update_s ps;
struct vehicle_control_flags_s control_flags;
memset(&control_flags, 0, sizeof(control_flags));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
orb_subscr_t param_sub = orb_subscribe(ORB_ID(parameter_update));
if (param_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to parameter_update topic\n");
exit(-1);
}
orb_subscr_t control_flags_sub = orb_subscribe(ORB_ID(vehicle_control_flags));
if (control_flags_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_control_flags topic\n");
exit(-1);
}
orb_subscr_t att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
if (att_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude topic\n");
exit(-1);
}
orb_subscr_t att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
if (att_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_attitude_setpoint topic\n");
exit(-1);
}
orb_subscr_t manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
if (manual_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to manual_control_setpoint topic\n");
exit(-1);
}
orb_subscr_t local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
if (local_pos_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position_setpoint topic\n");
exit(-1);
}
orb_subscr_t local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
if (local_pos_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_local_position topic\n");
exit(-1);
}
orb_subscr_t global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
if (global_pos_sp_sub < 0)
{
fprintf (stderr, "Position controller thread failed to subscribe to vehicle_global_position_setpoint topic\n");
exit(-1);
}
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint));
if (local_pos_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_local_position_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint));
if (global_vel_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_global_velocity_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint));
if (att_sp_pub == -1)
{
fprintf (stderr, "Comunicator thread failed to advertise the vehicle_attitude_setpoint topic\n");
exit (-1);
}
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
/* abort on a nonzero return value from the parameter init */
if (multirotor_position_control_params_init() != 0) {
/* parameter setup went wrong, abort */
fprintf (stderr, "Multirotor position controller aborting on startup due to an error\n");
exit(-1);
}
for (i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p, 0.0f,
multirotor_position_control_parameters.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p, multirotor_position_control_parameters.xy_vel_i,
multirotor_position_control_parameters.xy_vel_d, 1.0f, multirotor_position_control_parameters.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (!_shutdown_all_systems) {
updated = orb_check (ORB_ID(parameter_update), param_sub);
if (updated) {
/* clear updated flag */
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update multirotor_position_control_parameters */
multirotor_position_control_params_update();
for (i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), multirotor_position_control_parameters.xy_p,
0.0f, multirotor_position_control_parameters.xy_d, 1.0f, 0.0f);
if (multirotor_position_control_parameters.xy_vel_i > 0.0f) {
i_limit = multirotor_position_control_parameters.tilt_max / multirotor_position_control_parameters.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_vel_pids[i]), multirotor_position_control_parameters.xy_vel_p,
multirotor_position_control_parameters.xy_vel_i, multirotor_position_control_parameters.xy_vel_d, i_limit, multirotor_position_control_parameters.tilt_max);
}
pid_set_parameters(&z_pos_pid, multirotor_position_control_parameters.z_p, 0.0f,
multirotor_position_control_parameters.z_d, 1.0f, multirotor_position_control_parameters.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, multirotor_position_control_parameters.z_vel_p, multirotor_position_control_parameters.z_vel_i,
multirotor_position_control_parameters.z_vel_d, -multirotor_position_control_parameters.thr_max, -multirotor_position_control_parameters.thr_min);
}
updated = orb_check (ORB_ID(vehicle_control_flags), control_flags_sub);
if (updated) {
orb_copy(ORB_ID(vehicle_control_flags), control_flags_sub, &control_flags);
}
updated = orb_check (ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = 1 /* true */;
reset_mission_sp = 1 /* true */;
}
absolute_time t = get_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
if (control_flags.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = 1 /* true */;
reset_man_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
reset_auto_sp_xy = 1 /* true */;
reset_takeoff_sp = 1 /* true */;
reset_int_z = 1 /* true */;
reset_int_xy = 1 /* true */;
}
was_armed = control_flags.flag_armed;
t_prev = t;
if (control_flags.flag_control_altitude_enabled || control_flags.flag_control_velocity_enabled || control_flags.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = multirotor_position_control_parameters.z_vel_max / multirotor_position_control_parameters.z_p * 2.0f;
float xy_sp_offs_max = multirotor_position_control_parameters.xy_vel_max / multirotor_position_control_parameters.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_flags.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_flags.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
//mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.thrust - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * multirotor_position_control_parameters.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_flags.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
//mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / multirotor_position_control_parameters.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / multirotor_position_control_parameters.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * multirotor_position_control_parameters.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_flags.flag_control_position_enabled;
reset_auto_sp_z = !control_flags.flag_control_altitude_enabled;
reset_takeoff_sp = 1 /* true */;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = 1 /* true */;
} else if (control_flags.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_flags.auto_state == navigation_state_auto_ready) {
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_takeoff) {
if (reset_takeoff_sp) {
reset_takeoff_sp = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - multirotor_position_control_parameters.takeoff_alt - multirotor_position_control_parameters.takeoff_gap;
att_sp.yaw_body = att.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
reset_auto_sp_xy = 0 /* false */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_rtl) {
// TODO
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
} else if (control_flags.auto_state == navigation_state_auto_mission) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
reset_mission_sp = 1 /* true */;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
//mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (reset_mission_sp) {
reset_mission_sp = 0 /* false */;
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.latitude * 1e-7;
double sp_lon = global_pos_sp.longitude * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
att_sp.yaw_body = global_pos_sp.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {
reset_auto_sp_xy = 0 /* false */;
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
}
if (reset_auto_sp_z) {
reset_auto_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
}
//mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
}
if (control_flags.auto_state != navigation_state_auto_takeoff) {
reset_takeoff_sp = 1 /* true */;
}
if (control_flags.auto_state != navigation_state_auto_mission) {
reset_mission_sp = 1 /* true */;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = 1 /* true */;
reset_man_sp_z = 1 /* true */;
} else {
/* no control (failsafe), loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
//mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_man_sp_z = 1 /* true */;
} else {
/* in air: hold altitude */
if (reset_man_sp_z) {
reset_man_sp_z = 0 /* false */;
local_pos_sp.z = local_pos.z;
//mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
reset_auto_sp_z = 0 /* false */;
}
if (control_flags.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = 0 /* false */;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
//mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
reset_auto_sp_xy = 0 /* false */;
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_flags.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
} else {
reset_man_sp_z = 1 /* true */;
global_vel_sp.vz = 0.0f;
}
if (control_flags.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
/* limit horizontal speed */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / multirotor_position_control_parameters.xy_vel_max;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
} else {
reset_man_sp_xy = 1 /* true */;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
//fprintf (stderr, "global_vel_sp.vx:%.3f\tglobal_vel_sp.vy:%.3f\tglobal_vel_sp.vz:%.3f\n", global_vel_sp.vx, global_vel_sp.vy, global_vel_sp.vz);
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
if (control_flags.flag_control_climb_rate_enabled || control_flags.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
if (control_flags.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = 0 /* false */;
float i = multirotor_position_control_parameters.thr_min;
if (reset_int_z_manual) {
i = manual.thrust;
if (i < multirotor_position_control_parameters.thr_min) {
i = multirotor_position_control_parameters.thr_min;
} else if (i > multirotor_position_control_parameters.thr_max) {
i = multirotor_position_control_parameters.thr_max;
}
}
thrust_pid_set_integral(&z_vel_pid, -i);
//mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
} else {
/* reset thrust integral when altitude control enabled */
reset_int_z = 1 /* true */;
}
if (control_flags.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_int_xy) {
reset_int_xy = 0 /* false */;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
//mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
/* limit horizontal part of thrust */
float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > multirotor_position_control_parameters.tilt_max) {
tilt = multirotor_position_control_parameters.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
} else {
reset_int_xy = 1 /* true */;
}
att_sp.timestamp = get_absolute_time();
//fprintf (stderr, "att_sp.roll:%.3f\tatt_sp.pitch:%.3f\tatt_sp.yaw:%.3f\tatt_sp.thrust:%.3f\n", att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust);
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = 1 /* true */;
reset_man_sp_xy = 1 /* true */;
reset_int_z = 1 /* true */;
reset_int_xy = 1 /* true */;
reset_mission_sp = 1 /* true */;
reset_auto_sp_xy = 1 /* true */;
reset_auto_sp_z = 1 /* true */;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_flags.flag_armed && control_flags.flag_control_manual_enabled && !control_flags.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
/*
* do unsubscriptions
*/
orb_unsubscribe(ORB_ID(parameter_update), param_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_control_flags), control_flags_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_attitude), att_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, pthread_self());
orb_unsubscribe(ORB_ID(manual_control_setpoint), manual_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_local_position), local_pos_sub, pthread_self());
orb_unsubscribe(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, pthread_self());
/*
* do unadvertises
*/
orb_unadvertise(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, pthread_self());
orb_unadvertise(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, pthread_self());
orb_unadvertise(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, pthread_self());
return 0;
}
static int multirotor_pos_control_thread_main(int argc, char *argv[])
{
/* welcome user */
warnx("started");
static int mavlink_fd;
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
mavlink_log_info(mavlink_fd, "[mpc] started");
/* structures */
struct vehicle_control_mode_s control_mode;
memset(&control_mode, 0, sizeof(control_mode));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
struct vehicle_attitude_setpoint_s att_sp;
memset(&att_sp, 0, sizeof(att_sp));
struct manual_control_setpoint_s manual;
memset(&manual, 0, sizeof(manual));
struct vehicle_local_position_s local_pos;
memset(&local_pos, 0, sizeof(local_pos));
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
/* subscribe to attitude, motor setpoints and system state */
int param_sub = orb_subscribe(ORB_ID(parameter_update));
int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* publish setpoint */
orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
bool reset_mission_sp = false;
bool global_pos_sp_valid = false;
bool reset_man_sp_z = true;
bool reset_man_sp_xy = true;
bool reset_int_z = true;
bool reset_int_z_manual = false;
bool reset_int_xy = true;
bool was_armed = false;
bool reset_auto_sp_xy = true;
bool reset_auto_sp_z = true;
bool reset_takeoff_sp = true;
hrt_abstime t_prev = 0;
const float alt_ctl_dz = 0.2f;
const float pos_ctl_dz = 0.05f;
float ref_alt = 0.0f;
hrt_abstime ref_alt_t = 0;
uint64_t local_ref_timestamp = 0;
PID_t xy_pos_pids[2];
PID_t xy_vel_pids[2];
PID_t z_pos_pid;
thrust_pid_t z_vel_pid;
thread_running = true;
struct multirotor_position_control_params params;
struct multirotor_position_control_param_handles params_h;
parameters_init(¶ms_h);
parameters_update(¶ms_h, ¶ms);
for (int i = 0; i < 2; i++) {
pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
}
pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
while (!thread_should_exit) {
bool param_updated;
orb_check(param_sub, ¶m_updated);
if (param_updated) {
/* clear updated flag */
struct parameter_update_s ps;
orb_copy(ORB_ID(parameter_update), param_sub, &ps);
/* update params */
parameters_update(¶ms_h, ¶ms);
for (int i = 0; i < 2; i++) {
pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
/* use integral_limit_out = tilt_max / 2 */
float i_limit;
if (params.xy_vel_i > 0.0f) {
i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
} else {
i_limit = 0.0f; // not used
}
pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
}
pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
}
bool updated;
orb_check(control_mode_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
}
orb_check(global_pos_sp_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
global_pos_sp_valid = true;
reset_mission_sp = true;
}
hrt_abstime t = hrt_absolute_time();
float dt;
if (t_prev != 0) {
dt = (t - t_prev) * 0.000001f;
} else {
dt = 0.0f;
}
if (control_mode.flag_armed && !was_armed) {
/* reset setpoints and integrals on arming */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_auto_sp_z = true;
reset_auto_sp_xy = true;
reset_takeoff_sp = true;
reset_int_z = true;
reset_int_xy = true;
}
was_armed = control_mode.flag_armed;
t_prev = t;
if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_manual_enabled) {
/* manual control */
/* check for reference point updates and correct setpoint */
if (local_pos.ref_timestamp != ref_alt_t) {
if (ref_alt_t != 0) {
/* home alt changed, don't follow large ground level changes in manual flight */
local_pos_sp.z += local_pos.ref_alt - ref_alt;
}
ref_alt_t = local_pos.ref_timestamp;
ref_alt = local_pos.ref_alt;
// TODO also correct XY setpoint
}
/* reset setpoints to current position if needed */
if (control_mode.flag_control_altitude_enabled) {
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
}
/* move altitude setpoint with throttle stick */
float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
if (z_sp_ctl != 0.0f) {
sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
local_pos_sp.z += sp_move_rate[2] * dt;
if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
local_pos_sp.z = local_pos.z + z_sp_offs_max;
} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
local_pos_sp.z = local_pos.z - z_sp_offs_max;
}
}
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
/* move position setpoint with roll/pitch stick */
float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
/* calculate direction and increment of control in NED frame */
float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
local_pos_sp.x += sp_move_rate[0] * dt;
local_pos_sp.y += sp_move_rate[1] * dt;
/* limit maximum setpoint from position offset and preserve direction
* fail safe, should not happen in normal operation */
float pos_vec_x = local_pos_sp.x - local_pos.x;
float pos_vec_y = local_pos_sp.y - local_pos.y;
float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
if (pos_vec_norm > 1.0f) {
local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
}
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
reset_auto_sp_xy = !control_mode.flag_control_position_enabled;
reset_auto_sp_z = !control_mode.flag_control_altitude_enabled;
reset_takeoff_sp = true;
/* force reprojection of global setpoint after manual mode */
reset_mission_sp = true;
} else if (control_mode.flag_control_auto_enabled) {
/* AUTO mode, use global setpoint */
if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
if (reset_takeoff_sp) {
reset_takeoff_sp = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
reset_auto_sp_xy = false;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
// TODO
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
/* init local projection using local position ref */
if (local_pos.ref_timestamp != local_ref_timestamp) {
reset_mission_sp = true;
local_ref_timestamp = local_pos.ref_timestamp;
double lat_home = local_pos.ref_lat * 1e-7;
double lon_home = local_pos.ref_lon * 1e-7;
map_projection_init(lat_home, lon_home);
mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
}
if (reset_mission_sp) {
reset_mission_sp = false;
/* update global setpoint projection */
if (global_pos_sp_valid) {
/* global position setpoint valid, use it */
double sp_lat = global_pos_sp.lat * 1e-7;
double sp_lon = global_pos_sp.lon * 1e-7;
/* project global setpoint to local setpoint */
map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
if (global_pos_sp.altitude_is_relative) {
local_pos_sp.z = -global_pos_sp.altitude;
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
/* update yaw setpoint only if value is valid */
if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
att_sp.yaw_body = global_pos_sp.yaw;
}
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
} else {
if (reset_auto_sp_xy) {
reset_auto_sp_xy = false;
/* local position setpoint is invalid,
* use current position as setpoint for loiter */
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
}
if (reset_auto_sp_z) {
reset_auto_sp_z = false;
local_pos_sp.z = local_pos.z;
}
mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
}
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) {
reset_takeoff_sp = true;
}
if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
reset_mission_sp = true;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = true;
reset_man_sp_z = true;
} else {
/* no control (failsafe), loiter or stay on ground */
if (local_pos.landed) {
/* landed: move setpoint down */
/* in air: hold altitude */
if (local_pos_sp.z < 5.0f) {
/* set altitude setpoint to 5m under ground,
* don't set it too deep to avoid unexpected landing in case of false "landed" signal */
local_pos_sp.z = 5.0f;
mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
}
reset_man_sp_z = true;
} else {
/* in air: hold altitude */
if (reset_man_sp_z) {
reset_man_sp_z = false;
local_pos_sp.z = local_pos.z;
mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
}
reset_auto_sp_z = false;
}
if (control_mode.flag_control_position_enabled) {
if (reset_man_sp_xy) {
reset_man_sp_xy = false;
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
}
reset_auto_sp_xy = false;
}
}
/* publish local position setpoint */
orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
/* run position & altitude controllers, calculate velocity setpoint */
if (control_mode.flag_control_altitude_enabled) {
global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
} else {
reset_man_sp_z = true;
global_vel_sp.vz = 0.0f;
}
if (control_mode.flag_control_position_enabled) {
/* calculate velocity set point in NED frame */
global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
/* limit horizontal speed */
float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
if (xy_vel_sp_norm > 1.0f) {
global_vel_sp.vx /= xy_vel_sp_norm;
global_vel_sp.vy /= xy_vel_sp_norm;
}
} else {
reset_man_sp_xy = true;
global_vel_sp.vx = 0.0f;
global_vel_sp.vy = 0.0f;
}
/* publish new velocity setpoint */
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
// TODO subscribe to velocity setpoint if altitude/position control disabled
if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
if (control_mode.flag_control_climb_rate_enabled) {
if (reset_int_z) {
reset_int_z = false;
float i = params.thr_min;
if (reset_int_z_manual) {
i = manual.throttle;
if (i < params.thr_min) {
i = params.thr_min;
} else if (i > params.thr_max) {
i = params.thr_max;
}
}
thrust_pid_set_integral(&z_vel_pid, -i);
mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
}
thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
att_sp.thrust = -thrust_sp[2];
} else {
/* reset thrust integral when altitude control enabled */
reset_int_z = true;
}
if (control_mode.flag_control_velocity_enabled) {
/* calculate thrust set point in NED frame */
if (reset_int_xy) {
reset_int_xy = false;
pid_reset_integral(&xy_vel_pids[0]);
pid_reset_integral(&xy_vel_pids[1]);
mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
}
thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
/* limit horizontal part of thrust */
float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
/* assuming that vertical component of thrust is g,
* horizontal component = g * tan(alpha) */
float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
if (tilt > params.tilt_max) {
tilt = params.tilt_max;
}
/* convert direction to body frame */
thrust_xy_dir -= att.yaw;
/* calculate roll and pitch */
att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
} else {
reset_int_xy = true;
}
att_sp.timestamp = hrt_absolute_time();
/* publish new attitude setpoint */
orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
}
} else {
/* position controller disabled, reset setpoints */
reset_man_sp_z = true;
reset_man_sp_xy = true;
reset_int_z = true;
reset_int_xy = true;
reset_mission_sp = true;
reset_auto_sp_xy = true;
reset_auto_sp_z = true;
}
/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
/* run at approximately 50 Hz */
usleep(20000);
}
warnx("stopped");
mavlink_log_info(mavlink_fd, "[mpc] stopped");
thread_running = false;
fflush(stdout);
return 0;
}
__EXPORT int map_projection_global_project(double lat, double lon, float *x, float *y)
{
return map_projection_project(&mp_ref, lat, lon, x, y);
}
void FollowTarget::on_active()
{
struct map_projection_reference_s target_ref;
math::Vector<3> target_reported_velocity(0, 0, 0);
follow_target_s target_motion_with_offset = {};
uint64_t current_time = hrt_absolute_time();
bool _radius_entered = false;
bool _radius_exited = false;
bool updated = false;
float dt_ms = 0;
orb_check(_follow_target_sub, &updated);
if (updated) {
follow_target_s target_motion;
_target_updates++;
// save last known motion topic
_previous_target_motion = _current_target_motion;
orb_copy(ORB_ID(follow_target), _follow_target_sub, &target_motion);
if (_current_target_motion.timestamp == 0) {
_current_target_motion = target_motion;
}
_current_target_motion.timestamp = target_motion.timestamp;
_current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)(
1 - _responsiveness);
_current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)(
1 - _responsiveness);
target_reported_velocity(0) = _current_target_motion.vx;
target_reported_velocity(1) = _current_target_motion.vy;
} else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
reset_target_validity();
}
// update distance to target
if (target_position_valid()) {
// get distance to target
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
&_target_distance(1));
}
// update target velocity
if (target_velocity_valid() && updated) {
dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
// ignore a small dt
if (dt_ms > 10.0F) {
// get last gps known reference for target
map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
// calculate distance the target has moved
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon,
&(_target_position_delta(0)), &(_target_position_delta(1)));
// update the average velocity of the target based on the position
_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
// if the target is moving add an offset and rotation
if (_est_target_vel.length() > .5F) {
_target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset;
}
// are we within the target acceptance radius?
// give a buffer to exit/enter the radius to give the velocity controller
// a chance to catch up
_radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
_radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
// to keep the velocity increase/decrease smooth
// calculate how many velocity increments/decrements
// it will take to reach the targets velocity
// with the given amount of steps also add a feed forward input that adjusts the
// velocity as the position gap increases since
// just traveling at the exact velocity of the target will not
// get any closer or farther from the target
_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
// if we are less than 1 meter from the target don't worry about trying to yaw
// lock the yaw until we are at a distance that makes sense
if ((_target_distance).length() > 1.0F) {
// yaw rate smoothing
// this really needs to control the yaw rate directly in the attitude pid controller
// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = (_yaw_angle - _navigator->get_global_position()->yaw) / (dt_ms / 1000.0F);
_yaw_rate = _wrap_pi(_yaw_rate);
_yaw_rate = math::constrain(_yaw_rate, -1.0F * _yaw_auto_max, _yaw_auto_max);
} else {
_yaw_angle = _yaw_rate = NAN;
}
}
// warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d con ratio = %3.6f yaw rate = %3.6f",
// (double) _step_vel(0),
// (double) _step_vel(1),
// (double) _current_vel(0),
// (double) _current_vel(1),
// (double) _est_target_vel(0),
// (double) _est_target_vel(1),
// (double) (_target_distance).length(),
// (double) (_target_position_offset + _target_distance).length(),
// _follow_target_state,
// (double)_avg_cos_ratio, (double) _yaw_rate);
}
if (target_position_valid()) {
// get the target position using the calculated offset
map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1),
&target_motion_with_offset.lat, &target_motion_with_offset.lon);
}
// clamp yaw rate smoothing if we are with in
// 3 degrees of facing target
if (PX4_ISFINITE(_yaw_rate)) {
if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_global_position()->yaw)) < math::radians(3.0F)) {
_yaw_rate = NAN;
}
}
// update state machine
switch (_follow_target_state) {
case TRACK_POSITION: {
if (_radius_entered == true) {
_follow_target_state = TRACK_VELOCITY;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle);
// keep the current velocity updated with the target velocity for when it's needed
_current_vel = _est_target_vel;
update_position_sp(true, true, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case TRACK_VELOCITY: {
if (_radius_exited == true) {
_follow_target_state = TRACK_POSITION;
} else if (target_velocity_valid()) {
if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) {
_current_vel += _step_vel;
_last_update_time = current_time;
}
set_follow_target_item(&_mission_item, _param_min_alt.get(), target_motion_with_offset, _yaw_angle);
update_position_sp(true, false, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case SET_WAIT_FOR_TARGET_POSITION: {
// Climb to the minimum altitude
// and wait until a position is received
follow_target_s target = {};
// for now set the target at the minimum height above the uav
target.lat = _navigator->get_global_position()->lat;
target.lon = _navigator->get_global_position()->lon;
target.alt = 0.0F;
set_follow_target_item(&_mission_item, _param_min_alt.get(), target, _yaw_angle);
update_position_sp(false, false, _yaw_rate);
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
/* FALLTHROUGH */
case WAIT_FOR_TARGET_POSITION: {
if (is_mission_item_reached() && target_velocity_valid()) {
_target_position_offset(0) = _follow_offset;
_follow_target_state = TRACK_POSITION;
}
break;
}
}
}
void BlockLocalPositionEstimator::gpsCorrect()
{
// measure
Vector<double, n_y_gps> y_global;
if (gpsMeasure(y_global) != OK) { return; }
// gps measurement in local frame
double lat = y_global(Y_gps_x);
double lon = y_global(Y_gps_y);
float alt = y_global(Y_gps_z);
float px = 0;
float py = 0;
float pz = -(alt - _gpsAltOrigin);
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector<float, n_y_gps> y;
y.setZero();
y(Y_gps_x) = px;
y(Y_gps_y) = py;
y(Y_gps_z) = pz;
y(Y_gps_vx) = y_global(Y_gps_vx);
y(Y_gps_vy) = y_global(Y_gps_vy);
y(Y_gps_vz) = y_global(Y_gps_vz);
// gps measurement matrix, measures position and velocity
Matrix<float, n_y_gps, n_x> C;
C.setZero();
C(Y_gps_x, X_x) = 1;
C(Y_gps_y, X_y) = 1;
C(Y_gps_z, X_z) = 1;
C(Y_gps_vx, X_vx) = 1;
C(Y_gps_vy, X_vy) = 1;
C(Y_gps_vz, X_vz) = 1;
// gps covariance matrix
SquareMatrix<float, n_y_gps> R;
R.setZero();
// default to parameter, use gps cov if provided
float var_xy = _param_lpe_gps_xy.get() * _param_lpe_gps_xy.get();
float var_z = _param_lpe_gps_z.get() * _param_lpe_gps_z.get();
float var_vxy = _param_lpe_gps_vxy.get() * _param_lpe_gps_vxy.get();
float var_vz = _param_lpe_gps_vz.get() * _param_lpe_gps_vz.get();
// if field is not below minimum, set it to the value provided
if (_sub_gps.get().eph > _param_lpe_gps_xy.get()) {
var_xy = _sub_gps.get().eph * _sub_gps.get().eph;
}
if (_sub_gps.get().epv > _param_lpe_gps_z.get()) {
var_z = _sub_gps.get().epv * _sub_gps.get().epv;
}
float gps_s_stddev = _sub_gps.get().s_variance_m_s;
if (gps_s_stddev > _param_lpe_gps_vxy.get()) {
var_vxy = gps_s_stddev * gps_s_stddev;
}
if (gps_s_stddev > _param_lpe_gps_vz.get()) {
var_vz = gps_s_stddev * gps_s_stddev;
}
R(0, 0) = var_xy;
R(1, 1) = var_xy;
R(2, 2) = var_z;
R(3, 3) = var_vxy;
R(4, 4) = var_vxy;
R(5, 5) = var_vz;
// get delayed x
uint8_t i_hist = 0;
if (getDelayPeriods(_param_lpe_gps_delay.get(), &i_hist) < 0) { return; }
Vector<float, n_x> x0 = _xDelay.get(i_hist);
// residual
Vector<float, n_y_gps> r = y - C * x0;
// residual covariance
Matrix<float, n_y_gps, n_y_gps> S = C * _P * C.transpose() + R;
// publish innovations
for (size_t i = 0; i < 6; i++) {
_pub_innov.get().vel_pos_innov[i] = r(i);
_pub_innov.get().vel_pos_innov_var[i] = S(i, i);
}
// residual covariance, (inverse)
Matrix<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
// fault detection
float beta = (r.transpose() * (S_I * r))(0, 0);
// artifically increase beta threshhold to prevent fault during landing
float beta_thresh = 1e2f;
if (beta / BETA_TABLE[n_y_gps] > beta_thresh) {
if (!(_sensorFault & SENSOR_GPS)) {
mavlink_log_critical(&mavlink_log_pub, "[lpe] gps fault %3g %3g %3g %3g %3g %3g",
double(r(0) * r(0) / S_I(0, 0)), double(r(1) * r(1) / S_I(1, 1)), double(r(2) * r(2) / S_I(2, 2)),
double(r(3) * r(3) / S_I(3, 3)), double(r(4) * r(4) / S_I(4, 4)), double(r(5) * r(5) / S_I(5, 5)));
_sensorFault |= SENSOR_GPS;
}
} else if (_sensorFault & SENSOR_GPS) {
_sensorFault &= ~SENSOR_GPS;
mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] GPS OK");
}
// kalman filter correction always for GPS
Matrix<float, n_x, n_y_gps> K = _P * C.transpose() * S_I;
Vector<float, n_x> dx = K * r;
_x += dx;
_P -= K * C * _P;
}
int BlockLocalPositionEstimator::flowMeasure(Vector<float, n_y_flow> &y)
{
// check for agl
if (agl() < flow_min_agl) {
return -1;
}
// check quality
float qual = _sub_flow.get().quality;
if (qual < _flow_min_q.get()) {
return -1;
}
// calculate range to center of image for flow
if (!_validTZ) {
return -1;
}
float d = agl() * cosf(_sub_att.get().roll) * cosf(_sub_att.get().pitch);
// check for global accuracy
if (_gpsInitialized) {
double lat = _sub_gps.get().lat * 1.0e-7;
double lon = _sub_gps.get().lon * 1.0e-7;
float px = 0;
float py = 0;
map_projection_project(&_map_ref, lat, lon, &px, &py);
Vector2f delta(px - _flowX, py - _flowY);
if (delta.norm() > 3) {
mavlink_and_console_log_info(&mavlink_log_pub,
"[lpe] flow too far from GPS, resetting position");
_flowX = px;
_flowY = py;
return -1;
}
}
// optical flow in x, y axis
float flow_x_rad = _sub_flow.get().pixel_flow_x_integral;
float flow_y_rad = _sub_flow.get().pixel_flow_y_integral;
// angular rotation in x, y axis
float gyro_x_rad = _flow_gyro_x_high_pass.update(
_sub_flow.get().gyro_x_rate_integral);
float gyro_y_rad = _flow_gyro_y_high_pass.update(
_sub_flow.get().gyro_y_rate_integral);
//warnx("flow x: %10.4f y: %10.4f gyro_x: %10.4f gyro_y: %10.4f d: %10.4f",
//double(flow_x_rad), double(flow_y_rad), double(gyro_x_rad), double(gyro_y_rad), double(d));
// compute velocities in camera frame using ground distance
// assume camera frame is body frame
Vector3f delta_b(
-(flow_x_rad - gyro_x_rad)*d,
-(flow_y_rad - gyro_y_rad)*d,
0);
// rotation of flow from body to nav frame
Matrix3f R_nb(_sub_att.get().R);
Vector3f delta_n = R_nb * delta_b;
// flow integration
_flowX += delta_n(0);
_flowY += delta_n(1);
// measurement
y(Y_flow_x) = _flowX;
y(Y_flow_y) = _flowY;
_flowQStats.update(Scalarf(_sub_flow.get().quality));
// imporant to timestamp flow even if distance is bad
_time_last_flow = _timeStamp;
return OK;
}
void PrecLand::slewrate(float &sp_x, float &sp_y)
{
matrix::Vector2f sp_curr(sp_x, sp_y);
uint64_t now = hrt_absolute_time();
float dt = (now - _last_slewrate_time);
if (dt < 1) {
// bad dt, can't divide by it
return;
}
dt /= SEC2USEC;
if (!_last_slewrate_time) {
// running the first time since switching to precland
// assume dt will be about 50000us
dt = 50000 / SEC2USEC;
// set a best guess for previous setpoints for smooth transition
map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat,
_navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1));
_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
}
_last_slewrate_time = now;
// limit the setpoint speed to the maximum cruise speed
matrix::Vector2f sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
if (sp_vel.length() > _param_xy_vel_cruise.get()) {
sp_vel = sp_vel.normalized() * _param_xy_vel_cruise.get();
sp_curr = _sp_pev + sp_vel * dt;
}
// limit the setpoint acceleration to the maximum acceleration
matrix::Vector2f sp_acc = (sp_curr - _sp_pev * 2 + _sp_pev_prev) / (dt * dt); // acceleration of the setpoints
if (sp_acc.length() > _param_acceleration_hor.get()) {
sp_acc = sp_acc.normalized() * _param_acceleration_hor.get();
sp_curr = _sp_pev * 2 - _sp_pev_prev + sp_acc * (dt * dt);
}
// limit the setpoint speed such that we can stop at the setpoint given the maximum acceleration/deceleration
float max_spd = sqrtf(_param_acceleration_hor.get() * ((matrix::Vector2f)(_sp_pev - matrix::Vector2f(sp_x,
sp_y))).length());
sp_vel = (sp_curr - _sp_pev) / dt; // velocity of the setpoints
if (sp_vel.length() > max_spd) {
sp_vel = sp_vel.normalized() * max_spd;
sp_curr = _sp_pev + sp_vel * dt;
}
_sp_pev_prev = _sp_pev;
_sp_pev = sp_curr;
sp_x = sp_curr(0);
sp_y = sp_curr(1);
}
void AttitudePositionEstimatorEKF::pollData()
{
//Update arming status
bool armedUpdate;
orb_check(_armedSub, &armedUpdate);
if (armedUpdate) {
orb_copy(ORB_ID(actuator_armed), _armedSub, &_armed);
}
//Update Gyro and Accelerometer
static Vector3f lastAngRate;
static Vector3f lastAccel;
bool accel_updated = false;
orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
static hrt_abstime last_accel = 0;
static hrt_abstime last_mag = 0;
if (last_accel != _sensor_combined.accelerometer_timestamp) {
accel_updated = true;
} else {
accel_updated = false;
}
last_accel = _sensor_combined.accelerometer_timestamp;
// Copy gyro and accel
_last_sensor_timestamp = _sensor_combined.timestamp;
IMUmsec = _sensor_combined.timestamp / 1e3;
IMUusec = _sensor_combined.timestamp;
float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;
/* guard against too large deltaT's */
if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
deltaT = 0.01f;
}
_last_run = _sensor_combined.timestamp;
// Always store data, independent of init status
/* fill in last data set */
_ekf->dtIMU = deltaT;
int last_gyro_main = _gyro_main;
if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
(_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];
_gyro_main = 0;
_gyro_valid = true;
} else if (PX4_ISFINITE(_sensor_combined.gyro1_rad_s[0]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[1]) &&
PX4_ISFINITE(_sensor_combined.gyro1_rad_s[2])) {
_ekf->angRate.x = _sensor_combined.gyro1_rad_s[0];
_ekf->angRate.y = _sensor_combined.gyro1_rad_s[1];
_ekf->angRate.z = _sensor_combined.gyro1_rad_s[2];
_gyro_main = 1;
_gyro_valid = true;
} else {
_gyro_valid = false;
}
if (last_gyro_main != _gyro_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "GYRO FAILED! Switched from #%d to %d", last_gyro_main, _gyro_main);
}
if (!_gyro_valid) {
/* keep last value if he hit an out of band value */
lastAngRate = _ekf->angRate;
} else {
perf_count(_perf_gyro);
}
if (accel_updated) {
int last_accel_main = _accel_main;
/* fail over to the 2nd accel if we know the first is down */
if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
_accel_main = 0;
} else {
_ekf->accel.x = _sensor_combined.accelerometer1_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer1_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer1_m_s2[2];
_accel_main = 1;
}
if (!_accel_valid) {
lastAccel = _ekf->accel;
}
if (last_accel_main != _accel_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "ACCEL FAILED! Switched from #%d to %d", last_accel_main, _accel_main);
}
_accel_valid = true;
}
_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
lastAngRate = _ekf->angRate;
_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
lastAccel = _ekf->accel;
if (last_mag != _sensor_combined.magnetometer_timestamp) {
_newDataMag = true;
} else {
_newDataMag = false;
}
last_mag = _sensor_combined.magnetometer_timestamp;
//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);
//Update Land Detector
bool newLandData;
orb_check(_landDetectorSub, &newLandData);
if (newLandData) {
orb_copy(ORB_ID(vehicle_land_detected), _landDetectorSub, &_landDetector);
}
//Update AirSpeed
orb_check(_airspeed_sub, &_newAdsData);
if (_newAdsData) {
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
}
bool gps_update;
orb_check(_gps_sub, &gps_update);
if (gps_update) {
orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
perf_count(_perf_gps);
//We are more strict for our first fix
float requiredAccuracy = _parameters.pos_stddev_threshold;
if (_gpsIsGood) {
requiredAccuracy = _parameters.pos_stddev_threshold * 2.0f;
}
//Check if the GPS fix is good enough for us to use
if (_gps.fix_type >= 3 && _gps.eph < requiredAccuracy && _gps.epv < requiredAccuracy) {
_gpsIsGood = true;
} else {
_gpsIsGood = false;
}
if (_gpsIsGood) {
//Calculate time since last good GPS fix
const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
//Stop dead-reckoning mode
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] stop dead-reckoning");
}
_global_pos.dead_reckoning = false;
//Fetch new GPS data
_ekf->GPSstatus = _gps.fix_type;
_ekf->velNED[0] = _gps.vel_n_m_s;
_ekf->velNED[1] = _gps.vel_e_m_s;
_ekf->velNED[2] = _gps.vel_d_m_s;
_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
_ekf->gpsHgt = _gps.alt / 1e3f;
if (_previousGPSTimestamp != 0) {
//Calculate average time between GPS updates
_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
// update LPF
_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
}
//check if we had a GPS outage for a long time
if (_gps_initialized) {
//Convert from global frame to local frame
map_projection_project(&_pos_ref, (_gps.lat / 1.0e7), (_gps.lon / 1.0e7), &_ekf->posNE[0], &_ekf->posNE[1]);
if (dtLastGoodGPS > POS_RESET_THRESHOLD) {
_ekf->ResetPosition();
_ekf->ResetVelocity();
}
}
//warnx("gps alt: %6.1f, interval: %6.3f", (double)_ekf->gpsHgt, (double)dtGoodGPS);
// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
// _ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
// } else {
// _ekf->vneSigma = _parameters.velne_noise;
// }
// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
// _ekf->posNeSigma = sqrtf(_gps.p_variance_m);
// } else {
// _ekf->posNeSigma = _parameters.posne_noise;
// }
// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);
_previousGPSTimestamp = _gps.timestamp_position;
}
}
// If it has gone more than POS_RESET_THRESHOLD amount of seconds since we received a GPS update,
// then something is very wrong with the GPS (possibly a hardware failure or comlink error)
const float dtLastGoodGPS = static_cast<float>(_gps.timestamp_position - _previousGPSTimestamp) / 1e6f;
if (dtLastGoodGPS >= POS_RESET_THRESHOLD) {
if (_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] gave up dead-reckoning after long timeout");
}
_gpsIsGood = false;
_global_pos.dead_reckoning = false;
}
//If we have no good GPS fix for half a second, then enable dead-reckoning mode while armed (for up to POS_RESET_THRESHOLD seconds)
else if (dtLastGoodGPS >= 0.5f) {
if (_armed.armed) {
if (!_global_pos.dead_reckoning) {
mavlink_log_info(_mavlink_fd, "[ekf] dead-reckoning enabled");
}
_global_pos.dead_reckoning = true;
} else {
_global_pos.dead_reckoning = false;
}
}
//Update barometer
orb_check(_baro_sub, &_newHgtData);
if (_newHgtData) {
static hrt_abstime baro_last = 0;
orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
// init lowpass filters for baro and gps altitude
float baro_elapsed;
if (baro_last == 0) {
baro_elapsed = 0.0f;
} else {
baro_elapsed = (_baro.timestamp - baro_last) / 1e6f;
}
baro_last = _baro.timestamp;
if(!_baro_init) {
_baro_init = true;
_baroAltRef = _baro.altitude;
}
_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
_ekf->baroHgt = _baro.altitude;
_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
perf_count(_perf_baro);
}
//Update Magnetometer
if (_newDataMag) {
_mag_valid = true;
perf_count(_perf_mag);
int last_mag_main = _mag_main;
Vector3f mag0(_sensor_combined.magnetometer_ga[0], _sensor_combined.magnetometer_ga[1],
_sensor_combined.magnetometer_ga[2]);
Vector3f mag1(_sensor_combined.magnetometer1_ga[0], _sensor_combined.magnetometer1_ga[1],
_sensor_combined.magnetometer1_ga[2]);
const unsigned mag_timeout_us = 200000;
/* fail over to the 2nd mag if we know the first is down */
if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
mag0.length() > 0.1f) {
_ekf->magData.x = mag0.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = mag0.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = mag0.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 0;
} else if (hrt_elapsed_time(&_sensor_combined.magnetometer1_timestamp) < mag_timeout_us &&
mag1.length() > 0.1f) {
_ekf->magData.x = mag1.x;
_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
_ekf->magData.y = mag1.y;
_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset
_ekf->magData.z = mag1.z;
_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset
_mag_main = 1;
} else {
_mag_valid = false;
}
if (last_mag_main != _mag_main) {
mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
}
}
//Update range data
orb_check(_distance_sub, &_newRangeData);
if (_newRangeData) {
orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance);
if ((_distance.current_distance > _distance.min_distance)
&& (_distance.current_distance < _distance.max_distance)) {
_ekf->rngMea = _distance.current_distance;
_distance_last_valid = _distance.timestamp;
} else {
_newRangeData = false;
}
}
}
