MyDateTime::MyDateTime(std::string tz, std::string & fmt , std::string & str ):
pss(0),
_ldt(boost::local_time::not_a_date_time, boost::shared_ptr<boost::local_time::posix_time_zone> (new boost::local_time::posix_time_zone(tz)) )
{
initSS();
setFormatedtime(fmt, str);
}
MyDateTime::MyDateTime(std::string tz, std::time_t & t):
pss(0),
_ldt(boost::posix_time::from_time_t(t),
boost::shared_ptr<boost::local_time::posix_time_zone> (new boost::local_time::posix_time_zone(tz)) )
{
initSS();
}
MyDateTime::MyDateTime():
pss(0),
_ldt(boost::posix_time::not_a_date_time)
{
initSS();
}
uint8_t W5100Class::init(void)
{
static bool initialized = false;
uint8_t i;
if (initialized) return 1;
// Many Ethernet shields have a CAT811 or similar reset chip
// connected to W5100 or W5200 chips. The W5200 will not work at
// all, and may even drive its MISO pin, until given an active low
// reset pulse! The CAT811 has a 240 ms typical pulse length, and
// a 400 ms worst case maximum pulse length. MAX811 has a worst
// case maximum 560 ms pulse length. This delay is meant to wait
// until the reset pulse is ended. If your hardware has a shorter
// reset time, this can be edited or removed.
delay(560);
//Serial.println("w5100 init");
SPI.begin();
initSS();
resetSS();
SPI.beginTransaction(SPI_ETHERNET_SETTINGS);
// Attempt W5200 detection first, because W5200 does not properly
// reset its SPI state when CS goes high (inactive). Communication
// from detecting the other chips can leave the W5200 in a state
// where it won't recover, unless given a reset pulse.
if (isW5200()) {
CH_BASE_MSB = 0x40;
#ifdef ETHERNET_LARGE_BUFFERS
#if MAX_SOCK_NUM <= 1
SSIZE = 16384;
#elif MAX_SOCK_NUM <= 2
SSIZE = 8192;
#elif MAX_SOCK_NUM <= 4
SSIZE = 4096;
#else
SSIZE = 2048;
#endif
SMASK = SSIZE - 1;
#endif
for (i=0; i<MAX_SOCK_NUM; i++) {
writeSnRX_SIZE(i, SSIZE >> 10);
writeSnTX_SIZE(i, SSIZE >> 10);
}
for (; i<8; i++) {
writeSnRX_SIZE(i, 0);
writeSnTX_SIZE(i, 0);
}
// Try W5500 next. Wiznet finally seems to have implemented
// SPI well with this chip. It appears to be very resilient,
// so try it after the fragile W5200
} else if (isW5500()) {
uint8_t W5100Class::init(void)
{
uint16_t TXBUF_BASE, RXBUF_BASE;
uint8_t i;
delay(200);
//Serial.println("w5100 init");
#ifdef USE_SPIFIFO
SPIFIFO.begin(W5200_SS_PIN, SPI_CLOCK_12MHz); // W5100 is 14 MHz max
#else
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV2);
initSS();
#endif
if (isW5100()) {
CH_BASE = 0x0400;
SSIZE = 2048;
SMASK = 0x07FF;
TXBUF_BASE = 0x4000;
RXBUF_BASE = 0x6000;
writeTMSR(0x55);
writeRMSR(0x55);
} else if (isW5200()) {
#ifdef USE_SPIFIFO
//SPIFIFO.begin(W5200_SS_PIN, SPI_CLOCK_24MHz); // W5200 is 33 MHz max
#endif
CH_BASE = 0x4000;
SSIZE = 4096;
SMASK = 0x0FFF;
TXBUF_BASE = 0x8000;
RXBUF_BASE = 0xC000;
for (i=0; i<MAX_SOCK_NUM; i++) {
writeSnRX_SIZE(i, SSIZE >> 10);
writeSnTX_SIZE(i, SSIZE >> 10);
}
for (; i<8; i++) {
writeSnRX_SIZE(i, 0);
writeSnTX_SIZE(i, 0);
}
} else {
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();
}
MyDateTime::MyDateTime(std::string tz):
pss(0),
_ldt(boost::posix_time::second_clock::local_time(), boost::shared_ptr<boost::local_time::posix_time_zone> (new boost::local_time::posix_time_zone(tz)) )
{
initSS();
}
