diff --git a/src/cam.c b/src/cam.c index d1b087d..023813d 100644 --- a/src/cam.c +++ b/src/cam.c @@ -169,6 +169,22 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) ++lightship->pos_history_len; + // Acceleration + if (lightship->pos_history_len > 1) { + double delta_angle = (lightship->pos_history[0]->heading - lightship->pos_history[1]->heading) / 10.0; /* 1ยบ */ + double delta_speed = (lightship->pos_history[0]->speed - (lightship->pos_history[1]->speed * cos(delta_angle * M_PI/180))) / 10.0; /* 0.1 m/s */ + int16_t long_a = (int16_t) (delta_speed / (double) ((lightship->pos_history[0]->ts - lightship->pos_history[1]->ts) / 1000)); /* 0.1 m/s^2 */ + + if (long_a > 160) long_a = 160; + else if (long_a < -160) long_a = -160; + + bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = long_a; + bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + } else { + bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable; + bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable; + } + // Low frequency container if (now > lightship->last_cam_lfc + 500) {