diff --git a/libraries/AP_GPS/AP_GPS_406.cpp b/libraries/AP_GPS/AP_GPS_406.cpp index 6c557e7fe0..54bd45e0b1 100644 --- a/libraries/AP_GPS/AP_GPS_406.cpp +++ b/libraries/AP_GPS/AP_GPS_406.cpp @@ -45,11 +45,11 @@ AP_GPS_406::_configure_gps(void) const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1}; const uint8_t gps_ender[] = {0xB0, 0xB3}; - for(int z = 0; z < 2; z++) { - for(int x = 0; x < 5; x++) { + for(int16_t z = 0; z < 2; z++) { + for(int16_t x = 0; x < 5; x++) { _port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg.. _port->write(gps_payload[x]); // Prints the payload, is not the same for every msg - for(int y = 0; y < 6; y++) // Prints 6 zeros + for(int16_t y = 0; y < 6; y++) // Prints 6 zeros _port->write((uint8_t)0); _port->write(gps_checksum[x]); // Print the Checksum _port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's. diff --git a/libraries/AP_GPS/AP_GPS_IMU.cpp b/libraries/AP_GPS/AP_GPS_IMU.cpp index 8156ae2d0e..e51621a29d 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.cpp +++ b/libraries/AP_GPS/AP_GPS_IMU.cpp @@ -17,11 +17,11 @@ update() : Call this funcion as often as you want to ensure you read the incomming gps data Properties: - lattitude : lattitude * 10000000 (long value) - longitude : longitude * 10000000 (long value) - altitude : altitude * 100 (meters) (long value) - ground_speed : Speed (m/s) * 100 (long value) - ground_course : Course (degrees) * 100 (long value) + lattitude : lattitude * 10000000 (int32_t value) + longitude : longitude * 10000000 (int32_t value) + altitude : altitude * 100 (meters) (int32_t value) + ground_speed : Speed (m/s) * 100 (int32_t value) + ground_course : Course (degrees) * 100 (int32_t value) new_data : 1 when a new data is received. You need to write a 0 to new_data when you read the data fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix. @@ -56,12 +56,12 @@ bool AP_GPS_IMU::read(void) { byte data; - int numc = 0; + int16_t numc = 0; numc = _port->available(); if (numc > 0) { - for (int i=0; iread(); @@ -169,30 +169,30 @@ void AP_GPS_IMU::join_data(void) //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. //Storing IMU roll - roll_sensor = *(int *)&buffer[0]; + roll_sensor = *(int16_t *)&buffer[0]; //Storing IMU pitch - pitch_sensor = *(int *)&buffer[2]; + pitch_sensor = *(int16_t *)&buffer[2]; //Storing IMU heading (yaw) - ground_course = *(int *)&buffer[4]; + ground_course = *(int16_t *)&buffer[4]; imu_ok = true; } void AP_GPS_IMU::join_data_xplane() { //Storing IMU roll - roll_sensor = *(int *)&buffer[0]; + roll_sensor = *(int16_t *)&buffer[0]; //Storing IMU pitch - pitch_sensor = *(int *)&buffer[2]; + pitch_sensor = *(int16_t *)&buffer[2]; //Storing IMU heading (yaw) - ground_course = *(unsigned int *)&buffer[4]; + ground_course = *(uint16_t *)&buffer[4]; //Storing airspeed - airspeed = *(int *)&buffer[6]; + airspeed = *(int16_t *)&buffer[6]; imu_ok = true; @@ -200,17 +200,17 @@ void AP_GPS_IMU::join_data_xplane() void AP_GPS_IMU::GPS_join_data(void) { - longitude = *(long *)&buffer[0]; // degrees * 10e7 - latitude = *(long *)&buffer[4]; + longitude = *(int32_t *)&buffer[0]; // degrees * 10e7 + latitude = *(int32_t *)&buffer[4]; //Storing GPS Height above the sea level - altitude = (long)*(int *)&buffer[8] * 10; + altitude = (int32_t)*(int16_t *)&buffer[8] * 10; //Storing Speed - speed_3d = ground_speed = (float)*(int *)&buffer[10]; + speed_3d = ground_speed = (float)*(int16_t *)&buffer[10]; //We skip the gps ground course because we use yaw value from the IMU for ground course - time = *(long *)&buffer[14]; + time = *(int32_t *)&buffer[14]; imu_health = buffer[15]; @@ -234,5 +234,5 @@ void AP_GPS_IMU::checksum(unsigned char data) /**************************************************************** * Unused ****************************************************************/ -void AP_GPS_IMU::setHIL(long _time, float _latitude, float _longitude, float _altitude, +void AP_GPS_IMU::setHIL(uint32_t _time, float _latitude, float _longitude, float _altitude, float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {}; diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h index bfc2d0a9c3..a50bce01bf 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.h +++ b/libraries/AP_GPS/AP_GPS_IMU.h @@ -15,14 +15,14 @@ public: virtual bool read(void); // Properties - long roll_sensor; // how much we're turning in deg * 100 - long pitch_sensor; // our angle of attack in deg * 100 - int airspeed; + int32_t roll_sensor; // how much we're turning in deg * 100 + int32_t pitch_sensor; // our angle of attack in deg * 100 + int16_t airspeed; float imu_health; uint8_t imu_ok; // Unused - virtual void setHIL(long time, float latitude, float longitude, float altitude, + virtual void setHIL(uint32_t time, float latitude, float longitude, float altitude, float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); private: diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 3f93105cd1..817940309f 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -52,11 +52,11 @@ bool AP_GPS_MTK::read(void) { uint8_t data; - int numc; + int16_t numc; bool parsed = false; numc = _port->available(); - for (int i = 0; i < numc; i++) { // Process bytes received + for (int16_t i = 0; i < numc; i++) { // Process bytes received // read the next byte data = _port->read(); @@ -138,8 +138,8 @@ restart: num_sats = _buffer.msg.satellites; // time from gps is UTC, but convert here to msToD - long time_utc = _swapl(&_buffer.msg.utc_time); - long temp = (time_utc/10000000); + int32_t time_utc = _swapl(&_buffer.msg.utc_time); + int32_t temp = (time_utc/10000000); time_utc -= temp*10000000; time = temp * 3600000; temp = (time_utc/100000); diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index d4b4728d8d..cdd637e390 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp @@ -59,11 +59,11 @@ bool AP_GPS_MTK16::read(void) { uint8_t data; - int numc; + int16_t numc; bool parsed = false; numc = _port->available(); - for (int i = 0; i < numc; i++) { // Process bytes received + for (int16_t i = 0; i < numc; i++) { // Process bytes received // read the next byte data = _port->read(); @@ -136,8 +136,8 @@ restart: date = _buffer.msg.utc_date; // time from gps is UTC, but convert here to msToD - long time_utc = _buffer.msg.utc_time; - long temp = (time_utc/10000000); + int32_t time_utc = _buffer.msg.utc_time; + int32_t temp = (time_utc/10000000); time_utc -= temp*10000000; time = temp * 3600000; temp = (time_utc/100000); @@ -157,11 +157,11 @@ restart: /* Waiting on clarification of MAVLink protocol! if(!_offset_calculated && parsed) { - long tempd1 = date; - long day = tempd1/10000; + int32_t tempd1 = date; + int32_t day = tempd1/10000; tempd1 -= day * 10000; - long month = tempd1/100; - long year = tempd1 - month * 100; + int32_t month = tempd1/100; + int32_t year = tempd1 - month * 100; _time_offset = _calc_epoch_offset(day, month, year); _epoch = UNIX_EPOCH; _offset_calculated = TRUE; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 77a64bdff9..08d17212c0 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -115,7 +115,7 @@ void AP_GPS_NMEA::init(enum GPS_Engine_Setting nav_setting) bool AP_GPS_NMEA::read(void) { - int numc; + int16_t numc; bool parsed = false; numc = _port->available(); @@ -167,7 +167,7 @@ bool AP_GPS_NMEA::_decode(char c) // // internal utilities // -int AP_GPS_NMEA::_from_hex(char a) +int16_t AP_GPS_NMEA::_from_hex(char a) { if (a >= 'A' && a <= 'F') return a - 'A' + 10; @@ -197,7 +197,7 @@ uint32_t AP_GPS_NMEA::_parse_degrees() { char *p, *q; uint8_t deg = 0, min = 0; - unsigned int frac_min = 0; + uint16_t frac_min = 0; int32_t ret = 0; // scan for decimal point or end of field @@ -224,7 +224,7 @@ uint32_t AP_GPS_NMEA::_parse_degrees() // ten-thousandths of a minute if (*p == '.') { q = p + 1; - for (int i = 0; i < 5; i++) { + for (int16_t i = 0; i < 5; i++) { frac_min = (int32_t)(frac_min * 10); if (isdigit(*q)) frac_min += *q++ - '0'; diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 735341b401..c31b1f59ae 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -88,7 +88,7 @@ private: /// @param a The character to be converted /// @returns The value of the character as a hex digit /// - int _from_hex(char a); + int16_t _from_hex(char a); /// Parses the current term as a NMEA-style decimal number with /// up to two decimal digits. @@ -129,14 +129,14 @@ private: // The result of parsing terms within a message is stored temporarily until // the message is completely processed and the checksum validated. // This avoids the need to buffer the entire message. - long _new_time; ///< time parsed from a term - long _new_date; ///< date parsed from a term - long _new_latitude; ///< latitude parsed from a term - long _new_longitude; ///< longitude parsed from a term - long _new_altitude; ///< altitude parsed from a term - long _new_speed; ///< speed parsed from a term - long _new_course; ///< course parsed from a term - int _new_hdop; ///< HDOP parsed from a term + int32_t _new_time; ///< time parsed from a term + int32_t _new_date; ///< date parsed from a term + int32_t _new_latitude; ///< latitude parsed from a term + int32_t _new_longitude; ///< longitude parsed from a term + int32_t _new_altitude; ///< altitude parsed from a term + int32_t _new_speed; ///< speed parsed from a term + int32_t _new_course; ///< course parsed from a term + int16_t _new_hdop; ///< HDOP parsed from a term uint8_t _new_satellite_count; ///< satellite count parsed from a term /// @name Init strings diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index 32250d663c..94d4601c10 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -56,7 +56,7 @@ bool AP_GPS_SIRF::read(void) { uint8_t data; - int numc; + int16_t numc; bool parsed = false; numc = _port->available(); diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 82734dae4a..4e74e9aaeb 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -63,11 +63,11 @@ bool AP_GPS_UBLOX::read(void) { uint8_t data; - int numc; + int16_t numc; bool parsed = false; numc = _port->available(); - for (int i = 0; i < numc; i++) { // Process bytes received + for (int16_t i = 0; i < numc; i++) { // Process bytes received // read the next byte data = _port->read(); diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 42a6627af0..d935a260ab 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -25,7 +25,7 @@ public: /// void update(void); - void (*callback)(unsigned long t); + void (*callback)(uint32_t t); /// GPS status codes /// @@ -161,7 +161,7 @@ protected: /// long in the wrong byte order /// @returns endian-swapped value /// - long _swapl(const void *bytes); + int32_t _swapl(const void *bytes); /// perform an endian swap on an int /// @@ -215,12 +215,12 @@ private: float _velocity_east; }; -inline long +inline int32_t GPS::_swapl(const void *bytes) { const uint8_t *b = (const uint8_t *)bytes; union { - long v; + int32_t v; uint8_t b[4]; } u;