Added actuator control removal

5 files changed, 39 insertions, 89 deletions

diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index abc91d34f..a19252465 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -72,7 +72,6 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_effective_0;
 struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 struct airspeed_s airspeed;
@@ -119,7 +118,6 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
-static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -147,7 +145,6 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
-	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -249,7 +246,7 @@ l_vehicle_attitude(const struct listener *l)
 			last_sent_vfr = t;
 			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
 			uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
-			float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+			float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f;
 			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
 		}
 	}
@@ -604,32 +601,6 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls_effective(const struct listener *l)
-{
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
-
-	if (gcs_link) {
-		/* send, add spaces so that string buffer is at least 10 chars long */
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl0    ",
-						   actuators_effective_0.control_effective[0]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl1    ",
-						   actuators_effective_0.control_effective[1]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl2     ",
-						   actuators_effective_0.control_effective[2]);
-		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
-						   last_sensor_timestamp / 1000,
-						   "eff ctrl3     ",
-						   actuators_effective_0.control_effective[3]);
-	}
-}
-
-void
 l_vehicle_attitude_controls(const struct listener *l)
 {
 	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
@@ -839,9 +810,6 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
-	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
-
 	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 05897b4ce..35ef5fcf6 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -185,7 +185,7 @@ mixer_tick(void)
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
 			/* safe actuators for FMU feedback */
-			r_page_actuators[i] = (r_page_servos[i] - 1500) / 600.0f;
+			r_page_actuators[i] = FLOAT_TO_REG((r_page_servos[i] - 1500) / 600.0f);
 		}
 
 
@@ -201,6 +201,10 @@ mixer_tick(void)
 
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
+
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
+			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+		}
 	}
 
 	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index f94875d5b..2adb13f5c 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -68,7 +68,6 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -691,7 +690,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_rates_setpoint_s rates_sp;
 		struct actuator_outputs_s act_outputs;
 		struct actuator_controls_s act_controls;
-		struct actuator_controls_effective_s act_controls_effective;
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
@@ -717,7 +715,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int rates_sp_sub;
 		int act_outputs_sub;
 		int act_controls_sub;
-		int act_controls_effective_sub;
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
@@ -763,9 +760,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 20;
-	/* Sanity check variable and index */
+	/* number of subscriptions */
+	const ssize_t fdsc = 19;
+	/* sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
 	struct pollfd fds[fdsc];
@@ -824,12 +821,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- ACTUATOR CONTROL EFFECTIVE --- */
-	subs.act_controls_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
-	fds[fdsc_count].fd = subs.act_controls_effective_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
 	/* --- LOCAL POSITION --- */
 	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	fds[fdsc_count].fd = subs.local_pos_sub;
@@ -1114,12 +1105,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(ATTC);
 			}
 
-			/* --- ACTUATOR CONTROL EFFECTIVE --- */
-			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.act_controls_effective_sub, &buf.act_controls_effective);
-				// TODO not implemented yet
-			}
-
 			/* --- LOCAL POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 3514dca24..c6a252b55 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -169,13 +169,6 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
 #include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
-/* actuator controls, as set by actuators / mixers after limiting */
-#include "topics/actuator_controls_effective.h"
-ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
-ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
-
 #include "topics/actuator_outputs.h"
 ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h
index d7b404ad4..54d84231f 100644
--- a/src/modules/uORB/topics/actuator_controls_effective.h
+++ b/src/modules/uORB/topics/actuator_controls_effective.h
@@ -46,34 +46,34 @@
 #ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
 #define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
 
-#include <stdint.h>
-#include "../uORB.h"
-#include "actuator_controls.h"
+//#include <stdint.h>
+//#include "../uORB.h"
+//#include "actuator_controls.h"
+//
+//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
+//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
+//
+///**
+// * @addtogroup topics
+// * @{
+// */
+//
+//struct actuator_controls_effective_s {
+//	uint64_t timestamp;
+//	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
+//};
+//
+///**
+// * @}
+// */
+//
+///* actuator control sets; this list can be expanded as more controllers emerge */
+//ORB_DECLARE(actuator_controls_effective_0);
+//ORB_DECLARE(actuator_controls_effective_1);
+//ORB_DECLARE(actuator_controls_effective_2);
+//ORB_DECLARE(actuator_controls_effective_3);
+//
+///* control sets with pre-defined applications */
+//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
 
-#define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
-#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct actuator_controls_effective_s {
-	uint64_t timestamp;
-	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
-};
-
-/**
- * @}
- */
-
-/* actuator control sets; this list can be expanded as more controllers emerge */
-ORB_DECLARE(actuator_controls_effective_0);
-ORB_DECLARE(actuator_controls_effective_1);
-ORB_DECLARE(actuator_controls_effective_2);
-ORB_DECLARE(actuator_controls_effective_3);
-
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
-
-#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file
+#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */