fw_pos_ctrl_l1 stop using sensors_combined

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -76,7 +76,6 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/tecs_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
@@ -157,7 +156,6 @@ private:
 	int		_vehicle_land_detected_sub{-1};		///< vehicle land detected subscription */
 	int		_params_sub{-1};			///< notification of parameter updates */
 	int		_manual_control_sub{-1};		///< notification of manual control updates */
-	int		_sensor_combined_sub{-1};		///< for body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub{nullptr};		///< attitude setpoint */
 	orb_advert_t	_tecs_status_pub{nullptr};		///< TECS status publication */
@@ -169,7 +167,6 @@ private:
 	struct fw_pos_ctrl_status_s		_fw_pos_ctrl_status {};		///< navigation capabilities */
 	struct manual_control_setpoint_s	_manual {};			///< r/c channel data */
 	struct position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items */
-	struct sensor_combined_s		_sensor_combined {};		///< for body frame accelerations */
 	struct vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint */
 	struct vehicle_command_s		_vehicle_command {};		///< vehicle commands */
 	struct vehicle_control_mode_s		_control_mode {};		///< control mode */
@@ -385,7 +382,6 @@ private:
 	void		control_update();
 	void		manual_control_setpoint_poll();
 	void		position_setpoint_triplet_poll();
-	void		sensor_combined_poll();
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
 	void		vehicle_land_detected_poll();
@@ -474,12 +470,11 @@ private:
 	/*
 	 * Call TECS : a wrapper function to call the TECS implementation
 	 */
-	void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
+	void tecs_update_pitch_throttle(float alt_sp, float airspeed_sp,
 					float pitch_min_rad, float pitch_max_rad,
 					float throttle_min, float throttle_max, float throttle_cruise,
 					bool climbout_mode, float climbout_pitch_min_rad,
-					float altitude,
+					uint8_t mode = tecs_status_s::TECS_MODE_NORMAL);
-					unsigned mode = tecs_status_s::TECS_MODE_NORMAL);
 
 };
 
@@ -803,18 +798,6 @@ FixedwingPositionControl::control_state_poll()
 	_tecs.enable_airspeed(_airspeed_valid);
 }
 
-void
-FixedwingPositionControl::sensor_combined_poll()
-{
-	/* check if there is a new position */
-	bool sensors_updated;
-	orb_check(_sensor_combined_sub, &sensors_updated);
-
-	if (sensors_updated) {
-		orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
-	}
-}
-
 void
 FixedwingPositionControl::position_setpoint_triplet_poll()
 {
@@ -1101,7 +1084,7 @@ bool
 FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, const math::Vector<2> &ground_speed,
 		const struct position_setpoint_s &pos_sp_prev, const struct position_setpoint_s &pos_sp_curr)
 {
-	float dt = 0.01; // Using non zero value to a avoid division by zero
+	float dt = 0.01f;
 
 	if (_control_position_last_called > 0) {
 		dt = hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
@@ -1120,10 +1103,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
 	_att_sp.apply_flaps = false;		// by default we don't use flaps
 
-	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
-
 	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
+	math::Vector<3> accel_body(_ctrl_state.x_acc, _ctrl_state.y_acc, _ctrl_state.z_acc);
 
 	// tailsitters use the multicopter frame as reference, in fixed wing
 	// we need to use the fixed wing frame
@@ -1252,10 +1233,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 			_att_sp.roll_body = _l1_control.nav_roll();
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
-			tecs_update_pitch_throttle(pos_sp_curr.alt, calculate_target_airspeed(mission_airspeed), eas2tas,
+			tecs_update_pitch_throttle(pos_sp_curr.alt,
-						   radians(_parameters.pitch_limit_min), radians(_parameters.pitch_limit_max),
+						   calculate_target_airspeed(mission_airspeed),
-						   _parameters.throttle_min, _parameters.throttle_max, mission_throttle,
+						   radians(_parameters.pitch_limit_min),
-						   false, radians(_parameters.pitch_limit_min), _global_pos.alt);
+						   radians(_parameters.pitch_limit_max),
+						   _parameters.throttle_min,
+						   _parameters.throttle_max,
+						   mission_throttle,
+						   false,
+						   radians(_parameters.pitch_limit_min));
 
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 
@@ -1285,15 +1271,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 			tecs_update_pitch_throttle(alt_sp,
 						   calculate_target_airspeed(mission_airspeed),
-						   eas2tas,
 						   radians(_parameters.pitch_limit_min),
 						   radians(_parameters.pitch_limit_max),
 						   _parameters.throttle_min,
 						   _parameters.throttle_max,
 						   _parameters.throttle_cruise,
 						   false,
-						   radians(_parameters.pitch_limit_min),
+						   radians(_parameters.pitch_limit_min));
-						   _global_pos.alt);
 
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
 
@@ -1368,10 +1352,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 			}
 
 			/* Vertical landing control */
-			//xxx: using the tecs altitude controller for slope control for now
 			/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
-			// XXX this could make a great param
-
 			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
 			float airspeed_land = _parameters.land_airspeed_scale * _parameters.airspeed_min;
 			float airspeed_approach = _parameters.land_airspeed_scale * _parameters.airspeed_min;
@@ -1470,7 +1451,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 							   calculate_target_airspeed(airspeed_land),
-							   eas2tas,
 							   radians(_parameters.land_flare_pitch_min_deg),
 							   radians(_parameters.land_flare_pitch_max_deg),
 							   0.0f,
@@ -1478,7 +1458,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 							   throttle_land,
 							   false,
 							   _land_motor_lim ? radians(_parameters.land_flare_pitch_min_deg) : radians(_parameters.pitch_limit_min),
-							   _global_pos.alt,
 							   _land_motor_lim ? tecs_status_s::TECS_MODE_LAND_THROTTLELIM : tecs_status_s::TECS_MODE_LAND);
 
 				if (!_land_noreturn_vertical) {
@@ -1532,15 +1511,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 				}
 
 				tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
-							   calculate_target_airspeed(airspeed_approach), eas2tas,
+							   calculate_target_airspeed(airspeed_approach),
 							   radians(_parameters.pitch_limit_min),
 							   radians(_parameters.pitch_limit_max),
 							   _parameters.throttle_min,
 							   _parameters.throttle_max,
 							   _parameters.throttle_cruise,
 							   false,
-							   radians(_parameters.pitch_limit_min),
+							   radians(_parameters.pitch_limit_min));
-							   _global_pos.alt);
 			}
 
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
@@ -1584,7 +1562,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 				tecs_update_pitch_throttle(pos_sp_curr.alt,
 							   calculate_target_airspeed(_runway_takeoff.getMinAirspeedScaling() * _parameters.airspeed_min),
-							   eas2tas,
 							   radians(_parameters.pitch_limit_min),
 							   takeoff_pitch_max_rad,
 							   _parameters.throttle_min,
@@ -1592,7 +1569,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 							   _parameters.throttle_cruise,
 							   _runway_takeoff.climbout(),
 							   radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _parameters.pitch_limit_min)),
-							   _global_pos.alt,
 							   tecs_status_s::TECS_MODE_TAKEOFF);
 
 				// assign values
@@ -1620,7 +1596,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 						}
 
 						/* Detect launch */
-						_launchDetector.update(accel_body(0));
+						_launchDetector.update(_ctrl_state.x_acc);
 
 						/* update our copy of the launch detection state */
 						_launch_detection_state = _launchDetector.getLaunchDetected();
@@ -1659,7 +1635,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 						/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 						tecs_update_pitch_throttle(pos_sp_curr.alt,
 									   _parameters.airspeed_trim,
-									   eas2tas,
 									   radians(_parameters.pitch_limit_min),
 									   takeoff_pitch_max_rad,
 									   _parameters.throttle_min,
@@ -1667,7 +1642,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 									   _parameters.throttle_cruise,
 									   true,
 									   max(radians(pos_sp_curr.pitch_min), radians(10.0f)),
-									   _global_pos.alt,
 									   tecs_status_s::TECS_MODE_TAKEOFF);
 
 						/* limit roll motion to ensure enough lift */
@@ -1676,15 +1650,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					} else {
 						tecs_update_pitch_throttle(pos_sp_curr.alt,
 									   calculate_target_airspeed(mission_airspeed),
-									   eas2tas,
 									   radians(_parameters.pitch_limit_min),
 									   radians(_parameters.pitch_limit_max),
 									   _parameters.throttle_min,
 									   takeoff_throttle,
 									   _parameters.throttle_cruise,
 									   false,
-									   radians(_parameters.pitch_limit_min),
+									   radians(_parameters.pitch_limit_min));
-									   _global_pos.alt);
 					}
 
 				} else {
@@ -1762,7 +1734,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 		tecs_update_pitch_throttle(_hold_alt,
 					   altctrl_airspeed,
-					   eas2tas,
 					   radians(_parameters.pitch_limit_min),
 					   radians(_parameters.pitch_limit_max),
 					   _parameters.throttle_min,
@@ -1770,7 +1741,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					   _parameters.throttle_cruise,
 					   climbout_requested,
 					   climbout_requested ? radians(10.0f) : pitch_limit_min,
-					   _global_pos.alt,
 					   tecs_status_s::TECS_MODE_NORMAL);
 
 		/* heading control */
@@ -1871,7 +1841,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 
 		tecs_update_pitch_throttle(_hold_alt,
 					   altctrl_airspeed,
-					   eas2tas,
 					   radians(_parameters.pitch_limit_min),
 					   radians(_parameters.pitch_limit_max),
 					   _parameters.throttle_min,
@@ -1879,7 +1848,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 					   _parameters.throttle_cruise,
 					   climbout_requested,
 					   climbout_requested ? radians(10.0f) : pitch_limit_min,
-					   _global_pos.alt,
 					   tecs_status_s::TECS_MODE_NORMAL);
 
 		_att_sp.roll_body = _manual.y * _parameters.man_roll_max_rad;
@@ -1950,10 +1918,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &curr_pos, cons
 			    pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF &&
 			    (_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled()));
 
-
 	// flaring during landing
-	use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND &&
+	use_tecs_pitch &= !(pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LAND && _land_noreturn_vertical);
-			    _land_noreturn_vertical);
 
 	// manual attitude control
 	use_tecs_pitch &= !(_control_mode_current == FW_POSCTRL_MODE_OTHER);
@@ -2021,7 +1987,6 @@ FixedwingPositionControl::task_main()
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
-	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -2118,7 +2083,6 @@ FixedwingPositionControl::task_main()
 			control_state_poll();
 			manual_control_setpoint_poll();
 			position_setpoint_triplet_poll();
-			sensor_combined_poll();
 
 			math::Vector<2> curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
 			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
@@ -2196,7 +2160,8 @@ FixedwingPositionControl::task_main()
 	_control_task = -1;
 }
 
-void FixedwingPositionControl::reset_takeoff_state()
+void
+FixedwingPositionControl::reset_takeoff_state()
 {
 	// only reset takeoff if !armed or just landed
 	if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed)) {
@@ -2212,7 +2177,8 @@ void FixedwingPositionControl::reset_takeoff_state()
 	}
 }
 
-void FixedwingPositionControl::reset_landing_state()
+void
+FixedwingPositionControl::reset_landing_state()
 {
 	_time_started_landing = 0;
 
@@ -2233,12 +2199,12 @@ void FixedwingPositionControl::reset_landing_state()
 	}
 }
 
-void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
+void
+FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspeed_sp,
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
-		float altitude,
+		uint8_t mode)
-		unsigned mode)
 {
 	float dt = 0.01f; // prevent division with 0
 
@@ -2255,33 +2221,33 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 	// (it should also not run during VTOL blending because airspeed is too low still)
 	if (_vehicle_status.is_vtol) {
 		run_tecs &= !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode;
-	}
 
-	// we're in transition
+		if (_vehicle_status.in_transition_mode) {
-	if (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode) {
+			// we're in transition
-		_was_in_transition = true;
+			_was_in_transition = true;
 
-		// set this to transition airspeed to init tecs correctly
+			// set this to transition airspeed to init tecs correctly
-		if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
+			if (_parameters.airspeed_mode == control_state_s::AIRSPD_MODE_DISABLED) {
-			// some vtols fly without airspeed sensor
+				// some vtols fly without airspeed sensor
-			_asp_after_transition = _parameters.airspeed_trans;
+				_asp_after_transition = _parameters.airspeed_trans;
 
-		} else {
+			} else {
-			_asp_after_transition = _ctrl_state.airspeed;
+				_asp_after_transition = _ctrl_state.airspeed;
-		}
+			}
 
-		_asp_after_transition = constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max);
+			_asp_after_transition = constrain(_asp_after_transition, _parameters.airspeed_min, _parameters.airspeed_max);
 
-	} else if (_was_in_transition) {
+		} else if (_was_in_transition) {
-		// after transition we ramp up desired airspeed from the speed we had coming out of the transition
+			// 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
+			_asp_after_transition += dt * 2; // increase 2m/s
 
-		if (_asp_after_transition < v_sp && _ctrl_state.airspeed < v_sp) {
+			if (_asp_after_transition < airspeed_sp && _ctrl_state.airspeed < airspeed_sp) {
-			v_sp = fmaxf(_asp_after_transition, _ctrl_state.airspeed);
+				airspeed_sp = max(_asp_after_transition, _ctrl_state.airspeed);
 
-		} else {
+			} else {
-			_was_in_transition = false;
+				_was_in_transition = false;
-			_asp_after_transition = 0;
+				_asp_after_transition = 0;
+			}
 		}
 	}
 
@@ -2321,8 +2287,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		pitch_for_tecs = euler(1);
 	}
 
-	_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, altitude, alt_sp, v_sp,
+	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
-				    _ctrl_state.airspeed, eas2tas,
+
+	_tecs.update_pitch_throttle(_R_nb, pitch_for_tecs,
+				    _global_pos.alt, alt_sp,
+				    airspeed_sp, _ctrl_state.airspeed, eas2tas,
 				    climbout_mode, climbout_pitch_min_rad,
 				    throttle_min, throttle_max, throttle_cruise,
 				    pitch_min_rad, pitch_max_rad);