Source code for morse.sensors.airspeed
import logging
logger = logging.getLogger("morse." + __name__)
from morse.core.sensor import Sensor
from morse.helpers.components import add_data, add_property
from morse.helpers.velocity import linear_velocities
from copy import copy
from math import pow
AIR_DENSITY = 1.225 # kg / m³
[docs]class Airspeed(Sensor):
"""
Sensor to compute Airspeed
- https://en.wikipedia.org/wiki/Airspeed
The current implementation is only correct for speed < 1Mach
"""
_name = "Airspeed"
_short_desc = "Mesure airspeed"
add_data('diff_pressure', 0.0, "float", 'Diff pressure in Pa')
add_property('_type', 'Automatic', 'ComputationMode', 'string',
"Kind of computation, can be one of ['Velocity', 'Position']. "
"Only robot with dynamic and Velocity control can choose Velocity "
"computation. Default choice is Velocity for robot with physics, "
"and Position for others")
def __init__(self, obj, parent=None):
""" Constructor method.
"""
logger.info('%s initialization' % obj.name)
# Call the constructor of the parent class
Sensor.__init__(self, obj, parent)
has_physics = bool(self.robot_parent.bge_object.getPhysicsId())
if self._type == 'Automatic':
if has_physics:
self._type = 'Velocity'
else:
self._type = 'Position'
if self._type == 'Velocity' and not has_physics:
logger.error("Invalid configuration : Velocity computation without "
"physics")
return
if self._type == 'Velocity':
self.robot_vel_body = self.robot_parent.bge_object.localLinearVelocity
self.airspeed2body = self.sensor_to_robot_position_3d()
# rotate vector from body to airspeed frame
self.rot_b2a = self.airspeed2body.rotation.conjugated()
else:
self.pp = copy(self.position_3d)
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
[docs] def default_action(self):
if self._type == 'Velocity':
vel = self.rot_b2a * self.robot_vel_body
else:
vel = linear_velocities(self.pp, self.position_3d, 1 / self.frequency)
self.pp = copy(self.position_3d)
v_x = vel[0]
self.local_data['diff_pressure'] = 0.5 * AIR_DENSITY * v_x * v_x