mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-23 02:02:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.8.8 release

Browse Source
This commit is contained in:
Vehicle Researcher
2021-08-22 22:13:11 -07:00
parent 444aace15f
commit baffaeee93
193 changed files with 60566 additions and 1933 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/calibrationd.py
+2 -2
View File
@@ -31,7 +31,7 @@ INPUTS_WANTED = 50 # We want a little bit more than we need for stability
MAX_ALLOWED_SPREAD = np.radians(2)
RPY_INIT = np.array([0.0,0.0,0.0])
# These values are needed to accomodate biggest modelframe
# These values are needed to accommodate biggest modelframe
PITCH_LIMITS = np.array([-0.09074112085129739, 0.14907572052989657])
YAW_LIMITS = np.array([-0.06912048084718224, 0.06912048084718235])
DEBUG = os.getenv("DEBUG") is not None
@@ -115,7 +115,7 @@ class Calibrator():
self.cal_status = Calibration.INVALID
# If spread is too high, assume mounting was changed and reset to last block.
# Make the transition smooth. Abrupt transistion are not good foor feedback loop through supercombo model.
# Make the transition smooth. Abrupt transitions are not good foor feedback loop through supercombo model.
if max(self.calib_spread) > MAX_ALLOWED_SPREAD and self.cal_status == Calibration.CALIBRATED:
self.reset(self.rpys[self.block_idx - 1], valid_blocks=INPUTS_NEEDED, smooth_from=self.rpy)
selfdrive/locationd/locationd.cc
+5 -2
View File
@@ -82,8 +82,9 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
//fix_pos_geo_std = np.abs(coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo)
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = quat2rot(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
VectorXd calibrated_orientation_ecef = rot2euler(this->calib_from_device * device_from_ecef);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
//VectorXd calibrated_orientation_ecef = rot2euler(device_from_ecef);
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
@@ -115,6 +116,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
//orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
//ned_vel_std = self.converter->ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter->ecef2ned(fix_ecef + vel_ecef)
@@ -137,6 +139,7 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, true);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
selfdrive/locationd/ublox_msg.cc
+1 -1
View File
@@ -117,7 +117,7 @@ std::pair<std::string, kj::Array<capnp::word>> UbloxMsgParser::gen_msg() {
return {"ubloxGnss", gen_mon_hw2(static_cast<ubx_t::mon_hw2_t*>(body))};
break;
default:
LOGE("Unkown message type %x", ubx_message.msg_type());
LOGE("Unknown message type %x", ubx_message.msg_type());
return {"ubloxGnss", kj::Array<capnp::word>()};
break;
}