Merge pull request #244 from RI-SE/feature_distanceBasedTrigger

Feature distance based trigger

conf/triggeraction.conf

@@ -1,2 +1,2 @@
-#trigger_ip;trigger_index;trigger_type[parameter];action_type[parameter];action[parameter];
-10.168.15.58;3;DI[RISING_EDGE];SERVER[127.0.0.1];SEND_START[2000];
+#trigger_ip;trigger_type[parameter];action_ip;action_type[parameter];
+127.0.0.1;DISTANCE[TO:(x:5.3y:2.5z:2.1),LESS_THAN,5.0];0.0.0.0;TEST_SCENARIO_COMMAND[START,0];

core/inc/iso22133.h

@@ -138,6 +138,7 @@ typedef enum {
 	ACTION_PARAMETER_Y                  = 0x00000071,
 	ACTION_PARAMETER_Z                  = 0x00000072,
 	ACTION_PARAMETER_VS_BRAKE_WARNING   = 0xA0000000,
+	ACTION_PARAMETER_VS_SEND_START		= 0xA0000100,
 	ACTION_PARAMETER_UNAVAILABLE        = 0xFFFFFFFF
 } ActionTypeParameter_t;
 

core/inc/util.h

@@ -74,11 +74,6 @@ extern "C"{
 
 #define MAX_UTIL_VARIBLE_SIZE 512
 
-// TODO: Make these constants have more descriptive names
-#define a	6378137.0							            //meters in WGS84
-#define k	298.257223563 						        //in WGS84, f = 1/298.257223563
-#define b	6356752.3142451794975639665996337	//b = (1-f)*a
-#define l	1e-12
 #define PI	3.141592653589793
 #define ORIGO_DISTANCE_CALC_ITERATIONS 14
 #define TRAJECTORY_LINE_LENGTH 100

core/src/util.c

@@ -36,8 +36,11 @@
   -- Defines
   ------------------------------------------------------------*/
 
-#define FE_WGS84        (1.0/298.257223563)	// earth flattening (WGS84)
-#define RE_WGS84        6378137.0	// earth semimajor axis (WGS84) (m)
+#define EARTH_EQUATOR_RADIUS_M	6378137.0	// earth semimajor axis (WGS84) (m)
+#define INVERSE_FLATTENING	298.257223563	//in WGS84, f = 1/298.257223563
+#define EARTH_FLATTENING ( 1.0 / INVERSE_FLATTENING )
+#define EARTH_POLE_RADIUS_M	6356752.3142451794975639665996337	//b = (1-f)*a
+#define VINCENTY_MIN_STEP_TOLERANCE	1e-12
 
 #define SMALL_BUFFER_SIZE_128 128
 #define SMALL_BUFFER_SIZE_64 64
@@ -409,17 +412,17 @@ void util_error(const char *message) {
 }
 
 void xyzToLlh(double x, double y, double z, double *lat, double *lon, double *height) {
-	double e2 = FE_WGS84 * (2.0 - FE_WGS84);
+	double e2 = EARTH_FLATTENING * (2.0 - EARTH_FLATTENING);
 	double r2 = x * x + y * y;
 	double za = z;
 	double zk = 0.0;
 	double sinp = 0.0;
-	double v = RE_WGS84;
+	double v = EARTH_EQUATOR_RADIUS_M;
 
 	while (fabs(za - zk) >= 1E-4) {
 		zk = za;
 		sinp = za / sqrt(r2 + za * za);
-		v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
+		v = EARTH_EQUATOR_RADIUS_M / sqrt(1.0 - e2 * sinp * sinp);
 		za = z + v * e2 * sinp;
 	}
 
@@ -433,8 +436,8 @@ void llhToXyz(double lat, double lon, double height, double *x, double *y, doubl
 	double cosp = cos(lat * M_PI / 180.0);
 	double sinl = sin(lon * M_PI / 180.0);
 	double cosl = cos(lon * M_PI / 180.0);
-	double e2 = FE_WGS84 * (2.0 - FE_WGS84);
-	double v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);
+	double e2 = EARTH_FLATTENING * (2.0 - EARTH_FLATTENING);
+	double v = EARTH_EQUATOR_RADIUS_M / sqrt(1.0 - e2 * sinp * sinp);
 
 	*x = (v + height) * cosp * cosl;
 	*y = (v + height) * cosp * sinl;
@@ -796,7 +799,7 @@ int UtilStringToMonitorData(const char *monitorString, size_t stringLength, Moni
 uint8_t UtilIsPositionNearTarget(CartesianPosition position, CartesianPosition target, double tolerance_m) {
 	double distance = 0.0;
 
-	if (!position.isPositionValid || target.isPositionValid)
+	if (!position.isPositionValid || !target.isPositionValid)
 		return 0;
 	distance = sqrt(pow(position.xCoord_m - target.xCoord_m, 2)
 					+ pow(position.yCoord_m - target.yCoord_m, 2)
@@ -846,7 +849,7 @@ double UtilCalcPositionDelta(double P1Lat, double P1Long, double P2Lat, double P
 	P2LatRad = UtilDegToRad(P2Lat);
 	P2LongRad = UtilDegToRad(P2Long);
 
-	f = 1 / k;
+	f = 1 / INVERSE_FLATTENING;
 	U1 = atan((1 - f) * tan(P1LatRad));
 	U2 = atan((1 - f) * tan(P2LatRad));
 	L = P2LongRad - P1LongRad;
@@ -921,7 +924,7 @@ int UtilVincentyDirect(double refLat, double refLon, double a1, double distance,
 
 
 	// Variables only calculated once
-	double U1, f = 1 / k, sigma1, sina, pow2cosa, pow2u, A, B, C, L, lambda;
+	double U1, f = 1 / INVERSE_FLATTENING, sigma1, sina, pow2cosa, pow2u, A, B, C, L, lambda;
 
 	// Iterative variables
 	double sigma, deltaSigma, sigma2m;
@@ -935,14 +938,16 @@ int UtilVincentyDirect(double refLat, double refLon, double a1, double distance,
 	sina = cos(U1) * sin(a1);
 
 	pow2cosa = 1 - pow(sina, 2);
-	pow2u = pow2cosa * (pow(a, 2) - pow(b, 2)) / pow(b, 2);
+	pow2u =
+		pow2cosa * (pow(EARTH_EQUATOR_RADIUS_M, 2) - pow(EARTH_POLE_RADIUS_M, 2)) / pow(EARTH_POLE_RADIUS_M,
+																						2);
 
 	A = 1 + pow2u / 16384.0 * (4096.0 + pow2u * (-768.0 + pow2u * (320.0 - 175.0 * pow2u)));
 	B = pow2u / 1024.0 * (256.0 + pow2u * (-128.0 + pow2u * (74.0 - 47.0 * pow2u)));
 
 
 	int iterations = 0;
-	double init_sigma = distance / (b * A);
+	double init_sigma = distance / (EARTH_POLE_RADIUS_M * A);
 
 	sigma = init_sigma;
 	do {
@@ -960,7 +965,7 @@ int UtilVincentyDirect(double refLat, double refLon, double a1, double distance,
 							  )
 			);
 		sigma = init_sigma + deltaSigma;
-	} while (fabs(sigma - prev_sigma) > l);
+	} while (fabs(sigma - prev_sigma) > VINCENTY_MIN_STEP_TOLERANCE);
 
 
 	*resLat = UtilRadToDeg(atan2(sin(U1) * cos(sigma) + cos(U1) * sin(sigma) * cos(a1),
@@ -2840,7 +2845,7 @@ void traj2ldm(float time, double x, double y, double z, float hdg, float vel, mo
 
 	char pcTempBuffer[512];
 
-	double earth_radius = a;
+	double earth_radius = EARTH_EQUATOR_RADIUS_M;
 
 	double lat_origin = 0.0;
 	double lon_origin = 0.0;

modules/ScenarioControl/CMakeLists.txt

@@ -23,6 +23,7 @@ add_executable(${SCENARIO_CONTROL_TARGET}
 	${CMAKE_CURRENT_SOURCE_DIR}/src/action.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/booleantrigger.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/braketrigger.cpp
+	${CMAKE_CURRENT_SOURCE_DIR}/src/distancetrigger.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/causality.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/externalaction.cpp
 	${CMAKE_CURRENT_SOURCE_DIR}/src/isotrigger.cpp

modules/ScenarioControl/inc/action.h

@@ -63,6 +63,8 @@ class Action
     ActionReturnCode_t appendParameter(ActionParameter_t actionParameter);
     virtual ActionReturnCode_t appendParameter(std::string parameterString);
 
+	virtual ActionReturnCode_t parseParameters() = 0;
+
     static ActionTypeCode_t asTypeCode(const std::string &typeCodeString);
 
 protected:

modules/ScenarioControl/inc/distancetrigger.h

@@ -0,0 +1,55 @@
+#ifndef DISTANCETRIGGER_H
+#define DISTANCETRIGGER_H
+
+#include "booleantrigger.h"
+
+#include <set>
+#include <vector>
+#include <string>
+
+class DistanceTrigger : public BooleanTrigger
+{
+public:
+	DistanceTrigger(TriggerID_t triggerID);
+
+	TriggerReturnCode_t appendParameter(std::string inputStr) override;
+	TriggerReturnCode_t parseParameters() override;
+
+	using BooleanTrigger::update;
+	TriggerReturnCode_t update(MonitorDataType newValue) override;
+
+	void setTriggerDistance(double distance_m) { this->triggerDistance_m = distance_m; }
+	void setReferencePoint(CartesianPosition point) { this->referencePoint = point; }
+
+	double getTriggerDistance(void) const { return this->triggerDistance_m; }
+	CartesianPosition getReferencePoint(void) const { return this->referencePoint; }
+private:
+	static constexpr CartesianPosition defaultReferencePoint = {0.0, 0.0, 0.0, 0.0, true, false};
+
+	double triggerDistance_m;
+	CartesianPosition referencePoint;
+	enum {LESS_THAN, GREATER_THAN} oper = LESS_THAN;
+
+	const std::set<TriggerParameter_t> getAcceptedParameters() const override
+	{
+		std::set<TriggerParameter_t> accParams;
+		accParams.insert(TRIGGER_PARAMETER_X);
+		accParams.insert(TRIGGER_PARAMETER_Y);
+		accParams.insert(TRIGGER_PARAMETER_Z);
+		accParams.insert(TRIGGER_PARAMETER_EQUAL_TO);
+		accParams.insert(TRIGGER_PARAMETER_GREATER_THAN);
+		accParams.insert(TRIGGER_PARAMETER_GREATER_THAN_OR_EQUAL_TO);
+		accParams.insert(TRIGGER_PARAMETER_LESS_THAN);
+		accParams.insert(TRIGGER_PARAMETER_LESS_THAN_OR_EQUAL_TO);
+		accParams.insert(TRIGGER_PARAMETER_NOT_EQUAL_TO);
+		accParams.insert(TRIGGER_PARAMETER_RELATIVE);
+		accParams.insert(TRIGGER_PARAMETER_ABSOLUTE);
+		accParams.insert(TRIGGER_PARAMETER_MIN);
+		accParams.insert(TRIGGER_PARAMETER_MAX);
+		accParams.insert(TRIGGER_PARAMETER_MEAN);
+		return accParams;
+	}
+
+	TriggerReturnCode_t parseNumericParameter(std::string inputStr);
+};
+#endif // DISTANCETRIGGER_H

modules/ScenarioControl/inc/externalaction.h

@@ -10,13 +10,32 @@ class ExternalAction : public Action
     using Action::Action;
 
     ActionReturnCode_t execute(void) override;
+	ActionReturnCode_t parseParameters(void) override { return OK; }
+};
+
+class TestScenarioCommandAction : public ExternalAction {
+public:
+	TestScenarioCommandAction(ActionID_t actionID = 0, uint32_t allowedNumberOfRuns = 1);
+
+	ActionReturnCode_t appendParameter(std::string) override;
+	ActionReturnCode_t parseParameters(void) override { return parameters.size() == 1 ? OK : NOT_OK; }
+protected:
+	ActionParameter_t asParameterCode(const std::string &parameterCodeString) const override;
+private:
+	const std::set<ActionParameter_t> getAcceptedParameters(void) const override
+	{
+		std::set<ActionParameter_t> accParams;
+		accParams.insert(ACTION_PARAMETER_VS_SEND_START);
+		return accParams;
+	}
+	ActionReturnCode_t parseNumericParameter(std::string);
 };
 
 class InfrastructureAction : public ExternalAction
 {
 public:
     InfrastructureAction(ActionID_t actionID = 0, uint32_t allowedNumberOfRuns = 1);
-
+	ActionReturnCode_t parseParameters(void) override { return parameters.size() == 1 ? OK : NOT_OK; }
 protected:
     ActionParameter_t asParameterCode(const std::string &parameterCodeString) const;
 

modules/ScenarioControl/inc/scenario.h

@@ -5,7 +5,6 @@
 #include <fstream>
 
 #include "trigger.h"
-#include "braketrigger.h"
 #include "action.h"
 #include "causality.h"
 #include "logging.h"

modules/ScenarioControl/inc/trigger.h

@@ -87,12 +87,15 @@ class Trigger
     */
     virtual TriggerReturnCode_t update(void)    { throw std::invalid_argument("Invalid signal type"); }
     virtual TriggerReturnCode_t update(struct timeval)    { throw std::invalid_argument("Invalid signal type"); }
-    virtual TriggerReturnCode_t update(bool, struct timeval)    { throw std::invalid_argument("Invalid signal type"); }
-    virtual TriggerReturnCode_t update(char, struct timeval)    { throw std::invalid_argument("Invalid signal type"); }
-    virtual TriggerReturnCode_t update(int, struct timeval)     { throw std::invalid_argument("Invalid signal type"); }
-    virtual TriggerReturnCode_t update(float, struct timeval)   { throw std::invalid_argument("Invalid signal type"); }
-    virtual TriggerReturnCode_t update(double, struct timeval)  { throw std::invalid_argument("Invalid signal type"); }
-    virtual TriggerReturnCode_t update(TREOData){ throw std::invalid_argument("Invalid signal type"); }
+	virtual TriggerReturnCode_t update(bool, struct timeval)    { throw std::invalid_argument("Invalid signal type bool"); }
+	virtual TriggerReturnCode_t update(char, struct timeval)    { throw std::invalid_argument("Invalid signal type char"); }
+	virtual TriggerReturnCode_t update(int, struct timeval)     { throw std::invalid_argument("Invalid signal type int"); }
+	virtual TriggerReturnCode_t update(float, struct timeval)   { throw std::invalid_argument("Invalid signal type float"); }
+	virtual TriggerReturnCode_t update(double, struct timeval)  { throw std::invalid_argument("Invalid signal type double"); }
+	virtual TriggerReturnCode_t update(CartesianPosition, struct timeval) { throw std::invalid_argument("Invalid signal type cartesian position"); }
+	virtual TriggerReturnCode_t update(TREOData) { throw std::invalid_argument("Invalid signal type TREO data"); }
+	virtual TriggerReturnCode_t update(MonitorDataType) { throw std::invalid_argument("Invalid signal type monitor data"); }
+
 
     static TriggerTypeCode_t asTypeCode(std::string typeCodeString);
 protected: