[Draft] Indi devel

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -48,6 +48,12 @@ fi
 #                        End Estimator Group Selection                        #
 ###############################################################################
 
+#
+# Start Indi Controller
+#
+indi_control start
+
+
 #
 # Start Control Allocator
 #
@@ -56,26 +62,29 @@ control_allocator start
 #
 # Start Multicopter Rate Controller.
 #
-mc_rate_control start
+#mc_rate_control start
+#indi_rate_control start
 
 #
 # Start Multicopter Attitude Controller.
 #
-mc_att_control start
+#mc_att_control start
+#indi_att_control start
 
-if param greater -s MC_AT_EN 0
-then
-	mc_autotune_attitude_control start
-fi
+#if param greater -s MC_AT_EN 0
+#then
+#	mc_autotune_attitude_control start
+#fi
 
 #
 # Start Multicopter Position Controller.
 #
-mc_hover_thrust_estimator start
-flight_mode_manager start
-mc_pos_control start
+#mc_hover_thrust_estimator start
+#flight_mode_manager start
+#mc_pos_control start
+#indi_pos_control start
 
 #
 # Start Multicopter Land Detector.
 #
-land_detector start multicopter
+#land_detector start multicopter

ROMFS/px4fmu_common/init.d/rc.mc_defaults

@@ -21,3 +21,7 @@ param set-default RTL_RETURN_ALT 30
 param set-default RTL_DESCEND_ALT 10
 
 param set-default GPS_UBX_DYNMODEL 6
+
+
+# make sure commander doesnt disarm vehicle by thinking it hasnt taken off
+param set-default COM_DISARM_PRFLT -1

boards/px4/sitl/indi.px4board

@@ -0,0 +1,17 @@
+CONFIG_MODULES_AIRSPEED_SELECTOR=n
+CONFIG_MODULES_AIRSHIP_ATT_CONTROL=n
+CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
+CONFIG_MODULES_FW_ATT_CONTROL=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_FW_POS_CONTROL_L1=n
+CONFIG_MODULES_MC_ATT_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
+CONFIG_MODULES_MC_POS_CONTROL=n
+CONFIG_MODULES_MC_RATE_CONTROL=n
+CONFIG_MODULES_UUV_ATT_CONTROL=n
+CONFIG_MODULES_UUV_POS_CONTROL=n
+CONFIG_MODULES_VTOL_ATT_CONTROL=n
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
+CONFIG_MODULES_MICRORTPS_BRIDGE=y
+CONFIG_MODULES_INDI_CONTROL=y

src/drivers/uavcan/sensors/gnss.cpp

@@ -518,47 +518,48 @@ bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t
 		perf_count(_rtcm_stream_pub_perf);
 		msg.data.clear();
 	}
-	
-  return result;
+
+	return result;
 }
 
 bool UavcanGnssBridge::injectData(const uint8_t *const data, const size_t data_len)
 {
 	using uavcan::equipment::gnss::RTCMStream;
 
-  perf_count(_rtcm_perf);
+	perf_count(_rtcm_perf);
 
 	RTCMStream msg;
 	msg.protocol_id = RTCMStream::PROTOCOL_ID_RTCM3;
 
-  const size_t capacity = msg.data.capacity();
-  size_t written = 0;
-  bool result = true;
-  while (result && written < data_len) {
-    size_t chunk_size = data_len - written;
-
-    if (chunk_size > capacity) {
-      chunk_size = capacity;
-    }
-
-    for (size_t i = 0; i < chunk_size; ++i){
-      msg.data.push_back(data[written]);
-      written += 1;
-    }
-
-    result = _pub_rtcm.broadcast(msg) >= 0;
-    msg.data.clear();
-  }
-
-  return result;
+	const size_t capacity = msg.data.capacity();
+	size_t written = 0;
+	bool result = true;
+
+	while (result && written < data_len) {
+		size_t chunk_size = data_len - written;
+
+		if (chunk_size > capacity) {
+			chunk_size = capacity;
+		}
+
+		for (size_t i = 0; i < chunk_size; ++i) {
+			msg.data.push_back(data[written]);
+			written += 1;
+		}
+
+		result = _pub_rtcm.broadcast(msg) >= 0;
+		msg.data.clear();
+	}
+
+	return result;
 }
 
 
 bool UavcanGnssBridge::PublishMovingBaselineData(const uint8_t *data, size_t data_len)
 {
 	ardupilot::gnss::MovingBaselineData msg;
-	
-  const size_t capacity = msg.data.capacity();
+
+	const size_t capacity = msg.data.capacity();
 	size_t written = 0;
 	bool result = true;
 

src/drivers/uavcan/sensors/gnss.hpp

@@ -91,7 +91,7 @@ class UavcanGnssBridge : public UavcanSensorBridgeBase
 			  const float heading_accuracy);
 
 	void handleInjectDataTopic();
-  bool injectData(const uint8_t *const data, const size_t data_len);
+	bool injectData(const uint8_t *const data, const size_t data_len);
 	bool PublishRTCMStream(const uint8_t *data, size_t data_len);
 	bool PublishMovingBaselineData(const uint8_t *data, size_t data_len);
 

src/lib/CMakeLists.txt

@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(airspeed)
+add_subdirectory(allocator_qp)
 add_subdirectory(avoidance)
 add_subdirectory(battery)
 add_subdirectory(bezier)

src/lib/allocator_qp/CMakeLists.txt

@@ -0,0 +1,114 @@
+
+
+message(STATUS "*****************")
+message(STATUS "*****************")
+message(STATUS "COMPILING OSQP")
+
+# Detect operating system
+# ----------------------------------------------
+message(STATUS "We are on a ${CMAKE_SYSTEM_NAME} system")
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+    set(IS_LINUX ON)
+elseif(${CMAKE_SYSTEM_NAME} STREQUAL "Darwin")
+    set(IS_MAC ON)
+elseif(${CMAKE_SYSTEM_NAME} STREQUAL "Windows")
+    set(IS_WINDOWS ON)
+endif()
+
+
+set(EMBEDDED 2)
+set(PRINTING OFF)
+set(PROFILING OFF)
+option(DFLOAT "Use floats instead of doubles" OFF)
+option(DLONG "Use long (64bit) for indexing" OFF) # by default its ON
+
+
+# set types for QDLDL
+if(DFLOAT)
+  set(QDLDL_FLOAT_TYPE "float")
+else()
+  set(QDLDL_FLOAT_TYPE "double")
+endif()
+
+if(DLONG)
+  set(QDLDL_INT_TYPE "long long")
+else()
+  set(QDLDL_INT_TYPE "int")
+endif()
+
+
+set(QDLDL_BOOL_TYPE "unsigned char")
+
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure/qdldl_types.h.in
+               ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_types.h
+               NEWLINE_STYLE LF)
+
+# Include math library if EMBEDDED != 1
+if(NOT (EMBEDDED EQUAL 1))
+    set(CMAKE_C_STANDARD_LIBRARIES "${CMAKE_C_STANDARD_LIBRARIES} -lm")
+endif()
+# Include real time library in linux
+if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux")
+    set(CMAKE_C_STANDARD_LIBRARIES "${CMAKE_C_STANDARD_LIBRARIES} -lrt")
+endif()
+
+# Generate header file with the global options
+# ---------------------------------------------
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/configure/osqp_configure.h.in
+               ${CMAKE_CURRENT_SOURCE_DIR}/include/osqp_configure.h
+               NEWLINE_STYLE LF)
+
+# Include header directory
+# ----------------------------------------------
+include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
+
+
+# Set sources
+# ----------------------------------------------
+add_subdirectory (src/osqp)
+add_subdirectory (include)
+
+# Append the generated workspace files and qdldl files
+list (APPEND
+      osqp_src
+      ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/workspace.c
+      ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/qdldl.c
+      ${CMAKE_CURRENT_SOURCE_DIR}/src/osqp/qdldl_interface.c
+)
+
+list (APPEND
+      osqp_headers
+      ${CMAKE_CURRENT_SOURCE_DIR}/include/workspace.h
+      ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl.h
+      ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_types.h
+      ${CMAKE_CURRENT_SOURCE_DIR}/include/qdldl_interface.h
+)
+
+
+
+px4_add_library(allocator_qp 
+	${osqp_src}
+	${osqp_headers})
+
+
+message(STATUS "DONE COMPILING OSQP")
+message(STATUS "*****************")
+message(STATUS "*****************")
+
+# set(SRCS
+# 	conversions.c
+# 	conversions.h
+# 	crc.c
+# 	crc.h
+# 	mavlink_log.cpp
+# 	mavlink_log.h
+# )
+# 
+# if(${PX4_PLATFORM} STREQUAL "nuttx")
+# 	list(APPEND SRCS
+# 		otp.c
+# 	)
+# endif()
+# 
+# px4_add_library(systemlib ${SRCS})
+# target_compile_options(systemlib PRIVATE ${MAX_CUSTOM_OPT_LEVEL})