void LinTORManager::setupFolders()
{
// Use the folders response to construct the flickableTabBar:
QJson::Parser parser;
bool ok;
QVariantMap result = parser.parse(foldersReply->readAll(), &ok).toMap();
foldersReply->deleteLater();
foldersReply = 0;
if (!ok)
{
qWarning() << "Failed to parse The Old Reader folders data";
return;
}
// Clear out any existing folder items:
folderItems.clear();
torTreeWidget->clear();
QList<QVariant> folderVariants = result["tags"].toList();
if (!folderVariants.isEmpty())
{
QList<QVariant>::const_iterator index = folderVariants.constBegin();
QList<QVariant>::const_iterator end = folderVariants.constEnd();
QMap<QString, QVariant> singleFolderMap;
LinTORFolderItem *item;
QString folderID;
while (index != end)
{
singleFolderMap = (*index).toMap();
folderID = singleFolderMap["id"].toString(),
item = new LinTORFolderItem(
folderID,
singleFolderMap["sortid"].toString(),
Standard_TORFolder);
torTreeWidget->addTopLevelItem(item);
folderItems[folderID] = item;
++index;
}
}
// Add a "reading-list" item:
allSubsItem =
new LinTORFolderItem("user/-/state/com.google/reading-list", "", Standard_TORFolder);
torTreeWidget->addTopLevelItem(allSubsItem);
getSubscriptions();
}
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
_att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
_pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
_missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
_manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
_status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
_param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
// publications
_actuators(ORB_ID(actuator_controls_0), &getPublications())
{
}
void Block::updateSubscriptions()
{
UOrbSubscriptionBase *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
if (count++ > maxSubscriptionsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
printf("exceeded max subscriptions for block: %s\n", name);
break;
}
sub->update();
sub = sub->getSibling();
}
}
void Block::updateSubscriptions()
{
uORB::SubscriptionNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != nullptr) {
if (count++ > maxSubscriptionsPerBlock) {
char name[blockNameLengthMax];
getName(name, blockNameLengthMax);
PX4_ERR("exceeded max subscriptions for block: %s", name);
break;
}
sub->update();
sub = sub->getSibling();
}
}
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// this block has no parent, and has name LPE
SuperBlock(nullptr, "LPE"),
// subscriptions, set rate, add to list
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
// set flow max update rate higher than expected to we don't lose packets
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
// main prediction loop, 100 hz
_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
// status updates 2 hz
_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
// gps 10 hz
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
// vision 50 hz
_sub_vision_pos(ORB_ID(vehicle_vision_position), 1000 / 50, 0, &getSubscriptions()),
// mocap 50 hz
_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 50, 0, &getSubscriptions()),
// all distance sensors, 10 hz
_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
_dist_subs(),
_sub_lidar(nullptr),
_sub_sonar(nullptr),
_sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()),
// publications
_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
_pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),
// map projection
_map_ref(),
// block parameters
_fusion(this, "FUSION"),
_vxy_pub_thresh(this, "VXY_PUB"),
_z_pub_thresh(this, "Z_PUB"),
_sonar_z_stddev(this, "SNR_Z"),
_sonar_z_offset(this, "SNR_OFF_Z"),
_lidar_z_stddev(this, "LDR_Z"),
_lidar_z_offset(this, "LDR_OFF_Z"),
_accel_xy_stddev(this, "ACC_XY"),
_accel_z_stddev(this, "ACC_Z"),
_baro_stddev(this, "BAR_Z"),
_gps_delay(this, "GPS_DELAY"),
_gps_xy_stddev(this, "GPS_XY"),
_gps_z_stddev(this, "GPS_Z"),
_gps_vxy_stddev(this, "GPS_VXY"),
_gps_vz_stddev(this, "GPS_VZ"),
_gps_eph_max(this, "EPH_MAX"),
_gps_epv_max(this, "EPV_MAX"),
_vision_xy_stddev(this, "VIS_XY"),
_vision_z_stddev(this, "VIS_Z"),
_vision_delay(this, "VIS_DELAY"),
_mocap_p_stddev(this, "VIC_P"),
_flow_z_offset(this, "FLW_OFF_Z"),
_flow_scale(this, "FLW_SCALE"),
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),
_flow_r(this, "FLW_R"),
_flow_rr(this, "FLW_RR"),
_land_z_stddev(this, "LAND_Z"),
_land_vxy_stddev(this, "LAND_VXY"),
_pn_p_noise_density(this, "PN_P"),
_pn_v_noise_density(this, "PN_V"),
_pn_b_noise_density(this, "PN_B"),
_pn_t_noise_density(this, "PN_T"),
_t_max_grade(this, "T_MAX_GRADE"),
// landing target
_target_min_cov(this, "LT_COV"),
_target_mode(this, "LTEST_MODE", false),
// init origin
_fake_origin(this, "FAKE_ORIGIN"),
_init_origin_lat(this, "LAT"),
_init_origin_lon(this, "LON"),
// flow gyro
_flow_gyro_x_high_pass(this, "FGYRO_HP"),
_flow_gyro_y_high_pass(this, "FGYRO_HP"),
// stats
_baroStats(this, ""),
_sonarStats(this, ""),
_lidarStats(this, ""),
_flowQStats(this, ""),
_visionStats(this, ""),
_mocapStats(this, ""),
_gpsStats(this, ""),
// low pass
_xLowPass(this, "X_LP"),
// use same lp constant for agl
_aglLowPass(this, "X_LP"),
// delay
_xDelay(this, ""),
_tDelay(this, ""),
// misc
_polls(),
_timeStamp(hrt_absolute_time()),
_time_origin(0),
_timeStampLastBaro(hrt_absolute_time()),
_time_last_hist(0),
_time_last_flow(0),
_time_last_baro(0),
_time_last_gps(0),
_time_last_lidar(0),
_time_last_sonar(0),
_time_init_sonar(0),
_time_last_vision_p(0),
_time_last_mocap(0),
_time_last_land(0),
_time_last_target(0),
// reference altitudes
_altOrigin(0),
_altOriginInitialized(false),
_altOriginGlobal(false),
_baroAltOrigin(0),
_gpsAltOrigin(0),
// status
_receivedGps(false),
_lastArmedState(false),
// masks
_sensorTimeout(UINT16_MAX),
_sensorFault(0),
_estimatorInitialized(0),
// sensor update flags
_flowUpdated(false),
_gpsUpdated(false),
_visionUpdated(false),
_mocapUpdated(false),
_lidarUpdated(false),
_sonarUpdated(false),
_landUpdated(false),
_baroUpdated(false)
{
// assign distance subs to array
_dist_subs[0] = &_sub_dist0;
_dist_subs[1] = &_sub_dist1;
_dist_subs[2] = &_sub_dist2;
_dist_subs[3] = &_sub_dist3;
// setup event triggering based on new flow messages to integrate
_polls[POLL_FLOW].fd = _sub_flow.getHandle();
_polls[POLL_FLOW].events = POLLIN;
_polls[POLL_PARAM].fd = _sub_param_update.getHandle();
_polls[POLL_PARAM].events = POLLIN;
_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
_polls[POLL_SENSORS].events = POLLIN;
// initialize A, B, P, x, u
_x.setZero();
_u.setZero();
initSS();
// map
_map_ref.init_done = false;
// intialize parameter dependent matrices
updateParams();
// print fusion settings to console
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
(_fusion.get() & FUSE_GPS) != 0,
(_fusion.get() & FUSE_FLOW) != 0,
(_fusion.get() & FUSE_VIS_POS) != 0,
(_fusion.get() & FUSE_LAND_TARGET) != 0,
(_fusion.get() & FUSE_LAND) != 0,
(_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_fusion.get() & FUSE_BARO) != 0);
}
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// this block has no parent, and has name LPE
SuperBlock(nullptr, "LPE"),
ModuleParams(nullptr),
// subscriptions, set rate, add to list
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
// set flow max update rate higher than expected to we don't lose packets
_sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()),
// main prediction loop, 100 hz
_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
// status updates 2 hz
_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
// gps 10 hz
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
// vision 50 hz
_sub_vision_pos(ORB_ID(vehicle_vision_position), 1000 / 50, 0, &getSubscriptions()),
// mocap 50 hz
_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 50, 0, &getSubscriptions()),
// all distance sensors, 10 hz
_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
_dist_subs(),
_sub_lidar(nullptr),
_sub_sonar(nullptr),
_sub_landing_target_pose(ORB_ID(landing_target_pose), 1000 / 40, 0, &getSubscriptions()),
_sub_airdata(ORB_ID(vehicle_air_data), 0, 0, &getSubscriptions()),
// publications
_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
_pub_innov(ORB_ID(ekf2_innovations), -1, &getPublications()),
// map projection
_map_ref(),
// flow gyro
_flow_gyro_x_high_pass(this, "FGYRO_HP"),
_flow_gyro_y_high_pass(this, "FGYRO_HP"),
// stats
_baroStats(this, ""),
_sonarStats(this, ""),
_lidarStats(this, ""),
_flowQStats(this, ""),
_visionStats(this, ""),
_mocapStats(this, ""),
_gpsStats(this, ""),
// low pass
_xLowPass(this, "X_LP"),
// use same lp constant for agl
_aglLowPass(this, "X_LP"),
// delay
_xDelay(this, ""),
_tDelay(this, ""),
// misc
_polls(),
_timeStamp(hrt_absolute_time()),
_time_origin(0),
_timeStampLastBaro(hrt_absolute_time()),
_time_last_hist(0),
_time_last_flow(0),
_time_last_baro(0),
_time_last_gps(0),
_time_last_lidar(0),
_time_last_sonar(0),
_time_init_sonar(0),
_time_last_vision_p(0),
_time_last_mocap(0),
_time_last_land(0),
_time_last_target(0),
// reference altitudes
_altOrigin(0),
_altOriginInitialized(false),
_altOriginGlobal(false),
_baroAltOrigin(0),
_gpsAltOrigin(0),
// status
_receivedGps(false),
_lastArmedState(false),
// masks
_sensorTimeout(UINT16_MAX),
_sensorFault(0),
_estimatorInitialized(0),
// sensor update flags
_flowUpdated(false),
_gpsUpdated(false),
_visionUpdated(false),
_mocapUpdated(false),
_lidarUpdated(false),
_sonarUpdated(false),
_landUpdated(false),
_baroUpdated(false)
{
// assign distance subs to array
_dist_subs[0] = &_sub_dist0;
_dist_subs[1] = &_sub_dist1;
_dist_subs[2] = &_sub_dist2;
_dist_subs[3] = &_sub_dist3;
// setup event triggering based on new flow messages to integrate
_polls[POLL_FLOW].fd = _sub_flow.getHandle();
_polls[POLL_FLOW].events = POLLIN;
_polls[POLL_PARAM].fd = _sub_param_update.getHandle();
_polls[POLL_PARAM].events = POLLIN;
_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
_polls[POLL_SENSORS].events = POLLIN;
// initialize A, B, P, x, u
_x.setZero();
_u.setZero();
initSS();
// map
_map_ref.init_done = false;
// print fusion settings to console
printf("[lpe] fuse gps: %d, flow: %d, vis_pos: %d, "
"landing_target: %d, land: %d, pub_agl_z: %d, flow_gyro: %d, "
"baro: %d\n",
(_fusion.get() & FUSE_GPS) != 0,
(_fusion.get() & FUSE_FLOW) != 0,
(_fusion.get() & FUSE_VIS_POS) != 0,
(_fusion.get() & FUSE_LAND_TARGET) != 0,
(_fusion.get() & FUSE_LAND) != 0,
(_fusion.get() & FUSE_PUB_AGL_Z) != 0,
(_fusion.get() & FUSE_FLOW_GYRO_COMP) != 0,
(_fusion.get() & FUSE_BARO) != 0);
}
BlockLocalPositionEstimator::BlockLocalPositionEstimator() :
// this block has no parent, and has name LPE
SuperBlock(NULL, "LPE"),
// subscriptions, set rate, add to list
_sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()),
_sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()),
// flow 10 hz
_sub_flow(ORB_ID(optical_flow), 1000 / 10, 0, &getSubscriptions()),
// main prediction loop, 100 hz
_sub_sensor(ORB_ID(sensor_combined), 1000 / 100, 0, &getSubscriptions()),
// status updates 2 hz
_sub_param_update(ORB_ID(parameter_update), 1000 / 2, 0, &getSubscriptions()),
_sub_manual(ORB_ID(manual_control_setpoint), 1000 / 2, 0, &getSubscriptions()),
_sub_home(ORB_ID(home_position), 1000 / 2, 0, &getSubscriptions()),
// gps 10 hz
_sub_gps(ORB_ID(vehicle_gps_position), 1000 / 10, 0, &getSubscriptions()),
// vision 5 hz
_sub_vision_pos(ORB_ID(vision_position_estimate), 1000 / 5, 0, &getSubscriptions()),
// all distance sensors, 10 hz
_sub_mocap(ORB_ID(att_pos_mocap), 1000 / 10, 0, &getSubscriptions()),
_sub_dist0(ORB_ID(distance_sensor), 1000 / 10, 0, &getSubscriptions()),
_sub_dist1(ORB_ID(distance_sensor), 1000 / 10, 1, &getSubscriptions()),
_sub_dist2(ORB_ID(distance_sensor), 1000 / 10, 2, &getSubscriptions()),
_sub_dist3(ORB_ID(distance_sensor), 1000 / 10, 3, &getSubscriptions()),
_dist_subs(),
_sub_lidar(NULL),
_sub_sonar(NULL),
// publications
_pub_lpos(ORB_ID(vehicle_local_position), -1, &getPublications()),
_pub_gpos(ORB_ID(vehicle_global_position), -1, &getPublications()),
_pub_est_status(ORB_ID(estimator_status), -1, &getPublications()),
// map projection
_map_ref(),
// block parameters
_xy_pub_thresh(this, "XY_PUB"),
_z_pub_thresh(this, "Z_PUB"),
_sonar_z_stddev(this, "SNR_Z"),
_sonar_z_offset(this, "SNR_OFF_Z"),
_lidar_z_stddev(this, "LDR_Z"),
_lidar_z_offset(this, "LDR_OFF_Z"),
_accel_xy_stddev(this, "ACC_XY"),
_accel_z_stddev(this, "ACC_Z"),
_baro_stddev(this, "BAR_Z"),
_gps_on(this, "GPS_ON"),
_gps_delay(this, "GPS_DELAY"),
_gps_xy_stddev(this, "GPS_XY"),
_gps_z_stddev(this, "GPS_Z"),
_gps_vxy_stddev(this, "GPS_VXY"),
_gps_vz_stddev(this, "GPS_VZ"),
_gps_eph_max(this, "EPH_MAX"),
_gps_epv_max(this, "EPV_MAX"),
_vision_xy_stddev(this, "VIS_XY"),
_vision_z_stddev(this, "VIS_Z"),
_vision_on(this, "VIS_ON"),
_mocap_p_stddev(this, "VIC_P"),
_flow_z_offset(this, "FLW_OFF_Z"),
_flow_xy_stddev(this, "FLW_XY"),
//_flow_board_x_offs(NULL, "SENS_FLW_XOFF"),
//_flow_board_y_offs(NULL, "SENS_FLW_YOFF"),
_flow_min_q(this, "FLW_QMIN"),
_pn_p_noise_density(this, "PN_P"),
_pn_v_noise_density(this, "PN_V"),
_pn_b_noise_density(this, "PN_B"),
_pn_t_noise_density(this, "PN_T"),
// init home
_init_home_lat(this, "LAT"),
_init_home_lon(this, "LON"),
// flow gyro
_flow_gyro_x_high_pass(this, "FGYRO_HP"),
_flow_gyro_y_high_pass(this, "FGYRO_HP"),
// stats
_baroStats(this, ""),
_sonarStats(this, ""),
_lidarStats(this, ""),
_flowQStats(this, ""),
_visionStats(this, ""),
_mocapStats(this, ""),
_gpsStats(this, ""),
// low pass
_xLowPass(this, "X_LP"),
// delay
_xDelay(this, ""),
_tDelay(this, ""),
// misc
_polls(),
_timeStamp(hrt_absolute_time()),
_time_last_hist(0),
_time_last_xy(0),
_time_last_z(0),
_time_last_tz(0),
_time_last_flow(0),
_time_last_baro(0),
_time_last_gps(0),
_time_last_lidar(0),
_time_last_sonar(0),
_time_init_sonar(0),
_time_last_vision_p(0),
_time_last_mocap(0),
// initialization flags
_receivedGps(false),
_baroInitialized(false),
_gpsInitialized(false),
_lidarInitialized(false),
_sonarInitialized(false),
_flowInitialized(false),
_visionInitialized(false),
_mocapInitialized(false),
// reference altitudes
_altHome(0),
_altHomeInitialized(false),
_baroAltHome(0),
_gpsAltHome(0),
_visionHome(),
_mocapHome(),
// flow integration
_flowX(0),
_flowY(0),
_flowMeanQual(0),
// status
_validXY(false),
_validZ(false),
_validTZ(false),
_xyTimeout(true),
_zTimeout(true),
_tzTimeout(true),
_lastArmedState(false),
// faults
_baroFault(FAULT_NONE),
_gpsFault(FAULT_NONE),
_lidarFault(FAULT_NONE),
_flowFault(FAULT_NONE),
_sonarFault(FAULT_NONE),
_visionFault(FAULT_NONE),
_mocapFault(FAULT_NONE),
// loop performance
_loop_perf(),
_interval_perf(),
_err_perf(),
// kf matrices
_x(), _u(), _P()
{
// assign distance subs to array
_dist_subs[0] = &_sub_dist0;
_dist_subs[1] = &_sub_dist1;
_dist_subs[2] = &_sub_dist2;
_dist_subs[3] = &_sub_dist3;
// setup event triggering based on new flow messages to integrate
_polls[POLL_FLOW].fd = _sub_flow.getHandle();
_polls[POLL_FLOW].events = POLLIN;
_polls[POLL_PARAM].fd = _sub_param_update.getHandle();
_polls[POLL_PARAM].events = POLLIN;
_polls[POLL_SENSORS].fd = _sub_sensor.getHandle();
_polls[POLL_SENSORS].events = POLLIN;
// initialize A, B, P, x, u
_x.setZero();
_u.setZero();
_flowX = 0;
_flowY = 0;
initSS();
// perf counters
_loop_perf = perf_alloc(PC_ELAPSED,
"local_position_estimator_runtime");
//_interval_perf = perf_alloc(PC_INTERVAL,
//"local_position_estimator_interval");
_err_perf = perf_alloc(PC_COUNT, "local_position_estimator_err");
// map
_map_ref.init_done = false;
// intialize parameter dependent matrices
updateParams();
}
