semin ahn / Mbed OS zeta_stm_kinetic

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BufferedSerial

ros_lib_kinetic/duration.cpp@3:a4677501ae87, 2021-11-30 (annotated)

Committer:
_seminahn
Date:
Tue Nov 30 08:13:05 2021 +0000
Revision:
3:a4677501ae87
Parent:
0:4ff8aeb3e4d1 
v1.2.5, change imu freq

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_seminahn 0:4ff8aeb3e4d1 1 /*
_seminahn 0:4ff8aeb3e4d1 2 * Software License Agreement (BSD License)
_seminahn 0:4ff8aeb3e4d1 3 *
_seminahn 0:4ff8aeb3e4d1 4 * Copyright (c) 2011, Willow Garage, Inc.
_seminahn 0:4ff8aeb3e4d1 5 * All rights reserved.
_seminahn 0:4ff8aeb3e4d1 6 *
_seminahn 0:4ff8aeb3e4d1 7 * Redistribution and use in source and binary forms, with or without
_seminahn 0:4ff8aeb3e4d1 8 * modification, are permitted provided that the following conditions
_seminahn 0:4ff8aeb3e4d1 9 * are met:
_seminahn 0:4ff8aeb3e4d1 10 *
_seminahn 0:4ff8aeb3e4d1 11 * * Redistributions of source code must retain the above copyright
_seminahn 0:4ff8aeb3e4d1 12 * notice, this list of conditions and the following disclaimer.
_seminahn 0:4ff8aeb3e4d1 13 * * Redistributions in binary form must reproduce the above
_seminahn 0:4ff8aeb3e4d1 14 * copyright notice, this list of conditions and the following
_seminahn 0:4ff8aeb3e4d1 15 * disclaimer in the documentation and/or other materials provided
_seminahn 0:4ff8aeb3e4d1 16 * with the distribution.
_seminahn 0:4ff8aeb3e4d1 17 * * Neither the name of Willow Garage, Inc. nor the names of its
_seminahn 0:4ff8aeb3e4d1 18 * contributors may be used to endorse or promote prducts derived
_seminahn 0:4ff8aeb3e4d1 19 * from this software without specific prior written permission.
_seminahn 0:4ff8aeb3e4d1 20 *
_seminahn 0:4ff8aeb3e4d1 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
_seminahn 0:4ff8aeb3e4d1 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
_seminahn 0:4ff8aeb3e4d1 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
_seminahn 0:4ff8aeb3e4d1 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
_seminahn 0:4ff8aeb3e4d1 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
_seminahn 0:4ff8aeb3e4d1 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
_seminahn 0:4ff8aeb3e4d1 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
_seminahn 0:4ff8aeb3e4d1 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
_seminahn 0:4ff8aeb3e4d1 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
_seminahn 0:4ff8aeb3e4d1 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
_seminahn 0:4ff8aeb3e4d1 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
_seminahn 0:4ff8aeb3e4d1 32 * POSSIBILITY OF SUCH DAMAGE.
_seminahn 0:4ff8aeb3e4d1 33 */
_seminahn 0:4ff8aeb3e4d1 34
_seminahn 0:4ff8aeb3e4d1 35 #include <math.h>
_seminahn 0:4ff8aeb3e4d1 36 #include "ros/duration.h"
_seminahn 0:4ff8aeb3e4d1 37
_seminahn 0:4ff8aeb3e4d1 38 namespace ros
_seminahn 0:4ff8aeb3e4d1 39 {
_seminahn 0:4ff8aeb3e4d1 40 void normalizeSecNSecSigned(int32_t &sec, int32_t &nsec)
_seminahn 0:4ff8aeb3e4d1 41 {
_seminahn 0:4ff8aeb3e4d1 42 int32_t nsec_part = nsec;
_seminahn 0:4ff8aeb3e4d1 43 int32_t sec_part = sec;
_seminahn 0:4ff8aeb3e4d1 44
_seminahn 0:4ff8aeb3e4d1 45 while (nsec_part > 1000000000L)
_seminahn 0:4ff8aeb3e4d1 46 {
_seminahn 0:4ff8aeb3e4d1 47 nsec_part -= 1000000000L;
_seminahn 0:4ff8aeb3e4d1 48 ++sec_part;
_seminahn 0:4ff8aeb3e4d1 49 }
_seminahn 0:4ff8aeb3e4d1 50 while (nsec_part < 0)
_seminahn 0:4ff8aeb3e4d1 51 {
_seminahn 0:4ff8aeb3e4d1 52 nsec_part += 1000000000L;
_seminahn 0:4ff8aeb3e4d1 53 --sec_part;
_seminahn 0:4ff8aeb3e4d1 54 }
_seminahn 0:4ff8aeb3e4d1 55 sec = sec_part;
_seminahn 0:4ff8aeb3e4d1 56 nsec = nsec_part;
_seminahn 0:4ff8aeb3e4d1 57 }
_seminahn 0:4ff8aeb3e4d1 58
_seminahn 0:4ff8aeb3e4d1 59 Duration& Duration::operator+=(const Duration &rhs)
_seminahn 0:4ff8aeb3e4d1 60 {
_seminahn 0:4ff8aeb3e4d1 61 sec += rhs.sec;
_seminahn 0:4ff8aeb3e4d1 62 nsec += rhs.nsec;
_seminahn 0:4ff8aeb3e4d1 63 normalizeSecNSecSigned(sec, nsec);
_seminahn 0:4ff8aeb3e4d1 64 return *this;
_seminahn 0:4ff8aeb3e4d1 65 }
_seminahn 0:4ff8aeb3e4d1 66
_seminahn 0:4ff8aeb3e4d1 67 Duration& Duration::operator-=(const Duration &rhs){
_seminahn 0:4ff8aeb3e4d1 68 sec += -rhs.sec;
_seminahn 0:4ff8aeb3e4d1 69 nsec += -rhs.nsec;
_seminahn 0:4ff8aeb3e4d1 70 normalizeSecNSecSigned(sec, nsec);
_seminahn 0:4ff8aeb3e4d1 71 return *this;
_seminahn 0:4ff8aeb3e4d1 72 }
_seminahn 0:4ff8aeb3e4d1 73
_seminahn 0:4ff8aeb3e4d1 74 Duration& Duration::operator*=(double scale){
_seminahn 0:4ff8aeb3e4d1 75 sec *= scale;
_seminahn 0:4ff8aeb3e4d1 76 nsec *= scale;
_seminahn 0:4ff8aeb3e4d1 77 normalizeSecNSecSigned(sec, nsec);
_seminahn 0:4ff8aeb3e4d1 78 return *this;
_seminahn 0:4ff8aeb3e4d1 79 }
_seminahn 0:4ff8aeb3e4d1 80
_seminahn 0:4ff8aeb3e4d1 81 }