diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c index 7a5b5f20c3..883ca05ee1 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adapt.c @@ -171,7 +171,7 @@ void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) } /* Update State */ - gv_adapt_X = gv_adapt_X + (((int64_t)(K * residual)) >> K_FRAC); + gv_adapt_X = gv_adapt_X + ((((int64_t)K * residual)) >> K_FRAC); /* Output bounds. * Don't let it climb over a value that would diff --git a/sw/airborne/math/pprz_algebra_int.c b/sw/airborne/math/pprz_algebra_int.c index d1f2373be8..b855c56d8c 100644 --- a/sw/airborne/math/pprz_algebra_int.c +++ b/sw/airborne/math/pprz_algebra_int.c @@ -353,12 +353,12 @@ void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, struct I void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in) { const int64_t _2qi2_m1 = ((q->qi * q->qi) >> (INT32_QUAT_FRAC - 1)) - QUAT1_BFP_OF_REAL(1); - const int64_t _2qx2 = (q->qx * q->qx) >> (INT32_QUAT_FRAC - 1); - const int64_t _2qy2 = (q->qy * q->qy) >> (INT32_QUAT_FRAC - 1); - const int64_t _2qz2 = (q->qz * q->qz) >> (INT32_QUAT_FRAC - 1); - const int64_t _2qiqx = (q->qi * q->qx) >> (INT32_QUAT_FRAC - 1); - const int64_t _2qiqy = (q->qi * q->qy) >> (INT32_QUAT_FRAC - 1); - const int64_t _2qiqz = (q->qi * q->qz) >> (INT32_QUAT_FRAC - 1); + const int64_t _2qx2 = ((int64_t const)q->qx * q->qx) >> (INT32_QUAT_FRAC - 1); + const int64_t _2qy2 = ((int64_t const)q->qy * q->qy) >> (INT32_QUAT_FRAC - 1); + const int64_t _2qz2 = ((int64_t const)q->qz * q->qz) >> (INT32_QUAT_FRAC - 1); + const int64_t _2qiqx = ((int64_t const)q->qi * q->qx) >> (INT32_QUAT_FRAC - 1); + const int64_t _2qiqy = ((int64_t const)q->qi * q->qy) >> (INT32_QUAT_FRAC - 1); + const int64_t _2qiqz = ((int64_t const)q->qi * q->qz) >> (INT32_QUAT_FRAC - 1); const int64_t m01 = ((q->qx * q->qy) >> (INT32_QUAT_FRAC - 1)) + _2qiqz; const int64_t m02 = ((q->qx * q->qz) >> (INT32_QUAT_FRAC - 1)) - _2qiqy; const int64_t m12 = ((q->qy * q->qz) >> (INT32_QUAT_FRAC - 1)) + _2qiqx; @@ -425,16 +425,18 @@ void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r) const int32_t two_qi_two = TRIG_BFP_OF_REAL(1.) + tr; uint32_t two_qi = int32_sqrt(two_qi_two << INT32_TRIG_FRAC); two_qi = two_qi << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); - q->qi = two_qi / 2; - q->qx = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qi; - q->qy = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qi; - q->qz = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qi; + if (two_qi != 0) { + q->qi = two_qi / 2; + q->qx = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qi; + q->qy = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qi; + q->qz = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qi; + } } else { if (RMAT_ELMT(*r, 0, 0) > RMAT_ELMT(*r, 1, 1) && RMAT_ELMT(*r, 0, 0) > RMAT_ELMT(*r, 2, 2)) { @@ -442,46 +444,52 @@ void int32_quat_of_rmat(struct Int32Quat *q, struct Int32RMat *r) - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.); uint32_t two_qx = int32_sqrt(two_qx_two << INT32_TRIG_FRAC); two_qx = two_qx << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); - q->qi = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qx; - q->qx = two_qx / 2; - q->qy = ((RMAT_ELMT(*r, 0, 1) + RMAT_ELMT(*r, 1, 0)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qx; - q->qz = ((RMAT_ELMT(*r, 2, 0) + RMAT_ELMT(*r, 0, 2)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qx; + if (two_qx != 0) { + q->qi = ((RMAT_ELMT(*r, 1, 2) - RMAT_ELMT(*r, 2, 1)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qx; + q->qx = two_qx / 2; + q->qy = ((RMAT_ELMT(*r, 0, 1) + RMAT_ELMT(*r, 1, 0)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qx; + q->qz = ((RMAT_ELMT(*r, 2, 0) + RMAT_ELMT(*r, 0, 2)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qx; + } } else if (RMAT_ELMT(*r, 1, 1) > RMAT_ELMT(*r, 2, 2)) { const int32_t two_qy_two = RMAT_ELMT(*r, 1, 1) - RMAT_ELMT(*r, 0, 0) - RMAT_ELMT(*r, 2, 2) + TRIG_BFP_OF_REAL(1.); uint32_t two_qy = int32_sqrt(two_qy_two << INT32_TRIG_FRAC); two_qy = two_qy << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); - q->qi = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qy; - q->qx = ((RMAT_ELMT(*r, 0, 1) + RMAT_ELMT(*r, 1, 0)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qy; - q->qy = two_qy / 2; - q->qz = ((RMAT_ELMT(*r, 1, 2) + RMAT_ELMT(*r, 2, 1)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qy; + if (two_qy != 0) { + q->qi = ((RMAT_ELMT(*r, 2, 0) - RMAT_ELMT(*r, 0, 2)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qy; + q->qx = ((RMAT_ELMT(*r, 0, 1) + RMAT_ELMT(*r, 1, 0)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qy; + q->qy = two_qy / 2; + q->qz = ((RMAT_ELMT(*r, 1, 2) + RMAT_ELMT(*r, 2, 1)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qy; + } } else { const int32_t two_qz_two = RMAT_ELMT(*r, 2, 2) - RMAT_ELMT(*r, 0, 0) - RMAT_ELMT(*r, 1, 1) + TRIG_BFP_OF_REAL(1.); uint32_t two_qz = int32_sqrt(two_qz_two << INT32_TRIG_FRAC); two_qz = two_qz << (INT32_QUAT_FRAC - INT32_TRIG_FRAC); - q->qi = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qz; - q->qx = ((RMAT_ELMT(*r, 2, 0) + RMAT_ELMT(*r, 0, 2)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qz; - q->qy = ((RMAT_ELMT(*r, 1, 2) + RMAT_ELMT(*r, 2, 1)) << - (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) - / two_qz; - q->qz = two_qz / 2; + if (two_qz != 0) { + q->qi = ((RMAT_ELMT(*r, 0, 1) - RMAT_ELMT(*r, 1, 0)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qz; + q->qx = ((RMAT_ELMT(*r, 2, 0) + RMAT_ELMT(*r, 0, 2)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qz; + q->qy = ((RMAT_ELMT(*r, 1, 2) + RMAT_ELMT(*r, 2, 1)) << + (INT32_QUAT_FRAC - INT32_TRIG_FRAC + INT32_QUAT_FRAC - 1)) + / two_qz; + q->qz = two_qz / 2; + } } } } diff --git a/sw/simulator/nps/nps_flightgear.c b/sw/simulator/nps/nps_flightgear.c index 42dcf7f89b..d7331b5ec4 100644 --- a/sw/simulator/nps/nps_flightgear.c +++ b/sw/simulator/nps/nps_flightgear.c @@ -61,8 +61,15 @@ void nps_flightgear_init(const char *host, unsigned int port, unsigned int port if (port_in > 0) { struct sockaddr_in addr_in; flightgear.socket_in = socket(PF_INET, SOCK_DGRAM, pte->p_proto); - setsockopt(flightgear.socket_in, SOL_SOCKET, SO_REUSEADDR, - &so_reuseaddr, sizeof(so_reuseaddr)); + if (flightgear.socket_in < 0) { + perror("nps_flightgear_init socket()"); + exit(errno); + } + if ( setsockopt(flightgear.socket_in, SOL_SOCKET, SO_REUSEADDR, + &so_reuseaddr, sizeof(so_reuseaddr)) == -1) { + perror("nps_flightgear_init setsockopt()"); + exit(errno); + } addr_in.sin_family = PF_INET; addr_in.sin_port = htons(port_in); addr_in.sin_addr.s_addr = htonl(INADDR_ANY); diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index 01b15a5741..e716bc14df 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main.c @@ -369,7 +369,7 @@ static bool nps_main_parse_options(int argc, char **argv) nps_main.host_time_factor = atof(optarg); break; case 8: nps_main.fg_fdm = 1; break; - case 9: + case 9: nps_main.fg_port_in = atoi(optarg); break; } break; @@ -383,7 +383,7 @@ static bool nps_main_parse_options(int argc, char **argv) fprintf(stderr, usage, argv[0]); exit(0); - default: /* ’?’ */ + default: printf("?? getopt returned character code 0%o ??\n", c); fprintf(stderr, usage, argv[0]); exit(EXIT_FAILURE); diff --git a/sw/simulator/nps/nps_radio_control_spektrum.c b/sw/simulator/nps/nps_radio_control_spektrum.c index d065a8a1fe..a84db7319b 100644 --- a/sw/simulator/nps/nps_radio_control_spektrum.c +++ b/sw/simulator/nps/nps_radio_control_spektrum.c @@ -38,9 +38,13 @@ int nps_radio_control_spektrum_init(const char *device) termios.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IUCLC | IXON | IXANY | IXOFF | IMAXBEL); termios.c_iflag |= IGNPAR; + + termios.c_cflag = 0; // properly initialize variable /* control modes*/ termios.c_cflag &= ~(CSIZE | PARENB | CRTSCTS | PARODD | HUPCL); termios.c_cflag |= CREAD | CS8 | CSTOPB | CLOCAL; + + termios.c_lflag = 0; // properly initialize variable /* local modes */ termios.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | FLUSHO | PENDIN); termios.c_lflag |= NOFLSH;