diff --git a/posix-configs/SITL/init/ekf2/standard_vtol b/posix-configs/SITL/init/ekf2/standard_vtol index 265ba281d56..818b12f9f9e 100644 --- a/posix-configs/SITL/init/ekf2/standard_vtol +++ b/posix-configs/SITL/init/ekf2/standard_vtol @@ -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 diff --git a/posix-configs/SITL/init/lpe/standard_vtol b/posix-configs/SITL/init/lpe/standard_vtol index f548446dca5..7c71521a3f2 100644 --- a/posix-configs/SITL/init/lpe/standard_vtol +++ b/posix-configs/SITL/init/lpe/standard_vtol @@ -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 diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 74b26ea3fbd..beb86397986 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -2090,7 +2090,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_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> ¤t_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> ¤t_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); } diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index e455770411d..d93870acc6b 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -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]; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 4f004cf837c..1ebc81a5700 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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; diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index ab9545fb9e4..1f0917f12fa 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -53,45 +53,46 @@ #include #include #include + +#include +#include +#include +#include #include #include #include #include -#include -#include -#include -#include + #include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include -#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #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(); diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 7fa199a26d4..5db1a24c23b 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -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"); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index caf4aebd637..4adf118b834 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -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;