Master+adum+8.0.1

[mstrakl]

PR Type

Enhancement, Other

---

Description

This description is generated by an AI tool. It may have inaccuracies.

• Comprehensive update to the rotorpy simulator framework with extensive modifications across multiple modules

• Updates to vehicle parameters and configurations including ArduPilot multirotor support and various drone parameter files (Crazyflie, Hummingbird, PX4)

• Enhancements to wind simulation systems including Dryden wind models and spatial wind implementations

• Additions to trajectory generation utilities and multiple trajectory types (circular, Lissajous, speed-based, hover, batched)

• Improvements to controller and estimator templates with new implementations including wind UKF estimator

• Updates to learning utilities and reward functions for quadrotor training

• Build system updates with new Docker and shell scripts for SITL (Software-In-The-Loop) simulation

• Configuration files for INAV 8.0.1 autopilot with multiple vehicle setups

• Documentation updates across multiple README files for controllers, estimators, trajectories, wind, learning, sensors, vehicles, and worlds

• Utility enhancements including animation, post-processing, attitude gain system identification, and numpy encoding

• World environment definitions including grid forest, vortex shedding, and custom pillar scenarios

• Socket communication and test infrastructure improvements

---

Diagram Walkthrough

flowchart LR
  A["rotorpy Simulator<br/>Framework"] -->|"Vehicle Params"| B["Drone Configs<br/>ArduPilot/PX4/Crazyflie"]
  A -->|"Wind Systems"| C["Dryden Winds<br/>Spatial Winds"]
  A -->|"Trajectories"| D["Circular/Lissajous<br/>Speed/Hover/Batched"]
  A -->|"Control & Estimation"| E["Controllers<br/>Estimators/UKF"]
  A -->|"Learning"| F["Reward Functions<br/>Learning Utils"]
  A -->|"Build & Deploy"| G["Docker/SITL<br/>INAV 8.0.1"]
  A -->|"Environments"| H["Worlds & Scenarios<br/>Grid Forest/Vortex"]

Loading

File Walkthrough

	Relevant files

---

[github-actions]

Branch Targeting Suggestion

You've targeted the master branch with this PR. Please consider if a version branch might be more appropriate:

• maintenance-9.x - If your change is backward-compatible and won't create compatibility issues between INAV firmware and Configurator 9.x versions. This will allow your PR to be included in the next 9.x release.

• maintenance-10.x - If your change introduces compatibility requirements between firmware and configurator that would break 9.x compatibility. This is for PRs which will be included in INAV 10.x

If master is the correct target for this change, no action is needed.

---

This is an automated suggestion to help route contributions to the appropriate branch.

[qodo-code-review]

PR Compliance Guide 🔍

All compliance sections have been disabled in the configurations.

[mstrakl]

Disregard, opened by mistake.

[qodo-code-review]

High-level Suggestion

The new Python-based rotorpy simulation framework should be moved to a separate repository instead of being integrated directly into the firmware's codebase. This separation will improve modularity, simplify maintenance, and allow for independent development. [High-level, importance: 9]

Solution Walkthrough:

Before:

# Single repository containing both firmware and simulator
inav_firmware_and_simulator/
├── src/
│   └── main/
│       ├── fc/
│       ├── navigation/
│       └── target/
│           └── SITL/
│               └── sim/
│                   └── adumsim.c  # SITL backend
├── simulator/
│   └── rotorpy/                 # Python simulator project
│       ├── vehicles/
│       │   └── multirotor.py
│       └── ...
└── ...

After:

# Repository 1: Firmware
inav_firmware/
├── src/
│   └── main/
│       └── target/
│           └── SITL/
│               └── sim/
│                   └── adumsim.c # Connects to external sim
└── ...

# Repository 2: Simulator (separate project)
rotorpy_simulator/
├── rotorpy/
│   ├── vehicles/
│   │   └── multirotor.py
│   └── ...
└── setup.py

[qodo-code-review]

Suggestion: In BatchedMultirotor.hat_map, fix the vectorized implementation where torch.cat with dim=0 and subsequent reshaping constructs the batch of skew-symmetric matrices incorrectly. [possible issue, importance: 9]

Suggested change

	def hat_map(cls, s):
	"""
	Given vector s in R^3, return associate skew symmetric matrix S in R^3x3
	In the vectorized implementation, we assume that s is in the shape (N arrays, 3)
	"""
	device = s.device
	if len(s.shape) > 1: # Vectorized implementation
	s = s.unsqueeze(-1)
	hat = torch.cat([torch.zeros(s.shape[0], 1, device=device), -s[:, 2], s[:, 1],
	s[:, 2], torch.zeros(s.shape[0], 1, device=device), -s[:, 0],
	-s[:, 1], s[:, 0], torch.zeros(s.shape[0], 1, device=device)], dim=0).view(3, 3, s.shape[
	0]).double()
	return hat
	else:
	return torch.tensor([[0, -s[2], s[1]],
	[s[2], 0, -s[0]],
	[-s[1], s[0], 0]], device=device)
	def hat_map(cls, s):
	"""
	Given vector s in R^3, return associate skew symmetric matrix S in R^3x3
	In the vectorized implementation, we assume that s is in the shape (N arrays, 3)
	"""
	device = s.device
	if len(s.shape) > 1: # Vectorized implementation
	s = s.unsqueeze(-1)
	z = torch.zeros(s.shape[0], 1, 1, device=device)
	hat = torch.cat([
	z, -s[:, 2].unsqueeze(1), s[:, 1].unsqueeze(1),
	s[:, 2].unsqueeze(1), z, -s[:, 0].unsqueeze(1),
	-s[:, 1].unsqueeze(1), s[:, 0].unsqueeze(1), z
	], dim=1).view(s.shape[0], 3, 3).double()
	return hat
	else:
	return torch.tensor([[0, -s[2], s[1]],
	[s[2], 0, -s[0]],
	[-s[1], s[0], 0]], device=device)

[qodo-code-review]

Suggestion: In _pack_state, replace the hardcoded state vector size of 20 with a dynamic size based on 16 + self.num_rotors to support different vehicle configurations. [possible issue, importance: 7]

Suggested change

	s = np.zeros((20,)) # FIXME: this shouldn't be hardcoded. Should vary with the number of rotors.
	s = np.zeros((16 + self.num_rotors,))

[qodo-code-review]

Suggestion: In __init__, initialize self.s_dot with a dynamic size of 16 + self.num_rotors instead of a hardcoded 20 to correctly match the state derivative vector's length. [possible issue, importance: 7]

Suggested change

	self.s_dot = np.zeros(20,)
	self.s_dot = np.zeros(16 + self.num_rotors,)

[qodo-code-review]

Suggestion: In update_thrust_and_rotor_params, fix the rotor position update by assigning dict(rotor_pos) to self.rotor_pos[idx] instead of incorrectly indexing with a float value k_idx. [possible issue, importance: 9]

Suggested change

	self.rotor_pos[idx] = dict(rotor_pos[k_idx])
	self.rotor_pos[idx] = dict(rotor_pos)

[qodo-code-review]

Suggestion: In the reset method, use the provided options parameter if it is not None; otherwise, fall back to self.reset_options to enable runtime configuration. [possible issue, importance: 8]

Suggested change

	def reset(self, seed=None, options=None):
	"""
	Reset the environment
	Inputs:
	seed: the seed for any random number generation, mostly for reproducibility.
	options: dictionary for misc options for resetting the scene.
	'initial_states': determines how to set the quadrotor again. Options are...
	'random': will randomly select the state of the quadrotor.
	'deterministic': will set the state to the initial state selected by the user when creating
	the quadrotor environment (usually hover).
	the user can also specify the state itself as a dictionary... e.g.
	reset(options={'initial_states':
	{'x': torch.zeros(num_drones,3, device=device).double(),
	'v': torch.zeros(num_drones, 3, device=device).double(),
	'q': torch.tensor([0, 0, 0, 1], device=device).repeat(num_drones, 1).double(),
	'w': torch.zeros(num_drones, 3, device=device).double(),
	'wind': torch.zeros(num_drones, 3, device=device).double(),
	'rotor_speeds': torch.tensor([init_rotor_speed, init_rotor_speed, init_rotor_speed, init_rotor_speed], device=device).repeat(num_drones, 1).double()
	}
	'pos_bound': the min/max position region for random placement.
	'vel_bound': the min/max velocity region for random placement
	"""
	options = self.reset_options
	# If any options are not specified, set them to default values.
	if 'pos_bound' not in options:
	options['pos_bound'] = 2
	if 'vel_bound' not in options:
	options['vel_bound'] = 0
	if 'initial_states' not in options:
	options['initial_states'] = 'random'
	def reset(self, seed=None, options=None):
	"""
	Reset the environment
	Inputs:
	seed: the seed for any random number generation, mostly for reproducibility.
	options: dictionary for misc options for resetting the scene.
	'initial_states': determines how to set the quadrotor again. Options are...
	'random': will randomly select the state of the quadrotor.
	'deterministic': will set the state to the initial state selected by the user when creating
	the quadrotor environment (usually hover).
	the user can also specify the state itself as a dictionary... e.g.
	reset(options={'initial_states':
	{'x': torch.zeros(num_drones,3, device=device).double(),
	'v': torch.zeros(num_drones, 3, device=device).double(),
	'q': torch.tensor([0, 0, 0, 1], device=device).repeat(num_drones, 1).double(),
	'w': torch.zeros(num_drones, 3, device=device).double(),
	'wind': torch.zeros(num_drones, 3, device=device).double(),
	'rotor_speeds': torch.tensor([init_rotor_speed, init_rotor_speed, init_rotor_speed, init_rotor_speed], device=device).repeat(num_drones, 1).double()
	}
	'pos_bound': the min/max position region for random placement.
	'vel_bound': the min/max velocity region for random placement
	"""
	if options is None:
	options = self.reset_options
	# If any options are not specified, set them to default values.
	if 'pos_bound' not in options:
	options['pos_bound'] = 2
	if 'vel_bound' not in options:
	options['vel_bound'] = 0
	if 'initial_states' not in options:
	options['initial_states'] = 'random'

[qodo-code-review]

Suggestion: In reset_idx, when options['initial_states'] is a dictionary, use the values from it to set the vehicle state instead of incorrectly using self.initial_states. [possible issue, importance: 8]

Suggested change

	elif isinstance(options['initial_states'], dict):
	# Ensure the correct keys are in dict.
	assert all(key in options['initial_states'] for key in ('x', 'v', 'q', 'w', 'wind', 'rotor_speeds'))
	for key in options['initial_states'].keys():
	self.vehicle_states[key][env_idx] = self.initial_states[key][env_idx].double().to(self.device)
	elif isinstance(options['initial_states'], dict):
	# Ensure the correct keys are in dict.
	assert all(key in options['initial_states'] for key in ('x', 'v', 'q', 'w', 'wind', 'rotor_speeds'))
	for key in options['initial_states'].keys():
	self.vehicle_states[key][env_idx] = options['initial_states'][key][env_idx].double().to(self.device)

[qodo-code-review]

Suggestion: In reset_idx, correct the calculation of max_yaw_moment by indexing self.rotor_speed_max with env_idx. [possible issue, importance: 8]

Suggested change

	self.max_yaw_moment = self.quad_params.k_m[env_idx].cpu().numpy() * self.rotor_speed_max**2
	self.max_yaw_moment[env_idx] = self.quad_params.k_m[env_idx].cpu().numpy() * self.rotor_speed_max[env_idx]**2

[qodo-code-review]

Suggestion: Add the missing break in case 9 to prevent unintended fallthrough, and make FLOAT_MINUS_1_1_TO_PWM return uint16_t since it is assigned into uint16_t channel values. [Learned best practice, importance: 6]

Suggested change

	#define FLOAT_MINUS_1_1_TO_PWM(x) ((float)((uint16_t)((x + 1.0f) / 2.0f * 1000.0f) + 1000.0f))
	static uint8_t pwmMapping[XP_MAX_PWM_OUTS];
	static uint8_t mappingCount;
	static int listenFd, connFd;
	static int sockfd;
	static pthread_t listenThread;
	static bool initalized = false;
	static bool useImu = false;
	static float lattitude = 0;
	static float longitude = 0;
	static float elevation = 0;
	static float agl = 0;
	static float local_vx = 0;
	static float local_vy = 0;
	static float local_vz = 0;
	static float groundspeed = 0;
	static float airspeed = 0;
	static float roll = 0;
	static float pitch = 0;
	static float yaw = 0;
	static float hpath = 0;
	static float accel_x = 0;
	static float accel_y = 0;
	static float accel_z = 0;
	static float gyro_x = 0;
	static float gyro_y = 0;
	static float gyro_z = 0;
	static float barometer = 0;
	static bool hasJoystick = false;
	static float joystickRaw[ADUM_JOYSTICK_AXIS_COUNT];
	int init_fgear_socket(char* ip, int port) {
	struct sockaddr_in serverAddr, clientAddr;
	socklen_t clientLen = sizeof(clientAddr);
	// Create TCP socket
	listenFd = socket(AF_INET, SOCK_STREAM, 0);
	if (listenFd < 0) {
	perror("socket");
	return 1;
	}
	// Set SO_REUSEADDR
	int opt = 1;
	if (setsockopt(listenFd, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)) < 0) {
	perror("setsockopt");
	close(listenFd);
	return 1;
	}
	// Bind to IP and port
	memset(&serverAddr, 0, sizeof(serverAddr));
	serverAddr.sin_family = AF_INET;
	serverAddr.sin_port = htons(port);
	if (inet_pton(AF_INET, ip, &serverAddr.sin_addr) <= 0) {
	perror("inet_pton");
	close(listenFd);
	return 1;
	}
	if (bind(listenFd, (struct sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) {
	perror("bind");
	close(listenFd);
	return 1;
	}
	// Listen for incoming connections
	if (listen(listenFd, 1) < 0) {
	perror("listen");
	close(listenFd);
	return 1;
	}
	printf("Listening on %s:%d\n", ip, port);
	// Block until a client connects
	connFd = accept(listenFd, (struct sockaddr*)&clientAddr, &clientLen);
	if (connFd < 0) {
	perror("accept");
	close(listenFd);
	return 1;
	}
	// Disable Nagle's algorithm for low-latency sending
	int tcp_opt = 1;
	if (setsockopt(connFd, IPPROTO_TCP, TCP_NODELAY, &tcp_opt, sizeof(tcp_opt)) < 0) {
	perror("setsockopt TCP_NODELAY");
	}
	char clientIP[INET_ADDRSTRLEN];
	inet_ntop(AF_INET, &clientAddr.sin_addr, clientIP, sizeof(clientIP));
	printf("Connection established with %s:%d\n", clientIP, ntohs(clientAddr.sin_port));
	return 0;
	}
	int init_fgear_client(const char* server_ip, int server_port) {
	struct sockaddr_in serverAddr;
	// Create TCP socket
	sockfd = socket(AF_INET, SOCK_STREAM, 0);
	if (sockfd < 0) {
	perror("socket");
	return -1;
	}
	// Fill server address
	memset(&serverAddr, 0, sizeof(serverAddr));
	serverAddr.sin_family = AF_INET;
	serverAddr.sin_port = htons(server_port);
	if (inet_pton(AF_INET, server_ip, &serverAddr.sin_addr) <= 0) {
	perror("inet_pton");
	close(sockfd);
	return -1;
	}
	// Connect to server
	int conn_status = -1;
	for (int i=0; i<100; i++) {
	//printf("Connecting attempt nr: %d\n", i);
	conn_status = connect(sockfd, (struct sockaddr*)&serverAddr, sizeof(serverAddr));
	if (conn_status == 0) break;
	sleep(1);
	}
	if (conn_status < 0) {
	perror("connect");
	close(sockfd);
	return -1;
	}
	// Disable Nagle's algorithm for low-latency sending
	int tcp_opt = 1;
	if (setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, &tcp_opt, sizeof(tcp_opt)) < 0) {
	perror("setsockopt TCP_NODELAY");
	}
	printf("Connected to server %s:%d\n", server_ip, server_port);
	return 0; // return socket descriptor for sending/receiving
	}
	static void* listenWorker(void* arg)
	{
	char buf[BUF_SIZE];
	char *token;
	while (1) {
	// Read all outputs from inav
	// ----------------------------------------------------- //
	float motorValue[4] = { 0.0f };
	float yokeValues[3] = { 0 };
	int y = 0;
	int k = 0;
	for (int i = 0; i < mappingCount; i++) {
	if (y > 2) {
	break;
	}
	if (pwmMapping[i] & 0x80) { // Motor
	if (k < 4) motorValue[k] = PWM_TO_FLOAT_0_1(motor[pwmMapping[i] & 0x7f]);
	k++;
	//printf("Motor %d value: %f\n", i, motorValue);
	//printf("Motor %d value: %d\n", i, motor[pwmMapping[i] & 0x7f]);
	} else {
	yokeValues[y] = PWM_TO_FLOAT_MINUS_1_1(servo[pwmMapping[i]]);
	y++;
	}
	}
	// Send motor data to simulation
	char msg[256];
	int len = snprintf(msg, sizeof(msg),
	"%.6f;%.6f;%.6f;%.6f;\n",
	motorValue[0], motorValue[1], motorValue[2], motorValue[3]);
	if (len > 0) {
	ssize_t sent = send(sockfd, msg, len, 0);
	}
	// Read all data from socket
	// ----------------------------------------------------- //
	uint16_t channelValues[ADUM_JOYSTICK_AXIS_COUNT];
	ssize_t n = recv(sockfd, buf, sizeof(buf) - 1, 0);
	if (n > 0) {
	buf[n] = '\0'; // null terminate
	//printf("Rx: %s\n", buf);
	int index = 0;
	token = strtok(buf, ";");
	while (token != NULL) {
	float value = atof(token); // convert to float
	switch(index) {
	case 0:
	//trel=value;
	break;
	case 1:
	lattitude = value;
	break;
	case 2:
	longitude = value;
	break;
	case 3:
	elevation = value; // Altitude MSL in meters
	break;
	case 4:
	hpath = value; // Track
	break;
	case 5:
	yaw = value; // Heading
	break;
	case 6:
	//posx = value;
	break;
	case 7:
	//posy = value;
	break;
	case 8:
	agl = value; // posz
	break;
	case 9:
	groundspeed = value;
	case 10:
	roll = value;
	break;
	case 11:
	pitch = value;
	break;
	case 12:
	//yaw = value; // Gets this from heading
	break;
	case 13:
	accel_x = value;
	break;
	case 14:
	accel_y = value;
	break;
	case 15:
	accel_z = value;
	break;
	case 16:
	gyro_x = value;
	break;
	case 17:
	gyro_y = value;
	break;
	case 18:
	gyro_z = value;
	break;
	case 19:
	channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	#define FLOAT_MINUS_1_1_TO_PWM(x) ((uint16_t)(((x + 1.0f) * 0.5f * 1000.0f) + 1000.0f))
	...
	case 9:
	groundspeed = value;
	break;
	...
	case 19:
	channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;

[qodo-code-review]

Suggestion: Initialize channelValues to safe defaults and only call rxSimSetChannelValue when enough tokens were received to populate the channels, otherwise keep the previous/neutral values. [Learned best practice, importance: 5]

Suggested change

	uint16_t channelValues[ADUM_JOYSTICK_AXIS_COUNT];
	ssize_t n = recv(sockfd, buf, sizeof(buf) - 1, 0);
	if (n > 0) {
	buf[n] = '\0'; // null terminate
	//printf("Rx: %s\n", buf);
	int index = 0;
	token = strtok(buf, ";");
	while (token != NULL) {
	float value = atof(token); // convert to float
	switch(index) {
	case 0:
	//trel=value;
	break;
	case 1:
	lattitude = value;
	break;
	case 2:
	longitude = value;
	break;
	case 3:
	elevation = value; // Altitude MSL in meters
	break;
	case 4:
	hpath = value; // Track
	break;
	case 5:
	yaw = value; // Heading
	break;
	case 6:
	//posx = value;
	break;
	case 7:
	//posy = value;
	break;
	case 8:
	agl = value; // posz
	break;
	case 9:
	groundspeed = value;
	case 10:
	roll = value;
	break;
	case 11:
	pitch = value;
	break;
	case 12:
	//yaw = value; // Gets this from heading
	break;
	case 13:
	accel_x = value;
	break;
	case 14:
	accel_y = value;
	break;
	case 15:
	accel_z = value;
	break;
	case 16:
	gyro_x = value;
	break;
	case 17:
	gyro_y = value;
	break;
	case 18:
	gyro_z = value;
	break;
	case 19:
	channelValues[0] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	case 20:
	channelValues[1] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	case 21:
	channelValues[2] = FLOAT_0_1_TO_PWM(value);
	break;
	case 22:
	channelValues[3] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	case 23:
	channelValues[4] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	case 24:
	channelValues[5] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	case 25:
	channelValues[6] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	case 26:
	channelValues[7] = FLOAT_MINUS_1_1_TO_PWM(value);
	break;
	default:
	break;
	}
	token = strtok(NULL, ";");
	index++;
	}
	rxSimSetChannelValue(channelValues, ADUM_JOYSTICK_AXIS_COUNT);
	uint16_t channelValues[ADUM_JOYSTICK_AXIS_COUNT];
	for (int i = 0; i < ADUM_JOYSTICK_AXIS_COUNT; i++) {
	channelValues[i] = 1500;
	}
	ssize_t n = recv(sockfd, buf, sizeof(buf) - 1, 0);
	if (n > 0) {
	buf[n] = '\0'; // null terminate
	...
	token = strtok(buf, ";");
	while (token != NULL) {
	float value = atof(token); // convert to float
	...
	token = strtok(NULL, ";");
	index++;
	}
	if (index >= 27) {
	rxSimSetChannelValue(channelValues, ADUM_JOYSTICK_AXIS_COUNT);
	}

[sensei-hacker]

It looks like you might have something cool going on there.
It might dovetail with my own simulator work I have for testing.
Can I check out your project somewhere?