diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a9f1cf3abc..c784e4075b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -87,52 +87,58 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t * int _system_type = _param_mav_type.get(); - unsigned motors_count; + /* 'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust) + all other motors are configured for -1..1 range */ + unsigned pos_thrust_motors_count; bool is_fixed_wing; switch (_system_type) { case MAV_TYPE_AIRSHIP: case MAV_TYPE_VTOL_DUOROTOR: case MAV_TYPE_COAXIAL: - motors_count = 2; + pos_thrust_motors_count = 2; is_fixed_wing = false; break; case MAV_TYPE_TRICOPTER: - motors_count = 3; + pos_thrust_motors_count = 3; is_fixed_wing = false; break; case MAV_TYPE_QUADROTOR: case MAV_TYPE_VTOL_QUADROTOR: case MAV_TYPE_VTOL_TILTROTOR: - motors_count = 4; + pos_thrust_motors_count = 4; is_fixed_wing = false; break; case MAV_TYPE_VTOL_RESERVED2: - motors_count = 5; + pos_thrust_motors_count = 5; is_fixed_wing = false; break; case MAV_TYPE_HEXAROTOR: - motors_count = 6; + pos_thrust_motors_count = 6; is_fixed_wing = false; break; case MAV_TYPE_OCTOROTOR: + pos_thrust_motors_count = 8; + is_fixed_wing = false; + break; + case MAV_TYPE_SUBMARINE: - motors_count = 8; + pos_thrust_motors_count = 0; is_fixed_wing = false; break; case MAV_TYPE_FIXED_WING: - motors_count = 0; + pos_thrust_motors_count = 0; is_fixed_wing = true; break; default: - motors_count = 0; + pos_thrust_motors_count = 0; is_fixed_wing = false; break; } @@ -143,7 +149,7 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t * msg->controls[i] = 0.0f; } else if ((is_fixed_wing && i == 4) || - (!is_fixed_wing && i < motors_count)) { //multirotor, rotor channel + (!is_fixed_wing && i < pos_thrust_motors_count)) { //multirotor, rotor channel /* scale PWM out PWM_DEFAULT_MIN..PWM_DEFAULT_MAX us to 0..1 for rotors */ msg->controls[i] = (_actuator_outputs.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN); msg->controls[i] = math::constrain(msg->controls[i], 0.f, 1.f);