vtol remove unused vehicle_status

2026-10-03 16:58:54 +08:00 · 2016-09-27 08:49:45 +02:00
parent e27f396f55
commit 9fd0513be3
8 changed files with 78 additions and 96 deletions
@@ -46,7 +46,6 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set COM_DISARM_LAND 5
-param set COM_DL_LOSS_EN 1
 param set MPC_ACC_HOR_MAX 2
 param set EKF2_GBIAS_INIT 0.01
 param set EKF2_ANGERR_INIT 0.01
@@ -46,7 +46,6 @@ param set RTL_RETURN_ALT 30.0
 param set RTL_DESCEND_ALT 10.0
 param set RTL_LAND_DELAY 0
 param set COM_DISARM_LAND 5
-param set COM_DL_LOSS_EN 1
 param set MPC_ACC_HOR_MAX 2
 replay tryapplyparams
 simulator start -s 
@@ -2090,7 +2090,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		/* Set thrust to 0 to minimize damage */
 		_att_sp.thrust = 0.0f;

-	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
+	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && // launchdetector only available in auto
 		   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 		   _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS &&
 		   !_runway_takeoff.runwayTakeoffEnabled()) {
@@ -2100,16 +2100,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = (_launchDetector.launchDetectionEnabled()) ? _launchDetector.getThrottlePreTakeoff() :
 				 _parameters.throttle_idle;

-	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 		   _runway_takeoff.runwayTakeoffEnabled()) {
 		_att_sp.thrust = _runway_takeoff.getThrottle(math::min(get_tecs_thrust(), throttle_max));

-	} else if (_control_mode_current ==  FW_POSCTRL_MODE_AUTO &&
+	} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
 		   pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
 		_att_sp.thrust = 0.0f;

-	} else if (_control_mode_current ==  FW_POSCTRL_MODE_OTHER) {
+	} else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
 		_att_sp.thrust = math::min(_att_sp.thrust, _parameters.throttle_max);

 	} else {
@@ -2140,7 +2140,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			    _land_noreturn_vertical);

 	// manual attitude control
-	use_tecs_pitch &=  !(_control_mode_current == FW_POSCTRL_MODE_OTHER);
+	use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER);

 	if (use_tecs_pitch) {
 		_att_sp.pitch_body = get_tecs_pitch();
@@ -2442,6 +2442,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		// after transition we ramp up desired airspeed from the speed we had coming out of the transition
 		_asp_after_transition += dt * 2; // increase 2m/s

+
+		//throttle_cruise =
+
 		if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) {
 			v_sp = fmaxf(_asp_after_transition, _ctrl_state.airspeed);
 		}
@@ -202,6 +202,7 @@ void Standard::update_vtol_state()
 			     (float)hrt_elapsed_time(&_vtol_schedule.transition_start)
 			     > (_params_standard.front_trans_time_min * 1000000.0f)) ||
 			    can_transition_on_ground()) {
+
 				_vtol_schedule.flight_mode = FW_MODE;
 				// we can turn off the multirotor motors now
 				_flag_enable_mc_motors = false;
@@ -215,11 +216,11 @@ void Standard::update_vtol_state()
 	// map specific control phases to simple control modes
 	switch (_vtol_schedule.flight_mode) {
 	case MC_MODE:
-		_vtol_mode = ROTARY_WING;
+		_vtol_mode = mode::ROTARY_WING;
 		break;

 	case FW_MODE:
-		_vtol_mode = FIXED_WING;
+		_vtol_mode = mode::FIXED_WING;
 		break;

 	case TRANSITION_TO_FW:
@@ -280,7 +281,6 @@ void Standard::update_transition_state()
 			_mc_yaw_weight = weight;
 			_mc_throttle_weight = weight;

-
 		} else {
 			// at low speeds give full weight to mc
 			_mc_roll_weight = 1.0f;
@@ -416,33 +416,39 @@ void Standard::update_fw_state()
 */
 void Standard::fill_actuator_outputs()
 {
-	/* multirotor controls */
+	// multirotor controls
 	_actuators_out_0->timestamp = _actuators_mc_in->timestamp;
-	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL]
-			* _mc_roll_weight;	// roll

+	// roll
+	_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
+	// pitch
 	_actuators_out_0->control[actuator_controls_s::INDEX_PITCH] =
-		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;	// pitch
-
-
-	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] *
-			_mc_yaw_weight;	// yaw
+		_actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
+	// yaw
+	_actuators_out_0->control[actuator_controls_s::INDEX_YAW] =
+		_actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
+	// throttle
 	_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
-		_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;	// throttle
+		_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;

-	/* fixed wing controls */
+
+	// fixed wing controls
 	_actuators_out_1->timestamp = _actuators_fw_in->timestamp;
-	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]
-			* (1 - _mc_roll_weight);	//roll
-	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
-		(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);	//pitch
-	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]
-			* (1 - _mc_yaw_weight);	// yaw

-	_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
+	//roll
+	_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] =
+		-_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL] * (1 - _mc_roll_weight);
+	//pitch
+	_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] =
+		(_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH] + _params->fw_pitch_trim) * (1 - _mc_pitch_weight);
+	// yaw
+	_actuators_out_1->control[actuator_controls_s::INDEX_YAW] =
+		_actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * (1 - _mc_yaw_weight);

 	// set the fixed wing throttle control
 	if (_vtol_schedule.flight_mode == FW_MODE && _armed->armed) {
+
 		// take the throttle value commanded by the fw controller
 		_actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] =
 			_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
@@ -79,7 +79,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_airspeed_sub(-1),
 	_battery_status_sub(-1),
 	_vehicle_cmd_sub(-1),
-	_vehicle_status_sub(-1),
 	_tecs_status_sub(-1),
 	_land_detected_sub(-1),

@@ -113,7 +112,6 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_airspeed, 0, sizeof(_airspeed));
 	memset(&_batt_status, 0, sizeof(_batt_status));
 	memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
-	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
 	memset(&_tecs_status, 0, sizeof(_tecs_status));
 	memset(&_land_detected, 0, sizeof(_land_detected));

@@ -416,21 +414,6 @@ VtolAttitudeControl::vehicle_cmd_poll()
 	}
 }

-/**
-* Check for vehicle status updates.
-*/
-void
-VtolAttitudeControl::vehicle_status_poll()
-{
-	bool updated;
-
-	orb_check(_vehicle_status_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub , &_vehicle_status);
-	}
-}
-
 /**
 * Check for TECS status updates.
 */
@@ -641,7 +624,6 @@ void VtolAttitudeControl::task_main()
 	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
 	_battery_status_sub	   = orb_subscribe(ORB_ID(battery_status));
 	_vehicle_cmd_sub	   = orb_subscribe(ORB_ID(vehicle_command));
-	_vehicle_status_sub    = orb_subscribe(ORB_ID(vehicle_status));
 	_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
 	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));

@@ -721,7 +703,6 @@ void VtolAttitudeControl::task_main()
 		vehicle_airspeed_poll();
 		vehicle_battery_poll();
 		vehicle_cmd_poll();
-		vehicle_status_poll();
 		tecs_status_poll();
 		land_detected_poll();

@@ -778,7 +759,7 @@ void VtolAttitudeControl::task_main()
 			// vehicle is doing a transition
 			_vtol_vehicle_status.vtol_in_trans_mode = true;
 			_vtol_vehicle_status.vtol_in_rw_mode = true; //making mc attitude controller work during transition
-			_vtol_vehicle_status.in_transition_to_fw = _vtol_type->get_mode() == TRANSITION_TO_FW;
+			_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == TRANSITION_TO_FW);

 			bool got_new_data = false;

@@ -53,45 +53,46 @@
 #include <px4_defines.h>
 #include <px4_tasks.h>
 #include <px4_posix.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <math.h>
+#include <poll.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <drivers/drv_hrt.h>
+
 #include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/mc_virtual_attitude_setpoint.h>
-#include <uORB/topics/fw_virtual_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/mc_virtual_rates_setpoint.h>
-#include <uORB/topics/fw_virtual_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vtol_vehicle_status.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/airspeed.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/tecs_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/topics/control_state.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/circuit_breaker.h>
-#include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
-#include <fcntl.h>
+#include <lib/mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/control_state.h>
+#include <uORB/topics/fw_virtual_attitude_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/mc_virtual_attitude_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/tecs_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/uORB.h>

 #include "tiltrotor.h"
 #include "tailsitter.h"
@@ -130,7 +131,6 @@ public:
 	struct vehicle_local_position_s 		*get_local_pos() {return &_local_pos;}
 	struct airspeed_s 				*get_airspeed() {return &_airspeed;}
 	struct battery_status_s 			*get_batt_status() {return &_batt_status;}
-	struct vehicle_status_s 			*get_vehicle_status() {return &_vehicle_status;}
 	struct tecs_status_s 				*get_tecs_status() {return &_tecs_status;}
 	struct vehicle_land_detected_s			*get_land_detected() {return &_land_detected;}

@@ -146,19 +146,18 @@ private:
 	/* handlers for subscriptions */
 	int	_v_att_sub;				//vehicle attitude subscription
 	int	_v_att_sp_sub;			//vehicle attitude setpoint subscription
-	int 	_mc_virtual_att_sp_sub;
-	int 	_fw_virtual_att_sp_sub;
+	int	_mc_virtual_att_sp_sub;
+	int	_fw_virtual_att_sp_sub;
 	int	_mc_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
 	int	_fw_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
 	int	_v_control_mode_sub;	//vehicle control mode subscription
 	int	_params_sub;			//parameter updates subscription
 	int	_manual_control_sp_sub;	//manual control setpoint subscription
 	int	_armed_sub;				//arming status subscription
-	int 	_local_pos_sub;			// sensor subscription
-	int 	_airspeed_sub;			// airspeed subscription
-	int 	_battery_status_sub;	// battery status subscription
-	int 	_vehicle_cmd_sub;
-	int 	_vehicle_status_sub;
+	int	_local_pos_sub;			// sensor subscription
+	int	_airspeed_sub;			// airspeed subscription
+	int	_battery_status_sub;	// battery status subscription
+	int	_vehicle_cmd_sub;
 	int	_tecs_status_sub;
 	int	_land_detected_sub;

@@ -171,6 +170,7 @@ private:
 	orb_advert_t	_vtol_vehicle_status_pub;
 	orb_advert_t	_v_rates_sp_pub;
 	orb_advert_t	_v_att_sp_pub;
+
 //*******************data containers***********************************************************
 	struct vehicle_attitude_s			_v_att;				//vehicle attitude
 	struct vehicle_attitude_setpoint_s		_v_att_sp;			//vehicle attitude setpoint
@@ -191,7 +191,6 @@ private:
 	struct airspeed_s 				_airspeed;			// airspeed
 	struct battery_status_s 			_batt_status; 		// battery status
 	struct vehicle_command_s			_vehicle_cmd;
-	struct vehicle_status_s				_vehicle_status;
 	struct tecs_status_s				_tecs_status;
 	struct vehicle_land_detected_s			_land_detected;

@@ -247,7 +246,6 @@ private:
 	void		tecs_status_poll();
 	void		land_detected_poll();
 	void 		parameters_update_poll();		//Check if parameters have changed
-	void 		vehicle_status_poll();
 	int 		parameters_update();			//Update local paraemter cache
 	void 		fill_mc_att_rates_sp();
 	void 		fill_fw_att_rates_sp();
@@ -66,7 +66,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_local_pos = _attc->get_local_pos();
 	_airspeed = _attc->get_airspeed();
 	_batt_status = _attc->get_batt_status();
-	_vehicle_status = _attc->get_vehicle_status();
 	_tecs_status = _attc->get_tecs_status();
 	_land_detected = _attc->get_land_detected();
 	_params = _attc->get_params();
@@ -84,8 +83,6 @@ VtolType::~VtolType()
 */
 void VtolType::set_idle_mc()
 {
-	int ret;
-	unsigned servo_count;
 	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);

@@ -93,7 +90,8 @@ void VtolType::set_idle_mc()
 		PX4_WARN("can't open %s", dev);
 	}

-	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	unsigned servo_count;
+	int ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
 	unsigned pwm_value = _params->idle_pwm_mc;
 	struct pwm_output_values pwm_values;
 	memset(&pwm_values, 0, sizeof(pwm_values));
@@ -119,7 +117,6 @@ void VtolType::set_idle_mc()
 */
 void VtolType::set_idle_fw()
 {
-	int ret;
 	const char *dev = PWM_OUTPUT0_DEVICE_PATH;
 	int fd = px4_open(dev, 0);

@@ -137,7 +134,7 @@ void VtolType::set_idle_fw()
 		pwm_values.channel_count++;
 	}

-	ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+	int ret = px4_ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);

 	if (ret != OK) {
 		PX4_WARN("failed setting min values");
@@ -161,7 +161,6 @@ protected:
 	struct vehicle_local_position_s			*_local_pos;
 	struct airspeed_s 				*_airspeed;					// airspeed
 	struct battery_status_s 			*_batt_status; 				// battery status
-	struct vehicle_status_s 			*_vehicle_status;			// vehicle status from commander app
 	struct tecs_status_s				*_tecs_status;
 	struct vehicle_land_detected_s			*_land_detected;