Fixed code style for multirotor_att_control app

5 files changed, 51 insertions, 25 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 1b03e8c22..d94c0a69c 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[])
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 
-	/* 
+	/*
 	 * Do not rate-limit the loop to prevent aliasing
 	 * if rate-limiting would be desired later, the line below would
 	 * enable it.
@@ -125,9 +125,9 @@ mc_thread_main(int argc, char *argv[])
 	 * orb_set_interval(att_sub, 5);
 	 */
 	struct pollfd fds[2] = {
-					{ .fd = att_sub, .events = POLLIN },
-					{ .fd = param_sub, .events = POLLIN }
-				};
+		{ .fd = att_sub, .events = POLLIN },
+		{ .fd = param_sub, .events = POLLIN }
+	};
 
 	/* publish actuator controls */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
@@ -171,6 +171,7 @@ mc_thread_main(int argc, char *argv[])
 		if (ret < 0) {
 			/* poll error, count it in perf */
 			perf_count(mc_err_perf);
+
 		} else if (ret == 0) {
 			/* no return value, ignore */
 		} else {
@@ -193,9 +194,11 @@ mc_thread_main(int argc, char *argv[])
 				/* get a local copy of system state */
 				bool updated;
 				orb_check(state_sub, &updated);
+
 				if (updated) {
 					orb_copy(ORB_ID(vehicle_status), state_sub, &state);
 				}
+
 				/* get a local copy of manual setpoint */
 				orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
 				/* get a local copy of attitude */
@@ -204,9 +207,11 @@ mc_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
 				/* get a local copy of rates setpoint */
 				orb_check(setpoint_sub, &updated);
+
 				if (updated) {
 					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
 				}
+
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
@@ -222,6 +227,7 @@ mc_thread_main(int argc, char *argv[])
 //						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
 						rates_sp.timestamp = hrt_absolute_time();
 						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+
 					} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
 						att_sp.roll_body = offboard_sp.p1;
 						att_sp.pitch_body = offboard_sp.p2;
@@ -232,7 +238,7 @@ mc_thread_main(int argc, char *argv[])
 						/* STEP 2: publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 					}
-					
+
 
 				} else if (state.flag_control_manual_enabled) {
 
@@ -240,8 +246,8 @@ mc_thread_main(int argc, char *argv[])
 
 						/* initialize to current yaw if switching to manual or att control */
 						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-					 	    state.flag_control_manual_enabled != flag_control_manual_enabled ||
-					 	    state.flag_system_armed != flag_system_armed) {
+						    state.flag_control_manual_enabled != flag_control_manual_enabled ||
+						    state.flag_system_armed != flag_system_armed) {
 							att_sp.yaw_body = att.yaw;
 						}
 
@@ -257,13 +263,14 @@ mc_thread_main(int argc, char *argv[])
 							/*
 							 * Only go to failsafe throttle if last known throttle was
 							 * high enough to create some lift to make hovering state likely.
-							 * 
+							 *
 							 * This is to prevent that someone landing, but not disarming his
 							 * multicopter (throttle = 0) does not make it jump up in the air
 							 * if shutting down his remote.
 							 */
 							if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
 								att_sp.thrust = failsafe_throttle;
+
 							} else {
 								att_sp.thrust = 0.0f;
 							}
@@ -290,11 +297,12 @@ mc_thread_main(int argc, char *argv[])
 
 							/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
 							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
-								state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+							    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
 
 								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
 									rates_sp.yaw = manual.yaw;
 									control_yaw_position = false;
+
 								} else {
 									/*
 									 * This mode SHOULD be the default mode, which is:
@@ -309,11 +317,13 @@ mc_thread_main(int argc, char *argv[])
 										rates_sp.yaw = manual.yaw;
 										control_yaw_position = false;
 										first_time_after_yaw_speed_control = true;
+
 									} else {
 										if (first_time_after_yaw_speed_control) {
 											att_sp.yaw_body = att.yaw;
 											first_time_after_yaw_speed_control = false;
 										}
+
 										control_yaw_position = true;
 									}
 								}
@@ -340,7 +350,7 @@ mc_thread_main(int argc, char *argv[])
 					} else {
 						/* manual rate inputs, from RC control or joystick */
 						if (state.flag_control_rates_enabled &&
-							state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
 							rates_sp.roll = manual.roll;
 
 							rates_sp.pitch = manual.pitch;
@@ -354,9 +364,9 @@ mc_thread_main(int argc, char *argv[])
 
 				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 				if (state.flag_control_attitude_enabled) {
-				 	multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
+					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
 
-				 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
 
 				/* measure in what intervals the controller runs */
@@ -367,6 +377,7 @@ mc_thread_main(int argc, char *argv[])
 				/* get current rate setpoint */
 				bool rates_sp_valid = false;
 				orb_check(rates_sp_sub, &rates_sp_valid);
+
 				if (rates_sp_valid) {
 					orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
 				}
@@ -394,6 +405,7 @@ mc_thread_main(int argc, char *argv[])
 	/* kill all outputs */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
 		actuators.control[i] = 0.0f;
+
 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
 
@@ -415,6 +427,7 @@ usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
+
 	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] [-t] {start|status|stop}\n");
 	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
 	fprintf(stderr, "    -t enables motor test mode with 10%% thrust\n");
@@ -432,22 +445,25 @@ int multirotor_att_control_main(int argc, char *argv[])
 			motor_test_mode = true;
 			optioncount += 1;
 			break;
+
 		case ':':
 			usage("missing parameter");
 			break;
+
 		default:
 			fprintf(stderr, "option: -%c\n", ch);
 			usage("unrecognized option");
 			break;
 		}
 	}
+
 	argc -= optioncount;
 	//argv += optioncount;
 
 	if (argc < 1)
 		usage("missing command");
 
-	if (!strcmp(argv[1+optioncount], "start")) {
+	if (!strcmp(argv[1 + optioncount], "start")) {
 
 		thread_should_exit = false;
 		mc_task = task_spawn("multirotor_att_control",
@@ -459,7 +475,7 @@ int multirotor_att_control_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	if (!strcmp(argv[1+optioncount], "stop")) {
+	if (!strcmp(argv[1 + optioncount], "stop")) {
 		thread_should_exit = true;
 		exit(0);
 	}
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index b98736cee..76dbb36d3 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -158,16 +158,18 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-		const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
 	float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
 	last_run = hrt_absolute_time();
+
 	if (last_input != att_sp->timestamp) {
 		last_input = att_sp->timestamp;
 	}
+
 	static int sensor_delay;
 	sensor_delay = hrt_absolute_time() - att->timestamp;
 
@@ -189,9 +191,9 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_update(&h, &p);
 
 		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-				1000.0f, PID_MODE_DERIVATIV_SET);
+			 1000.0f, PID_MODE_DERIVATIV_SET);
 		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-				1000.0f, PID_MODE_DERIVATIV_SET);
+			 1000.0f, PID_MODE_DERIVATIV_SET);
 
 		initialized = true;
 	}
@@ -207,7 +209,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	}
 
 	/* reset integral if on ground */
-	if(att_sp->thrust < 0.1f) {
+	if (att_sp->thrust < 0.1f) {
 		pid_reset_integral(&pitch_controller);
 		pid_reset_integral(&roll_controller);
 	}
@@ -217,13 +219,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	/* control pitch (forward) output */
 	rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body ,
-			att->pitch, att->pitchspeed, deltaT);
+					att->pitch, att->pitchspeed, deltaT);
 
 	/* control roll (left/right) output */
 	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body ,
-			att->roll, att->rollspeed, deltaT);
+				       att->roll, att->rollspeed, deltaT);
 
-	if(control_yaw_position) {
+	if (control_yaw_position) {
 		/* control yaw rate */
 
 		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
@@ -233,12 +235,14 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 		if (yaw_error > M_PI_F) {
 			yaw_error -= M_TWOPI_F;
+
 		} else if (yaw_error < -M_PI_F) {
 			yaw_error += M_TWOPI_F;
 		}
 
 		rates_sp->yaw = p.yaw_p * (yaw_error) - (p.yaw_d * att->yawspeed);
 	}
+
 	rates_sp->thrust = att_sp->thrust;
 
 	motor_skip_counter++;
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
index abc94d617..2cf83e443 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.h
+++ b/apps/multirotor_att_control/multirotor_attitude_control.h
@@ -52,6 +52,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 93f7085ae..deba1ac03 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -148,7 +148,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 }
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-	const float rates[], struct actuator_controls_s *actuators)
+			      const float rates[], struct actuator_controls_s *actuators)
 {
 	static float roll_control_last  = 0;
 	static float pitch_control_last = 0;
@@ -157,6 +157,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	static uint64_t last_input = 0;
 
 	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
+
 	if (last_input != rate_sp->timestamp) {
 		last_input = rate_sp->timestamp;
 	}
@@ -186,12 +187,14 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	}
 
 	/* calculate current control outputs */
-	
+
 	/* control pitch (forward) output */
 	float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
+
 	/* increase resilience to faulty control inputs */
 	if (isfinite(pitch_control)) {
 		pitch_control_last = pitch_control;
+
 	} else {
 		pitch_control = 0.0f;
 		warnx("rej. NaN ctrl pitch");
@@ -199,9 +202,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	/* control roll (left/right) output */
 	float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
+
 	/* increase resilience to faulty control inputs */
 	if (isfinite(roll_control)) {
 		roll_control_last = roll_control;
+
 	} else {
 		roll_control = 0.0f;
 		warnx("rej. NaN ctrl roll");
@@ -209,6 +214,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	/* control yaw rate */
 	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
+
 	/* increase resilience to faulty control inputs */
 	if (!isfinite(yaw_rate_control)) {
 		yaw_rate_control = 0.0f;
diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h
index 3c3c50801..03dec317a 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.h
+++ b/apps/multirotor_att_control/multirotor_rate_control.h
@@ -51,6 +51,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-	const float rates[], struct actuator_controls_s *actuators);
+			      const float rates[], struct actuator_controls_s *actuators);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */