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