diff --git a/.gitmodules b/.gitmodules index 2f789aaeff5e..70c0dc8f03cc 100644 --- a/.gitmodules +++ b/.gitmodules @@ -94,3 +94,6 @@ [submodule "Tools/simulation/gz/plugins/px4-gzsim-plugins"] path = Tools/simulation/gz/plugins/px4-gzsim-plugins url = https://github.com/tiiuae/px4-gzsim-plugins.git +[submodule "src/modules/redundancy"] + path = src/modules/redundancy + url = git@github.com:tiiuae/px4_redundancy.git diff --git a/Tools/simulation/gz/hitl_run.sh b/Tools/simulation/gz/hitl_run.sh index 9a9c98e71be0..9aa5a84ab1dd 100755 --- a/Tools/simulation/gz/hitl_run.sh +++ b/Tools/simulation/gz/hitl_run.sh @@ -1,10 +1,16 @@ #!/bin/bash +GZ_SIM_SYSTEM_PLUGIN_PATH=$(pwd)/build/px4_sitl_default/build_gz-sim_plugins/ +PX4_GZ_MODELS=$(pwd)/Tools/simulation/gz/models +PX4_GZ_WORLDS=$(pwd)/Tools/simulation/gz/worlds + #example for run #./Tools/simulation/gz/hitl_run.sh x500/model_hitl.sdf MODEL_PATH=$1 + + if [ -z $MODEL_PATH ]; then echo "You should specify a path to the using model" echo "./Tools/simulation/gz/hitl_run.sh x500/model_hitl.sdf" diff --git a/Tools/simulation/gz/models/ssrc_holybro_x500/model.sdf b/Tools/simulation/gz/models/ssrc_holybro_x500/model.sdf index 014606889724..471c1ccc2af8 100644 --- a/Tools/simulation/gz/models/ssrc_holybro_x500/model.sdf +++ b/Tools/simulation/gz/models/ssrc_holybro_x500/model.sdf @@ -605,256 +605,6 @@ - - - 1 - -2 0 1 0 0.3 0 - - 0 - - 0.0 - 0.0 - 0.0 - - - - - 1.7 - - 1280 - 720 - - - 0.1 - 1000 - - - true - 30 - true - - - - 1 - camera_link - base_link - - 50 -50 50 - - 0 - 0 - 0 - 0 - - - 0 - 0 - - - - - - - - 0 0 0 0 1.57 0 - - 0.01 - - 0.000166667 - 0.000166667 - 0.000166667 - - - - - - 0.01 0.01 0.01 - - - - - - - 0.01 0.01 0.01 - - - - - - 1.047 - - 1280 - 720 - - - 0.1 - 100 - - - true - 30 - true - - - - gimbal_camera_link - camera_link_pan - - 0 1 0 - - - - - 0.01 - - 0.000166667 - 0.000166667 - 0.000166667 - - - - - - - 1 - - - - - - - - - camera_link_pan - base_link - - 0 0 1 - - - - - - - 0 0 0.089 0 0 0 - - 0.01 - - 0.000166667 - 0.000166667 - 0.000166667 - - - - true - 20 - false - - - - 720 - 1 - -3.1241390751 - 3.1241390751 - - - - 0.15 - 14 - 0.01 - - - gaussian - 0.0 - 0.01 - - - - - 0 0 0 0 0 0 - - - 1 1 1 - meshes/x500_rplidar.dae - - - - - - 2d_scanner_link - base_link - - - - - - -0.007 -0.04 -0.039 0 1.5708 1.5708 - - 0.01 - - 0.000166667 - 0.000166667 - 0.000166667 - - - - 0 0 0 0 0 0 - - - 0.001 0.001 0.001 - meshes/garmin_lidar_v3.stl - - - - - - - - - - - true - 100 - false - - - - 1 - 1 - -0 - 0 - - - - 0.06 - 35 - 0.01 - - - gaussian - 0.0 - 0.01 - - - - - - - - - - - - lidar0_link - base_link - - - @@ -871,45 +621,6 @@ 3 - - camera_joint_pan - 1 - 0.1 - 0.01 - 1 - -1 - 1000 - -1000 - - - - camera_joint_tilt - 1 - 0.1 - 0.01 - 1 - -1 - 1000 - -1000 - - - - camera_joint_pan - - - - camera_joint_tilt - - - \ No newline at end of file diff --git a/Tools/simulation/gz/models/ssrc_holybro_x500/model_hitl.sdf b/Tools/simulation/gz/models/ssrc_holybro_x500/model_hitl.sdf index 28e4783bd37d..6098c46353cd 100644 --- a/Tools/simulation/gz/models/ssrc_holybro_x500/model_hitl.sdf +++ b/Tools/simulation/gz/models/ssrc_holybro_x500/model_hitl.sdf @@ -8,10 +8,12 @@ /link/base_link/sensor/imu_sensor/imu /pose/info - 192.168.200.101 - 14542 - 14543 + 192.168.202.1 + 14561 + 14560 4560 + 192.168.202.65 + 14562 0 diff --git a/Tools/simulation/gz/plugins/px4-gzsim-plugins b/Tools/simulation/gz/plugins/px4-gzsim-plugins index f2da227901b1..57b6c8c2c903 160000 --- a/Tools/simulation/gz/plugins/px4-gzsim-plugins +++ b/Tools/simulation/gz/plugins/px4-gzsim-plugins @@ -1 +1 @@ -Subproject commit f2da227901b115679b18767ecf73ae695cf4113a +Subproject commit 57b6c8c2c9035fdec83a8ec1e3269654be50a9f2 diff --git a/boards/ssrc/saluki-pi b/boards/ssrc/saluki-pi index 4534047159f6..cbbb46658a24 160000 --- a/boards/ssrc/saluki-pi +++ b/boards/ssrc/saluki-pi @@ -1 +1 @@ -Subproject commit 4534047159f66067813bbf5f147a5323de87b5d8 +Subproject commit cbbb46658a240c1ea1b412745f9ea0ee310b7a33 diff --git a/clone_public.sh b/clone_public.sh index c5ed7d21e599..e8bdf4073eae 100755 --- a/clone_public.sh +++ b/clone_public.sh @@ -9,6 +9,7 @@ do [[ "${repo}" == *pfsoc_crypto ]] || \ [[ "${repo}" == *pfsoc_keystore ]] || \ [[ "${repo}" == *pf_crypto ]] || \ + [[ "${repo}" == src/modules/redundancy ]] || \ [[ "${repo}" == *process ]] && continue git submodule update --init --recursive "${repo}" done <<< "$(git submodule status | awk '{print $2}')" diff --git a/msg/ActuatorOutputs.msg b/msg/ActuatorOutputs.msg index 3209e54e34cc..82836ace2742 100644 --- a/msg/ActuatorOutputs.msg +++ b/msg/ActuatorOutputs.msg @@ -6,3 +6,4 @@ float32[16] output # output data, in natural output units # actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1]) # TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug +# TOPICS redundant_actuator_outputs0 redundant_actuator_outputs1 diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 878b4b04ed36..0e244896f96b 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -122,3 +122,8 @@ bool calibration_enabled bool pre_flight_checks_pass # true if all checks necessary to arm pass +# Maximum number of redundant (backup) flight controllers +uint8 MAX_REDUNDANT_CONTROLLERS = 2 + +# TOPICS vehicle_status +# TOPICS redundant_status0 redundant_status1 diff --git a/src/drivers/pwm_esc/pwm_esc.cpp b/src/drivers/pwm_esc/pwm_esc.cpp index ce2bd28ec573..9e9a6b4f2002 100644 --- a/src/drivers/pwm_esc/pwm_esc.cpp +++ b/src/drivers/pwm_esc/pwm_esc.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include @@ -59,6 +60,7 @@ #include #include #include +#include #include @@ -92,7 +94,7 @@ class PWMESC : public OutputModuleInterface * * Initialize all class variables. */ - PWMESC(); + PWMESC(bool hitl); /** * Destructor. @@ -120,6 +122,16 @@ class PWMESC : public OutputModuleInterface */ static int stop(); + /** + * Status of PWMESC driver + */ + static int status(); + + /** + * Usage of PWMESC driver + */ + static int print_usage(const char *reason); + /** * Return if the PWMESC driver is already running */ @@ -155,12 +167,12 @@ class PWMESC : public OutputModuleInterface uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; - MixingOutput _mixing_output{PARAM_PREFIX, PWMESC_MAX_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true}; + MixingOutput _mixing_output; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1000000}; /* advertised topics */ - uORB::PublicationMulti _actuator_outputs_pub{ORB_ID(actuator_outputs)}; + uORB::PublicationMulti _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)}; actuator_armed_s _actuator_armed; @@ -175,6 +187,11 @@ class PWMESC : public OutputModuleInterface /* Singleton pointer */ static PWMESC *_instance; + /** + * Status of PWMESC driver + */ + int printStatus(); + /** * Trampoline to the worker task */ @@ -199,11 +216,11 @@ class PWMESC : public OutputModuleInterface /** * Get the singleton instance */ - static inline PWMESC *getInstance() + static inline PWMESC *getInstance(bool allocate = false, bool hitl = false) { - if (_instance == nullptr) { + if (_instance == nullptr && allocate) { /* create the driver */ - _instance = new PWMESC(); + _instance = new PWMESC(hitl); } return _instance; @@ -218,14 +235,15 @@ class PWMESC : public OutputModuleInterface PWMESC *PWMESC::_instance = nullptr; -PWMESC::PWMESC() : +PWMESC::PWMESC(bool hitl) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), _task(-1), _task_should_exit(false), _perf_update(perf_alloc(PC_ELAPSED, "pwm update")), - _hitl_mode(false) + _mixing_output(hitl ? "PWM_MAIN" : PARAM_PREFIX, PWMESC_MAX_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, + true), + _hitl_mode(hitl) { - /* initialize tick semaphores */ px4_sem_init(&_update_sem, 0, 0); px4_sem_setprotocol(&_update_sem, SEM_PRIO_NONE); @@ -260,8 +278,6 @@ PWMESC::~PWMESC() int PWMESC::init(bool hitl_mode) { - _hitl_mode = hitl_mode; - /* start the main task */ _task = px4_task_spawn_cmd("pwm_esc", SCHED_DEFAULT, @@ -309,14 +325,47 @@ PWMESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne pwm.channels[i].channel = i + 1; } - /* Only publish if not in hitl */ - if (!_hitl_mode && - ::ioctl(_pwm_fd[0], PWMIOC_SETCHARACTERISTICS, - (unsigned long)((uintptr_t)&pwm)) < 0) { - PX4_ERR("PWMIOC_SETCHARACTERISTICS failed: %d\n", - errno); - ret = false; + if (!_hitl_mode) { + /* When not in hitl, run PWMs */ + if (::ioctl(_pwm_fd[0], PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)) < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS failed: %d\n", + errno); + ret = false; + } + + } else { + // In hitl, publish actuator_outputs_sim + // Only publish once we receive actuator_controls (important for lock-step to work correctly) + actuator_outputs_s actuator_outputs_sim{}; + + if (num_control_groups_updated > 0) { + for (int i = 0; i < (int)num_outputs; i++) { + uint16_t disarmed = _mixing_output.disarmedValue(i); + uint16_t min = _mixing_output.minValue(i); + uint16_t max = _mixing_output.maxValue(i); + + OutputFunction function = _mixing_output.outputFunction(i); + float output = outputs[i]; + + if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax)) { + // Scale motors to [0, 1] + actuator_outputs_sim.output[i] = (output - disarmed) / (max - disarmed); + + } else { + // Scale everything else to [-1, 1] + const float pwm_center = (max + min) / 2.f; + const float pwm_delta = (max - min) / 2.f; + actuator_outputs_sim.output[i] = (output - pwm_center) / pwm_delta; + } + } + + actuator_outputs_sim.timestamp = hrt_absolute_time(); + _actuator_outputs_sim_pub.publish(actuator_outputs_sim); + } + + ret = true; } return ret; @@ -482,8 +531,14 @@ int PWMESC::start(int argc, char *argv[]) { int ret = 0; + int32_t hitl_mode; - if (PWMESC::getInstance() == nullptr) { + if (param_get(param_find("SYS_HITL"), &hitl_mode) != PX4_OK) { + PX4_ERR("Can't read parameter SYS_HITL"); + return -1; + } + + if (PWMESC::getInstance(true, hitl_mode != 0) == nullptr) { PX4_ERR("Driver allocation failed"); return -1; } @@ -493,18 +548,6 @@ PWMESC::start(int argc, char *argv[]) return -1; } - bool hitl_mode = false; - - /* Check if started in hil mode */ - for (int extra_args = 1; extra_args < argc; extra_args++) { - if (!strcmp(argv[extra_args], "hil")) { - hitl_mode = true; - - } else if (argv[extra_args][0] != '\0') { - PX4_WARN("unknown argument: %s", argv[extra_args]); - } - } - if (OK != PWMESC::getInstance()->init(hitl_mode)) { delete PWMESC::getInstance(); PX4_ERR("Driver init failed"); @@ -518,7 +561,7 @@ int PWMESC::stop() { if (PWMESC::getInstance() == nullptr) { - PX4_ERR("Driver allocation failed"); + PX4_ERR("Not started"); return -1; } @@ -533,22 +576,66 @@ PWMESC::stop() return 0; } +int PWMESC::printStatus() +{ + _mixing_output.printStatus(); + return 0; +} + +int +PWMESC::status() +{ + if (PWMESC::getInstance() == nullptr) { + PX4_INFO("Not started"); + return 0; + } + + if (!PWMESC::getInstance()->running()) { + PX4_ERR("Not running"); + return -1; + } + + return PWMESC::getInstance()->printStatus(); +} + +int PWMESC::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for PWM outputs. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pwm_esc", "driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + int pwm_esc_main(int argc, char *argv[]) { /* check for sufficient number of arguments */ if (argc < 2) { - PX4_ERR("Need a command, try 'start' / 'stop'"); + PWMESC::print_usage("Need a command"); return -1; } if (!strcmp(argv[1], "start")) { return PWMESC::start(argc - 1, argv + 1); - } - - if (!strcmp(argv[1], "stop")) { + } else if (!strcmp(argv[1], "stop")) { return PWMESC::stop(); + } else if (!strcmp(argv[1], "status")) { + return PWMESC::status(); + } else { + PWMESC::print_usage("Invalid command"); } - return 0; + return -1; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 36958cbe69b2..3bbbeb59a74b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -313,6 +313,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_gimbal_device_attitude_status(msg); break; + case MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS: + handle_message_actuator_output_status(msg); + break; + + case MAVLINK_MSG_ID_ESC_STATUS: + handle_message_esc_status(msg); + break; + default: break; } @@ -2078,8 +2086,56 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) mavlink_msg_heartbeat_decode(msg, &hb); const bool same_system = (msg->sysid == mavlink_system.sysid); + const bool redundant_fc = same_system && msg->compid >= MAV_COMP_ID_AUTOPILOT1 + && msg->compid < MAV_COMP_ID_AUTOPILOT1 + 4; // TODO: MAX_REDUNDANT_AUTOPILOTS + + if (redundant_fc) { + int fc_idx = msg->compid - MAV_COMP_ID_AUTOPILOT1; + + _redundant_status[fc_idx].system_id = msg->sysid; + _redundant_status[fc_idx].component_id = msg->compid; + + + /* publish the redundant status topic */ + + uint8_t arming_state; + + switch (hb.system_status) { + case MAV_STATE_ACTIVE: + case MAV_STATE_CRITICAL: + arming_state = vehicle_status_s::ARMING_STATE_ARMED; + break; + + case MAV_STATE_STANDBY: + arming_state = vehicle_status_s::ARMING_STATE_STANDBY; + break; + + case MAV_STATE_EMERGENCY: + case MAV_STATE_FLIGHT_TERMINATION: + arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; + break; + + case MAV_STATE_POWEROFF: + arming_state = vehicle_status_s::ARMING_STATE_SHUTDOWN; + break; + + default: + arming_state = vehicle_status_s::ARMING_STATE_INIT; + break; + } - if (same_system || hb.type == MAV_TYPE_GCS) { + _redundant_status[fc_idx].arming_state = arming_state; + + _redundant_status[fc_idx].calibration_enabled = hb.system_status == MAV_STATE_CALIBRATING; + + _redundant_status[fc_idx].hil_state = hb.base_mode & MAV_MODE_FLAG_HIL_ENABLED ? vehicle_status_s::HIL_STATE_ON : + vehicle_status_s::HIL_STATE_OFF; + + _redundant_status[fc_idx].timestamp = hrt_absolute_time(); + + _redundant_status_pub[fc_idx].publish(_redundant_status[fc_idx]); + + } else if (same_system || hb.type == MAV_TYPE_GCS) { camera_status_s camera_status{}; @@ -3066,6 +3122,52 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t _gimbal_device_attitude_status_pub.publish(gimbal_attitude_status); } +void +MavlinkReceiver::handle_message_actuator_output_status(mavlink_message_t *msg) +{ + int fc_idx = msg->compid - MAV_COMP_ID_AUTOPILOT1; + mavlink_actuator_output_status_t actuator_output_status_msg; + mavlink_msg_actuator_output_status_decode(msg, &actuator_output_status_msg); + actuator_outputs_s actuator_outputs{}; + + if (fc_idx < vehicle_status_s::MAX_REDUNDANT_CONTROLLERS) { + actuator_outputs.timestamp = hrt_absolute_time(); + actuator_outputs.noutputs = actuator_output_status_msg.active; + static constexpr size_t mavlink_actuator_output_status_size = sizeof(actuator_output_status_msg.actuator) / sizeof( + actuator_output_status_msg.actuator[0]); + static constexpr size_t actuator_outputs_size = sizeof(actuator_outputs.output) / sizeof(actuator_outputs.output[0]); + + for (size_t i = 0; i < math::min(mavlink_actuator_output_status_size, actuator_outputs_size); i++) { + actuator_outputs.output[i] = actuator_output_status_msg.actuator[i]; + } + + _redundant_actuator_outputs_pub[fc_idx].publish(actuator_outputs); + } +} + +void +MavlinkReceiver::handle_message_esc_status(mavlink_message_t *msg) +{ + static constexpr int batch_size = MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN; + mavlink_esc_status_t esc_status_msg; + mavlink_msg_esc_status_decode(msg, &esc_status_msg); + esc_status_s esc_status{}; + + esc_status.timestamp = hrt_absolute_time(); + + /* Status is sent in batches, esc_idx is the actual esc index, idx is the index within the received batch */ + + size_t esc_idx = esc_status_msg.index; + + for (int idx = 0; idx < batch_size && esc_idx < sizeof(esc_status.esc) / sizeof(esc_status.esc[0]); idx++, esc_idx++) { + esc_status.esc[esc_idx].esc_rpm = esc_status_msg.rpm[idx]; + esc_status.esc[esc_idx].esc_voltage = esc_status_msg.voltage[idx]; + esc_status.esc[esc_idx].esc_current = esc_status_msg.current[idx]; + } + + _esc_status_pub.publish(esc_status); +} + void MavlinkReceiver::run() { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index b47ca22c564b..91379552c9f7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -70,6 +70,7 @@ #include #include #include +#include #include #include #include @@ -111,6 +112,7 @@ #include #include #include +#include #if !defined(CONSTRAINED_FLASH) # include @@ -202,6 +204,8 @@ class MavlinkReceiver : public ModuleParams void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg); void handle_message_gimbal_device_information(mavlink_message_t *msg); void handle_message_gimbal_device_attitude_status(mavlink_message_t *msg); + void handle_message_actuator_output_status(mavlink_message_t *msg); + void handle_message_esc_status(mavlink_message_t *msg); #if !defined(CONSTRAINED_FLASH) void handle_message_debug(mavlink_message_t *msg); @@ -322,6 +326,10 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::Publication _trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; uORB::Publication _trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; + uORB::Publication _redundant_status_pub[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {ORB_ID(redundant_status0), ORB_ID(redundant_status1)}; + vehicle_status_s _redundant_status[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {}; + uORB::Publication _redundant_actuator_outputs_pub[vehicle_status_s::MAX_REDUNDANT_CONTROLLERS] {ORB_ID(redundant_actuator_outputs0), ORB_ID(redundant_actuator_outputs1)}; + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; #if !defined(CONSTRAINED_FLASH) uORB::Publication _debug_array_pub {ORB_ID(debug_array)}; diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 26d966a85ac5..18665491f03e 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -122,7 +122,8 @@ class MavlinkStreamHeartbeat : public MavlinkStream } // system_status overrides - if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown + if (actuator_armed.force_failsafe || (actuator_armed.lockdown + && vehicle_status.hil_state == vehicle_status_s::HIL_STATE_OFF) || actuator_armed.manual_lockdown || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { system_status = MAV_STATE_FLIGHT_TERMINATION; diff --git a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp index 182c1d40fab4..77732c34cbb9 100644 --- a/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp +++ b/src/modules/mavlink/streams/HIL_ACTUATOR_CONTROLS.hpp @@ -38,7 +38,6 @@ #include #include #include -#include class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams { @@ -62,51 +61,11 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams ModuleParams(nullptr) { _act_sub = uORB::Subscription{ORB_ID(actuator_outputs_sim)}; - - for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) { - char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1); - param_get(param_find(param_name), &_output_functions[i]); - } } uORB::Subscription _act_sub{ORB_ID(actuator_outputs)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; - - int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {}; - - void send_esc_telemetry(mavlink_hil_actuator_controls_t &hil_act_control, vehicle_status_s &vehicle_status) - { - esc_status_s esc_status{}; - esc_status.timestamp = hrt_absolute_time(); - const int max_esc_count = math::min(actuator_outputs_s::NUM_ACTUATOR_OUTPUTS, esc_status_s::CONNECTED_ESC_MAX); - - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - int max_esc_index = 0; - - for (int i = 0; i < max_esc_count; i++) { - if (_output_functions[i] != 0) { - max_esc_index = i; - } - - esc_status.esc[i].actuator_function = _output_functions[i]; // TODO: this should be in pwm_sim... - esc_status.esc[i].timestamp = esc_status.timestamp; - esc_status.esc[i].esc_errorcount = 0; // TODO - esc_status.esc[i].esc_voltage = 11.1f + math::abs_t(hil_act_control.controls[i]) * 3.0f; // TODO: magic number - esc_status.esc[i].esc_current = armed ? 1.0f + math::abs_t(hil_act_control.controls[i]) * 15.0f : - 0.0f; // TODO: magic number - esc_status.esc[i].esc_rpm = hil_act_control.controls[i] * 6000; // TODO: magic number - esc_status.esc[i].esc_temperature = 20.0f + math::abs_t(hil_act_control.controls[i]) * 40.0f; - } - - esc_status.esc_count = max_esc_index + 1; - esc_status.esc_armed_flags = (1u << esc_status.esc_count) - 1; - esc_status.esc_online_flags = (1u << esc_status.esc_count) - 1; - - _esc_status_pub.publish(esc_status); - } bool send() override { @@ -159,8 +118,6 @@ class MavlinkStreamHILActuatorControls : public MavlinkStream, ModuleParams mavlink_msg_hil_actuator_controls_send_struct(_mavlink->get_channel(), &msg); - send_esc_telemetry(msg, status); - return true; } diff --git a/src/modules/redundancy b/src/modules/redundancy new file mode 160000 index 000000000000..bec5f76a2a2b --- /dev/null +++ b/src/modules/redundancy @@ -0,0 +1 @@ +Subproject commit bec5f76a2a2b2d9941b2d1c9dbaa8ee6a94a52a2 diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp index c11a5eb365fe..fec88dd2f493 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp @@ -44,6 +44,12 @@ PWMSim::PWMSim(bool hil_mode_enabled) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { + for (int i = 0; i < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS; ++i) { + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1); + param_get(param_find(param_name), &_output_functions[i]); + } + _mixing_output.setAllDisarmedValues(PWM_SIM_DISARMED_MAGIC); _mixing_output.setAllFailsafeValues(PWM_SIM_FAILSAFE_MAGIC); _mixing_output.setAllMinValues(PWM_SIM_PWM_MIN_MAGIC); @@ -58,6 +64,33 @@ PWMSim::~PWMSim() perf_free(_interval_perf); } +void PWMSim::send_esc_telemetry(hrt_abstime ts, float actuator_outputs[MAX_ACTUATORS], + bool actuator_armed[MAX_ACTUATORS], unsigned num_outputs) +{ + esc_status_s esc_status{}; + + if (num_outputs > esc_status_s::CONNECTED_ESC_MAX) { + num_outputs = esc_status_s::CONNECTED_ESC_MAX; + } + + for (unsigned i = 0; i < num_outputs; i++) { + esc_status.esc[i].actuator_function = _output_functions[i]; + esc_status.esc[i].timestamp = ts; + esc_status.esc[i].esc_errorcount = 0; // TODO + esc_status.esc[i].esc_voltage = 11.1f + math::abs_t(actuator_outputs[i]) * 3.0f; // TODO: magic number + esc_status.esc[i].esc_current = actuator_armed[i] ? 1.0f + math::abs_t(actuator_outputs[i]) * 15.0f : + 0.0f; // TODO: magic number + esc_status.esc[i].esc_rpm = actuator_outputs[i] * 6000; // TODO: magic number + esc_status.esc[i].esc_temperature = 20.0f + math::abs_t(actuator_outputs[i]) * 40.0f; + } + + esc_status.esc_count = num_outputs; + esc_status.esc_armed_flags = (1u << esc_status.esc_count) - 1; + esc_status.esc_online_flags = (1u << esc_status.esc_count) - 1; + + _esc_status_pub.publish(esc_status); +} + bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { @@ -65,6 +98,7 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un if (num_control_groups_updated > 0) { actuator_outputs_s actuator_outputs{}; actuator_outputs.noutputs = num_outputs; + bool actuator_armed[MAX_ACTUATORS] {}; const uint32_t reversible_outputs = _mixing_output.reversibleOutputs(); @@ -75,6 +109,8 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un bool is_reversible = reversible_outputs & (1u << i); float output = outputs[i]; + actuator_armed[i] = true; + if (((int)function >= (int)OutputFunction::Motor1 && (int)function <= (int)OutputFunction::MotorMax) && !is_reversible) { // Scale non-reversible motors to [0, 1] @@ -89,8 +125,13 @@ bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], un } } - actuator_outputs.timestamp = hrt_absolute_time(); + hrt_abstime now = hrt_absolute_time(); + actuator_outputs.timestamp = now; _actuator_outputs_sim_pub.publish(actuator_outputs); + + // TODO: Add an option to receive ESC telemetry from gazebo via Mavlink and skip this one + send_esc_telemetry(now, actuator_outputs.output, actuator_armed, num_outputs); + return true; } diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp index b16a9fac5e75..fbba3601dd65 100644 --- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp +++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #if defined(CONFIG_ARCH_BOARD_PX4_SITL) #define PARAM_PREFIX "PWM_MAIN" @@ -76,6 +77,9 @@ class PWMSim : public ModuleBase, public OutputModuleInterface private: void Run() override; + void send_esc_telemetry(hrt_abstime now, float actuator_outputs[MAX_ACTUATORS], bool actuator_armed[MAX_ACTUATORS], + unsigned num_outputs); + static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900; static constexpr uint16_t PWM_SIM_FAILSAFE_MAGIC = 600; static constexpr uint16_t PWM_SIM_PWM_MIN_MAGIC = 1000; @@ -85,7 +89,8 @@ class PWMSim : public ModuleBase, public OutputModuleInterface uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Publication _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)}; - + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; + int32_t _output_functions[actuator_outputs_s::NUM_ACTUATOR_OUTPUTS] {}; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; }; diff --git a/src/systemcmds/netconfig/netconfig.cpp b/src/systemcmds/netconfig/netconfig.cpp index 617d260d06d4..06782506ee8f 100644 --- a/src/systemcmds/netconfig/netconfig.cpp +++ b/src/systemcmds/netconfig/netconfig.cpp @@ -51,25 +51,42 @@ int netconfig_main(int argc, char *argv[]) { struct in_addr addr; int32_t mav_id; + int32_t mav_comp_id; + int32_t ip; const char ifname[] = CONFIG_NETCONFIG_IFNAME; param_get(param_find("MAV_SYS_ID"), &mav_id); + param_get(param_find("MAV_COMP_ID"), &mav_comp_id); - if (mav_id < 1) { + if (mav_id < 1 || mav_id > 63 || mav_comp_id < 1 || mav_comp_id > 4) { return PX4_ERROR; } - /* IP: CONFIG_NETCONFIG_IPSUBNET + mav_id */ - - addr.s_addr = CONFIG_NETCONFIG_IPSUBNET; - - mav_id += 100; - - if (mav_id > 253) { - return PX4_ERROR; - } - - addr.s_addr |= ((uint32_t)mav_id << 24); + /* IP: CONFIG_NETCONFIG_IPSUBNET 3 bytes + * last byte: + * 2 high bits: mav_comp_id - 1 + * 6 low bits: mav_id + */ + + addr.s_addr = CONFIG_NETCONFIG_IPSUBNET & 0xffffff; + + /* Autopilot IP examples: + MAV_ID 1: + 192.168.202.1 : primary FC1 (comp_id 1) + 192.168.202.65 : redundant FC2 (comp_id 2) + 192.168.202.129 : redundant FC3 (comp_id 3) + 192.168.202.193 : redundant FC4 (comp_id 4) + MAV_ID 2: + 192.168.202.2 : primary FC1 (comp_id 1) + 192.168.202.66 : redundant FC2 (comp_id 2) + 192.168.202.130 : redundant FC3 (comp_id 3) + 192.168.202.194 : redundant FC4 (comp_id 4) + */ + + mav_comp_id--; + ip = ((mav_comp_id & 0x3) << 6) + mav_id; + + addr.s_addr |= ip << 24; netlib_set_ipv4addr(ifname, &addr); /* GW */ diff --git a/ssrc_config/config_hitl_eth_gzsim.txt b/ssrc_config/config_hitl_eth_gzsim.txt index 9544446a4adb..e82e418ec6d3 100644 --- a/ssrc_config/config_hitl_eth_gzsim.txt +++ b/ssrc_config/config_hitl_eth_gzsim.txt @@ -7,7 +7,7 @@ # Set HITL related flag param set SYS_HITL 1 -param set MAV_HITL_SHOME 1 +param set MAV_HITL_SHOME 0 param set SENS_EN_GPSSIM 1 param set SENS_EN_BAROSIM 1 param set SENS_EN_MAGSIM 1 @@ -23,15 +23,15 @@ param set NAV_RCL_ACT 0 param set COM_RC_IN_MODE 1 # EKF2 -param set EKF2_MULTI_IMU 3 - -# Sensor -param set CAL_ACC0_ID 1310988 -param set CAL_GYRO0_ID 1310988 -param set CAL_ACC1_ID 1310996 -param set CAL_GYRO1_ID 1310996 -param set CAL_ACC2_ID 1311004 -param set CAL_GYRO2_ID 1311004 -param set CAL_MAG1_ID 197388 -param set CAL_MAG1_PRIO 50 +param set EKF2_MULTI_IMU 1 +# Need mavlink version 2 +param set MAV_PROTO_VER 2 + +# Disable PX4 IO if enabled +# In HITL PX4 IO publishes actuator outputs, but +# it is also published by PWMSIM. Disble PX4IO for now +# to avoid this. NOTE: Thus breaks safety button and +# RC controller in HITL! + +param set SYS_USE_IO 0