diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 03c044982e..4a2cd3b146 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -166,8 +166,10 @@ class Calibrator: self.cal_status = log.LiveCalibrationData.Status.recalibrating write_this_cycle = (self.idx == 0) and (self.block_idx % (INPUTS_WANTED//5) == 5) - if self.param_put and write_this_cycle: - self.params.put_nonblocking("CalibrationParams", self.get_msg(True).to_bytes()) + if self.param_put and True:#write_this_cycle: + m = self.get_msg(True) + print(m) + self.params.put_nonblocking("CalibrationParams", m.to_bytes()) def handle_v_ego(self, v_ego: float) -> None: self.v_ego = v_ego diff --git a/selfdrive/locationd/lagd.py b/selfdrive/locationd/lagd.py index d7834f7f1f..a07a124f6d 100755 --- a/selfdrive/locationd/lagd.py +++ b/selfdrive/locationd/lagd.py @@ -201,7 +201,7 @@ class LateralLagEstimator: def get_msg(self, valid: bool, debug: bool = False) -> capnp._DynamicStructBuilder: msg = messaging.new_message('liveDelay') - msg.valid = valid + msg.valid = True # valid liveDelay = msg.liveDelay @@ -227,8 +227,8 @@ class LateralLagEstimator: liveDelay.lateralDelayEstimateStd = 0.0 liveDelay.validBlocks = self.block_avg.valid_blocks - liveDelay.calPerc = min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) // - (self.min_valid_block_count * self.block_size), 100) + liveDelay.calPerc = 99# min(100 * (self.block_avg.valid_blocks * self.block_size + self.block_avg.idx) // + # (self.min_valid_block_count * self.block_size), 100) if debug: liveDelay.points = self.block_avg.values.flatten().tolist() @@ -376,7 +376,7 @@ def main(): while True: sm.update() - if sm.all_checks(): + if sm.all_checks() or 1: for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): if sm.updated[which]: t = sm.logMonoTime[which] * 1e-9 @@ -387,8 +387,13 @@ def main(): if sm.frame % 5 == 0: lag_learner.update_estimate() lag_msg = lag_learner.get_msg(sm.all_checks(), DEBUG) + print(lag_msg) lag_msg_dat = lag_msg.to_bytes() pm.send('liveDelay', lag_msg_dat) - if sm.frame % 1200 == 0: # cache every 60 seconds + if True:#sm.frame % 1200 == 0: # cache every 60 seconds params.put_nonblocking("LiveDelay", lag_msg_dat) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index 3f9b846e82..456286530d 100755 --- a/selfdrive/locationd/torqued.py +++ b/selfdrive/locationd/torqued.py @@ -203,6 +203,7 @@ class TorqueEstimator(ParameterEstimator): self.all_torque_points.append([steer, lateral_acc]) def get_msg(self, valid=True, with_points=False): + valid = True msg = messaging.new_message('liveTorqueParameters') msg.valid = valid liveTorqueParameters = msg.liveTorqueParameters @@ -234,7 +235,7 @@ class TorqueEstimator(ParameterEstimator): liveTorqueParameters.latAccelOffsetFiltered = float(self.filtered_params['latAccelOffset'].x) liveTorqueParameters.frictionCoefficientFiltered = float(self.filtered_params['frictionCoefficient'].x) liveTorqueParameters.totalBucketPoints = len(self.filtered_points) - liveTorqueParameters.calPerc = self.filtered_points.get_valid_percent() + liveTorqueParameters.calPerc = 24 # self.filtered_points.get_valid_percent() liveTorqueParameters.decay = self.decay liveTorqueParameters.maxResets = self.resets return msg @@ -264,8 +265,9 @@ def main(demo=False): pm.send('liveTorqueParameters', estimator.get_msg(valid=sm.all_checks(), with_points=DEBUG)) # Cache points every 60 seconds while onroad - if sm.frame % 240 == 0: + if True:#sm.frame % 240 == 0: msg = estimator.get_msg(valid=sm.all_checks(), with_points=True) + print(msg) params.put_nonblocking("LiveTorqueParameters", msg.to_bytes())