Flarm linux

boards/px4/fmu-v2/default.cmake

@@ -119,8 +119,8 @@ px4_add_board(
 		#hwtest # Hardware test
 		#matlab_csv_serial
 		#position_estimator_inav
-		#px4_mavlink_debug # Tutorial code from https://px4.io/dev/debug_values
-		#px4_simple_app # Tutorial code from https://px4.io/dev/px4_simple_app
+		px4_mavlink_debug # Tutorial code from https://px4.io/dev/debug_values
+		px4_simple_app # Tutorial code from https://px4.io/dev/px4_simple_app
 		#rover_steering_control # Rover example app
 		#segway
 		#uuv_example_app

boards/px4/sitl/default.cmake

@@ -72,6 +72,8 @@ px4_add_board(
 		tune_control
 		ver
 
+
+
 	EXAMPLES
 		bottle_drop # OBC challenge
 		dyn_hello # dynamically loading modules example
@@ -83,6 +85,7 @@ px4_add_board(
 		px4_simple_app # Tutorial code from https://px4.io/dev/px4_simple_app
 		rover_steering_control # Rover example app
 		segway
+
 	)
 
 set(config_sitl_viewer jmavsim CACHE STRING "viewer for sitl")
@@ -100,3 +103,4 @@ if(REPLAY_FILE)
 endif()
 
 set(ENABLE_LOCKSTEP_SCHEDULER yes)
+

msg/vehicle_command.msg

@@ -11,6 +11,7 @@ uint16 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20		# Return to launch location |Empty
 uint16 VEHICLE_CMD_NAV_LAND = 21			# Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|
 uint16 VEHICLE_CMD_NAV_TAKEOFF = 22			# Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
 uint16 VEHICLE_CMD_NAV_PRECLAND = 23			# Attempt a precision landing
+uint16 VEHICLE_CMD_NAV_CONUS_AVOIDANCE = 24
 uint16 VEHICLE_CMD_DO_ORBIT = 34            # Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z |
 uint16 VEHICLE_CMD_NAV_ROI = 80				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
 uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81		# Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|

src/examples/px4_simple_app/CMakeLists.txt

@@ -31,10 +31,10 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE examples__px4_simple_app
-	MAIN px4_simple_app
-	STACK_MAIN 2000
-	SRCS
-		px4_simple_app.c
-	DEPENDS
-	)
+    MODULE examples__px4_simple_app
+    MAIN px4_simple_app
+    STACK_MAIN 2000
+    SRCS
+        px4_simple_app.c
+    DEPENDS
+    )

src/examples/px4_simple_app/px4_simple_app.c

@@ -51,79 +51,141 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 
+#include <uORB/topics/transponder_report.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_command.h>
+
+#include <drivers/drv_hrt.h>
+
+
 __EXPORT int px4_simple_app_main(int argc, char *argv[]);
 
+/*void fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
+                 float altitude_diff, float hor_velocity, float ver_velocity)
+{
+    double lat, lon;
+    waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, direction, distance, &lat,
+                       &lon);
+    float alt = get_global_position()->alt + altitude_diff;
+
+    // float vel_n = get_global_position()->vel_n;
+    // float vel_e = get_global_position()->vel_e;
+    // float vel_d = get_global_position()->vel_d;
+
+    transponder_report_s tr = {};
+    tr.timestamp = hrt_absolute_time();
+    tr.icao_address = 1234;
+    tr.lat = lat; // Latitude, expressed as degrees
+    tr.lon = lon; // Longitude, expressed as degrees
+    tr.altitude_type = 0;
+    tr.altitude = alt;
+    tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians
+    tr.hor_velocity	= hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s
+    tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up
+    strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign) - 1);
+    tr.callsign[sizeof(tr.callsign) - 1] = 0;
+    tr.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum
+    tr.tslc = 2; // Time since last communication in seconds
+    tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
+           transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
+           transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
+           transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; // Flags to indicate various statuses including valid data fields
+    tr.squawk = 6667;
+
+    orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &tr, transponder_report_s::ORB_QUEUE_LENGTH);
+    (void)orb_unadvertise(h);
+}*/
+
 int px4_simple_app_main(int argc, char *argv[])
 {
-	PX4_INFO("Hello Sky!");
-
-	/* subscribe to sensor_combined topic */
-	int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
-	/* limit the update rate to 5 Hz */
-	orb_set_interval(sensor_sub_fd, 200);
-
-	/* advertise attitude topic */
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
-
-	/* one could wait for multiple topics with this technique, just using one here */
-	px4_pollfd_struct_t fds[] = {
-		{ .fd = sensor_sub_fd,   .events = POLLIN },
-		/* there could be more file descriptors here, in the form like:
-		 * { .fd = other_sub_fd,   .events = POLLIN },
-		 */
-	};
-
-	int error_counter = 0;
-
-	for (int i = 0; i < 5; i++) {
-		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
-		int poll_ret = px4_poll(fds, 1, 1000);
-
-		/* handle the poll result */
-		if (poll_ret == 0) {
-			/* this means none of our providers is giving us data */
-			PX4_ERR("Got no data within a second");
-
-		} else if (poll_ret < 0) {
-			/* this is seriously bad - should be an emergency */
-			if (error_counter < 10 || error_counter % 50 == 0) {
-				/* use a counter to prevent flooding (and slowing us down) */
-				PX4_ERR("ERROR return value from poll(): %d", poll_ret);
-			}
-
-			error_counter++;
-
-		} else {
-
-			if (fds[0].revents & POLLIN) {
-				/* obtained data for the first file descriptor */
-				struct sensor_combined_s raw;
-				/* copy sensors raw data into local buffer */
-				orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
-				PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
-					 (double)raw.accelerometer_m_s2[0],
-					 (double)raw.accelerometer_m_s2[1],
-					 (double)raw.accelerometer_m_s2[2]);
-
-				/* set att and publish this information for other apps
-				 the following does not have any meaning, it's just an example
-				*/
-				att.q[0] = raw.accelerometer_m_s2[0];
-				att.q[1] = raw.accelerometer_m_s2[1];
-				att.q[2] = raw.accelerometer_m_s2[2];
-
-				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
-			}
-
-			/* there could be more file descriptors here, in the form like:
-			 * if (fds[1..n].revents & POLLIN) {}
-			 */
-		}
-	}
-
-	PX4_INFO("exiting");
-
-	return 0;
+   	 PX4_INFO("Hello Sky!");
+     PX4_WARN("This is the px4_simple_app!");
+
+    /* subscribe to topic */
+    int _traffic_sub = orb_subscribe(ORB_ID(transponder_report));
+   	struct transponder_report_s tr;
+	orb_copy(ORB_ID(transponder_report), _traffic_sub, &tr);
+
+    PX4_INFO("Transponder report:");
+    PX4_WARN("Lat, Lon,ICAO Add: %f5 %f5 %u", tr.lat, tr.lon, tr.icao_address);
+    PX4_WARN("Alt: %f %f %f %f", (double)tr.altitude, (double)tr.heading, (double)tr.hor_velocity, (double)tr.ver_velocity);
+	PX4_WARN("flags: %d %d %u %u", tr.flags, tr.squawk, tr.altitude_type, tr.emitter_type);
+	PX4_WARN("tslc:  %u", tr.tslc);
+
+    /* subscribe to topic */
+    int _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+    struct vehicle_global_position_s gl;
+    orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &gl);
+
+    PX4_INFO("vehicl_global_position:");
+    PX4_WARN("pos:  %f5 %f5", gl.lat, gl.lon);
+
+    /* subscribe to topic */
+    int _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+    struct vehicle_gps_position_s gps;
+    orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &gps);
+
+    PX4_INFO("vehicl_gps_position:");
+    PX4_WARN("pos:  %i %i", gps.lat, gps.lon);
+
+    //_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+
+
+
+
+
+
+    /*
+    //Navigator::fake_traffic("LX007", 100, 1.0f, -1.0f, 100.0f, 90.0f, 0.001f);//500
+    PX4_INFO("own new Faketraffic:");
+    struct transponder_report_s trnew = {};
+    trnew.timestamp = hrt_absolute_time();
+    trnew.icao_address = 1234;
+    trnew.lat = 47.0; // Latitude, expressed as degrees
+    trnew.lon = 8.0; // Longitude, expressed as degrees
+    trnew.altitude_type = 0;
+    trnew.altitude = 51.0f;
+    trnew.heading = 0.2f; //-atan2(vel_e, vel_n); // Course over ground in radians
+    trnew.hor_velocity	= 10.0f; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s
+    trnew.ver_velocity = 0.2f; //-vel_d; // The vertical velocity in m/s, positive is up
+    //strncpy(&trnew.callsign[0], callsign, sizeof(trnew.callsign) - 1);
+    //trnew.callsign[sizeof(trnew.callsign) - 1] = 0;
+    trnew.emitter_type = 0; // Type from ADSB_EMITTER_TYPE enum
+    trnew.tslc = 2; // Time since last communication in seconds
+    trnew.flags = 271;
+    trnew.squawk = 6667;
+
+    //advertise attitude topic
+    memset(&trnew, 0, sizeof(trnew));
+    //orb_advert_t h = orb_advertise_queue(ORB_ID(transponder_report), &trnew, transponder_report_s::ORB_QUEUE_LENGTH);
+    //(void)orb_unadvertise(h);
+
+    orb_advert_t h = orb_advertise(ORB_ID(transponder_report), &trnew);
+    orb_publish(ORB_ID(transponder_report), h, &trnew);*/
+
+
+
+
+
+
+
+
+
+	    /* advertise attitude topic */
+    struct vehicle_attitude_s att;
+    memset(&att, 0, sizeof(att));
+    orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);  
+
+	orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
+
+  //  PX4_WARN("U: %f %f, %f %f",U(0),U(1),(double)Vu(0),(double)Vu(1));
+   // PX4_WARN("I: %f %f, %f %f",U(0),I(1),(double)Vi(0),(double)Vi(1));
+
+    PX4_INFO("exiting");
+
+    return 0;
 }

src/modules/commander/Commander.cpp

@@ -42,6 +42,7 @@
  *			- State machines will be model driven
  */
 
+
 #include "Commander.hpp"
 
 /* commander module headers */
@@ -1056,7 +1057,10 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
-		/* ignore commands that are handled by other parts of the system */
+/*********************************************************************************************************************/
+    case vehicle_command_s::VEHICLE_CMD_NAV_CONUS_AVOIDANCE: //TK
+/*********************************************************************************************************************/
+        /* ignore commands that are handled by other parts of the system */
 		break;
 
 	default:

src/modules/navigator/CMakeLists.txt

@@ -53,6 +53,7 @@ px4_add_module(
 		enginefailure.cpp
 		gpsfailure.cpp
 		follow_target.cpp
+		#conusavoidance.cpp # Tobias Kieser
 	DEPENDS
 		git_ecl
 		ecl_geo

src/modules/navigator/land.cpp

@@ -41,6 +41,8 @@
 #include "land.h"
 #include "navigator.h"
 
+//test
+
 Land::Land(Navigator *navigator) :
 	MissionBlock(navigator)
 {