TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

CAM longitudinal acceleration

This commit is contained in:
emanuel 2021-04-06 00:27:13 +01:00
parent 6b2577c631
commit 6a98a26fc5
1 changed files with 16 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/cam.c
View File

@ -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) {