working offboard

2 files changed, 39 insertions, 32 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 40f1bbfde..7d5821d3f 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -165,8 +165,30 @@ mc_thread_main(int argc, char *argv[])
 
 
 		/** STEP 1: Define which input is the dominating control input */
-
-		if (state.flag_control_manual_enabled) {
+		if (state.flag_control_offboard_enabled) {
+					/* offboard inputs */
+					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
+						rates_sp.roll = offboard_sp.p1;
+						rates_sp.pitch = offboard_sp.p2;
+						rates_sp.yaw = offboard_sp.p3;
+						rates_sp.thrust = offboard_sp.p4;
+						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;
+						att_sp.yaw_body = offboard_sp.p3;
+						att_sp.thrust = offboard_sp.p4;
+						printf("thrust_att=%8.4f\n",offboard_sp.p4);
+						att_sp.timestamp = hrt_absolute_time();
+						/* STEP 2: publish the result to the vehicle actuators */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+					}
+
+					/* decide wether we want rate or position input */
+				}
+		else if (state.flag_control_manual_enabled) {
 			/* manual inputs, from RC control or joystick */
 
 			if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
@@ -188,7 +210,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);
-			
+
 			if (motor_test_mode) {
 				att_sp.roll_body = 0.0f;
 				att_sp.pitch_body = 0.0f;
@@ -199,39 +221,19 @@ mc_thread_main(int argc, char *argv[])
 				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 			}
 
-		} else if (state.flag_control_offboard_enabled) {
-			/* offboard inputs */
-			if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-				rates_sp.roll = offboard_sp.p1;
-				rates_sp.pitch = offboard_sp.p2;
-				rates_sp.yaw = offboard_sp.p3;
-				rates_sp.thrust = 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;
-				att_sp.yaw_body = offboard_sp.p3;
-				att_sp.thrust = offboard_sp.p4;
-				att_sp.timestamp = hrt_absolute_time();
-				/* STEP 2: publish the result to the vehicle actuators */
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-			}
-
-			/* decide wether we want rate or position input */
 		}
 
 		/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 
 
 		/* run attitude controller */
-		// if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
-		// 	multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
-		// 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		// } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
-		// 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
-		// 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-		// }
+		 if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
+		 	multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
+		 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		 } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
+		 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
+		 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+		 }
 
 
 		if (state.flag_control_rates_enabled) {
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 372b378d1..7532dffa2 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -144,6 +144,8 @@ 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)
 {
+	static float roll_control_last=0;
+	static float pitch_control_last=0;
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 	last_run = hrt_absolute_time();
@@ -172,10 +174,13 @@ 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 * deltaT * (rate_sp->pitch-rates[1]);
+
+	float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
+	pitch_control_last=pitch_control;
 	/* control roll (left/right) output */
-	float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0]  );
 
+	float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
+	roll_control_last=roll_control;
 	/* control yaw rate */
 	float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2]  );