diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index e9dd533644..f89f470e2f 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -377,20 +377,22 @@ void Ekf2::task_main() orb_check(_optical_flow_sub, &optical_flow_updated); - if(optical_flow_updated) { + if (optical_flow_updated) { orb_copy(ORB_ID(optical_flow), _optical_flow_sub, &optical_flow); } orb_check(_range_finder_sub, &range_finder_updated); - if(range_finder_updated) { + if (range_finder_updated) { orb_copy(ORB_ID(distance_sensor), _range_finder_sub, &range_finder); } // in replay mode we are getting the actual timestamp from the sensor topic hrt_abstime now = 0; + if (_replay_mode) { now = sensors.timestamp; + } else { now = hrt_absolute_time(); } @@ -434,7 +436,7 @@ void Ekf2::task_main() _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); } - if(optical_flow_updated) { + if (optical_flow_updated) { flow_message flow; flow.flowdata(0) = optical_flow.pixel_flow_x_integral; flow.flowdata(1) = optical_flow.pixel_flow_y_integral; @@ -442,12 +444,13 @@ void Ekf2::task_main() flow.gyrodata(0) = optical_flow.gyro_x_rate_integral; flow.gyrodata(1) = optical_flow.gyro_y_rate_integral; flow.dt = optical_flow.integration_timespan; - if(!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) { + + if (!isnan(optical_flow.pixel_flow_y_integral) && !isnan(optical_flow.pixel_flow_x_integral)) { _ekf->setOpticalFlowData(optical_flow.timestamp, &flow); } } - if(range_finder_updated) { + if (range_finder_updated) { _ekf->setRangeData(range_finder.timestamp, &range_finder.current_distance); } @@ -461,156 +464,155 @@ void Ekf2::task_main() _ekf->set_arm_status(status.arming_state & vehicle_status_s::ARMING_STATE_ARMED); } - // run the EKF update - _ekf->update(); - - // generate vehicle attitude data - struct vehicle_attitude_s att = {}; - att.timestamp = hrt_absolute_time(); - - _ekf->copy_quaternion(att.q); - matrix::Quaternion q(att.q[0], att.q[1], att.q[2], att.q[3]); - matrix::Euler euler(q); - att.roll = euler(0); - att.pitch = euler(1); - att.yaw = euler(2); - - // generate vehicle local position data - struct vehicle_local_position_s lpos = {}; - float pos[3] = {}; - float vel[3] = {}; - - lpos.timestamp = hrt_absolute_time(); - - // Position in local NED frame - _ekf->copy_position(pos); - lpos.x = pos[0]; - lpos.y = pos[1]; - lpos.z = pos[2]; - - // Velocity in NED frame (m/s) - _ekf->copy_velocity(vel); - lpos.vx = vel[0]; - lpos.vy = vel[1]; - lpos.vz = vel[2]; - - // TODO: better status reporting - lpos.xy_valid = _ekf->local_position_is_valid(); - lpos.z_valid = true; - lpos.v_xy_valid = _ekf->local_position_is_valid(); - lpos.v_z_valid = true; - - // Position of local NED origin in GPS / WGS84 frame - struct map_projection_reference_s ekf_origin = {}; - _ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) - lpos.xy_global = _ekf->global_position_is_valid(); - lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt) - lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees - lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees - - // The rotation of the tangent plane vs. geographical north - lpos.yaw = att.yaw; - - float terrain_vpos; - lpos.dist_bottom_valid = _ekf->get_terrain_vert_pos(&terrain_vpos); - lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters - lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate - lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found - - // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres - // TODO: Should use sqrt of filter position variances - // get pos vel state variance - Vector3f pos_var, vel_var; - _ekf->get_pos_var(pos_var); - _ekf->get_vel_var(vel_var); - lpos.eph = sqrt(pos_var(0)+pos_var(1)); - lpos.epv = sqrt(pos_var(2)); - - // publish vehicle local position data - if (_lpos_pub == nullptr) { - _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); + // run the EKF update and output + if (_ekf->update()) { - } else { - orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); - } + // generate vehicle attitude quaternion data + struct vehicle_attitude_s att = {}; + _ekf->copy_quaternion(att.q); + matrix::Quaternion q(att.q[0], att.q[1], att.q[2], att.q[3]); - // generate control state data - control_state_s ctrl_state = {}; - ctrl_state.timestamp = hrt_absolute_time(); - ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); - ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); - ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); + // generate control state data + control_state_s ctrl_state = {}; + ctrl_state.timestamp = hrt_absolute_time(); + ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]); + ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]); - ctrl_state.q[0] = q(0); - ctrl_state.q[1] = q(1); - ctrl_state.q[2] = q(2); - ctrl_state.q[3] = q(3); + ctrl_state.q[0] = q(0); + ctrl_state.q[1] = q(1); + ctrl_state.q[2] = q(2); + ctrl_state.q[3] = q(3); - // publish control state data - if (_control_state_pub == nullptr) { - _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); + // publish control state data + if (_control_state_pub == nullptr) { + _control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); - } else { - orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); - } + } else { + orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state); + } - // generate vehicle attitude data - att.q[0] = q(0); - att.q[1] = q(1); - att.q[2] = q(2); - att.q[3] = q(3); - att.q_valid = true; - att.rollspeed = sensors.gyro_rad_s[0]; - att.pitchspeed = sensors.gyro_rad_s[1]; - att.yawspeed = sensors.gyro_rad_s[2]; + // generate remaining vehicle attitude data + att.timestamp = hrt_absolute_time(); + matrix::Euler euler(q); + att.roll = euler(0); + att.pitch = euler(1); + att.yaw = euler(2); - // publish vehicle attitude data - if (_att_pub == nullptr) { - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); + att.q[0] = q(0); + att.q[1] = q(1); + att.q[2] = q(2); + att.q[3] = q(3); + att.q_valid = true; - } else { - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); - } + att.rollspeed = sensors.gyro_rad_s[0]; + att.pitchspeed = sensors.gyro_rad_s[1]; + att.yawspeed = sensors.gyro_rad_s[2]; + + // publish vehicle attitude data + if (_att_pub == nullptr) { + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - // generate and publish global position data - struct vehicle_global_position_s global_pos = {}; + } else { + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); + } + + // generate vehicle local position data + struct vehicle_local_position_s lpos = {}; + float pos[3] = {}; + float vel[3] = {}; + + lpos.timestamp = hrt_absolute_time(); + + // Position in local NED frame + _ekf->copy_position(pos); + lpos.x = pos[0]; + lpos.y = pos[1]; + lpos.z = pos[2]; + + // Velocity in NED frame (m/s) + _ekf->copy_velocity(vel); + lpos.vx = vel[0]; + lpos.vy = vel[1]; + lpos.vz = vel[2]; + + // TODO: better status reporting + lpos.xy_valid = _ekf->local_position_is_valid(); + lpos.z_valid = true; + lpos.v_xy_valid = _ekf->local_position_is_valid(); + lpos.v_z_valid = true; + + // Position of local NED origin in GPS / WGS84 frame + struct map_projection_reference_s ekf_origin = {}; + // true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) + _ekf->get_ekf_origin(&lpos.ref_timestamp, &ekf_origin, &lpos.ref_alt); + lpos.xy_global = _ekf->global_position_is_valid(); + lpos.z_global = true; // true if z is valid and has valid global reference (ref_alt) + lpos.ref_lat = ekf_origin.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees + lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees + + // The rotation of the tangent plane vs. geographical north + lpos.yaw = att.yaw; + + float terrain_vpos; + lpos.dist_bottom_valid = _ekf->get_terrain_vert_pos(&terrain_vpos); + lpos.dist_bottom = terrain_vpos - pos[2]; // Distance to bottom surface (ground) in meters + lpos.dist_bottom_rate = -vel[2]; // Distance to bottom surface (ground) change rate + lpos.surface_bottom_timestamp = hrt_absolute_time(); // Time when new bottom surface found + + // TODO: uORB definition does not define what these variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres + Vector3f pos_var, vel_var; + _ekf->get_pos_var(pos_var); + _ekf->get_vel_var(vel_var); + lpos.eph = sqrt(pos_var(0) + pos_var(1)); + lpos.epv = sqrt(pos_var(2)); + + // publish vehicle local position data + if (_lpos_pub == nullptr) { + _lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos); - if (_ekf->global_position_is_valid()) { - // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator + } else { + orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos); + } - global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start - global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds + // generate and publish global position data + struct vehicle_global_position_s global_pos = {}; - double est_lat, est_lon; - map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon); - global_pos.lat = est_lat; // Latitude in degrees - global_pos.lon = est_lon; // Longitude in degrees + if (_ekf->global_position_is_valid()) { + global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start + global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds - global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters + double est_lat, est_lon; + map_projection_reproject(&ekf_origin, lpos.x, lpos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; // Latitude in degrees + global_pos.lon = est_lon; // Longitude in degrees - global_pos.vel_n = vel[0]; // Ground north velocity, m/s - global_pos.vel_e = vel[1]; // Ground east velocity, m/s - global_pos.vel_d = vel[2]; // Ground downside velocity, m/s + global_pos.alt = -pos[2] + lpos.ref_alt; // Altitude AMSL in meters - global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. + global_pos.vel_n = vel[0]; // Ground north velocity, m/s + global_pos.vel_e = vel[1]; // Ground east velocity, m/s + global_pos.vel_d = vel[2]; // Ground downside velocity, m/s - global_pos.eph = sqrt(pos_var(0)+pos_var(1));; // Standard deviation of position estimate horizontally - global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically + global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. - // TODO: implement terrain estimator - global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 - global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid - // TODO use innovatun consistency check timouts to set this - global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning + global_pos.eph = sqrt(pos_var(0) + pos_var(1));; // Standard deviation of position estimate horizontally + global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically - global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) + // TODO: implement terrain estimator + global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 + global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid + // TODO use innovatun consistency check timouts to set this + global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning - if (_vehicle_global_position_pub == nullptr) { - _vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m) - } else { - orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); + if (_vehicle_global_position_pub == nullptr) { + _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); + } } } @@ -642,6 +644,7 @@ void Ekf2::task_main() _ekf->get_heading_innov_var(&innovations.heading_innov_var); _ekf->get_flow_innov_var(&innovations.flow_innov_var[0]); _ekf->get_hagl_innov_var(&innovations.hagl_innov_var); + if (_estimator_innovations_pub == nullptr) { _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); @@ -658,6 +661,7 @@ void Ekf2::task_main() // publish replay message if in replay mode bool publish_replay_message = (bool)_param_record_replay_msg->get(); + if (publish_replay_message) { struct ekf2_replay_s replay = {}; replay.time_ref = now; @@ -666,7 +670,8 @@ void Ekf2::task_main() replay.magnetometer_timestamp = sensors.magnetometer_timestamp[0]; replay.baro_timestamp = sensors.baro_timestamp[0]; memcpy(&replay.gyro_integral_rad[0], &sensors.gyro_integral_rad[0], sizeof(replay.gyro_integral_rad)); - memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], sizeof(replay.accelerometer_integral_m_s)); + memcpy(&replay.accelerometer_integral_m_s[0], &sensors.accelerometer_integral_m_s[0], + sizeof(replay.accelerometer_integral_m_s)); memcpy(&replay.magnetometer_ga[0], &sensors.magnetometer_ga[0], sizeof(replay.magnetometer_ga)); replay.baro_alt_meter = sensors.baro_alt_meter[0]; @@ -687,6 +692,7 @@ void Ekf2::task_main() replay.vel_e_m_s = gps.vel_e_m_s; replay.vel_d_m_s = gps.vel_d_m_s; replay.vel_ned_valid = gps.vel_ned_valid; + } else { // this will tell the logging app not to bother logging any gps replay data replay.time_usec = 0; @@ -700,6 +706,7 @@ void Ekf2::task_main() replay.flow_gyro_integral[1] = optical_flow.gyro_y_rate_integral; replay.flow_time_integral = optical_flow.integration_timespan; replay.flow_quality = optical_flow.quality; + } else { replay.flow_timestamp = 0; } @@ -707,6 +714,7 @@ void Ekf2::task_main() if (range_finder_updated) { replay.rng_timestamp = range_finder.timestamp; replay.range_to_ground = range_finder.current_distance; + } else { replay.rng_timestamp = 0; }