diff --git a/src/modules/gyro_fft/GyroFFT.hpp b/src/modules/gyro_fft/GyroFFT.hpp index 3a555d9297..1c7d7da8ea 100644 --- a/src/modules/gyro_fft/GyroFFT.hpp +++ b/src/modules/gyro_fft/GyroFFT.hpp @@ -111,7 +111,8 @@ private: return (_gyro_data_buffer_x && _gyro_data_buffer_y && _gyro_data_buffer_z && _hanning_window && _fft_input_buffer - && _fft_outupt_buffer); + && _fft_outupt_buffer + && _peak_magnitudes_all); } uORB::Publication _sensor_gyro_fft_pub{ORB_ID(sensor_gyro_fft)}; diff --git a/src/modules/mavlink/mavlink_command_sender.cpp b/src/modules/mavlink/mavlink_command_sender.cpp index 4ce6a078e3..f32448cff5 100644 --- a/src/modules/mavlink/mavlink_command_sender.cpp +++ b/src/modules/mavlink/mavlink_command_sender.cpp @@ -52,11 +52,19 @@ void MavlinkCommandSender::initialize() if (_instance == nullptr) { _instance = new MavlinkCommandSender(); + + if (_instance == nullptr) { + PX4_ERR("MavlinkCommandSender alloc failed"); + } } } MavlinkCommandSender &MavlinkCommandSender::instance() { + if (_instance == nullptr) { + initialize(); + } + return *_instance; } diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 4894358ed1..f638099c77 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1307,6 +1307,12 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) /* copy stream name */ unsigned n = strlen(stream_name) + 1; char *s = new char[n]; + + if (s == nullptr) { + PX4_ERR("stream allocation failed"); + return; + } + strcpy(s, stream_name); /* set subscription task */ @@ -2622,7 +2628,7 @@ Mavlink::task_main(int argc, char *argv[]) _receiver.stop(); - delete _subscribe_to_stream; + delete[] _subscribe_to_stream; _subscribe_to_stream = nullptr; /* delete streams */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 60841a34e6..c391d40b2d 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -716,6 +716,12 @@ void MavlinkParametersManager::enque_uavcan_request(uavcan_parameter_request_s * } _uavcan_open_request_list_item *new_reqest = new _uavcan_open_request_list_item; + + if (new_reqest == nullptr) { + PX4_ERR("uavcan request alloc failed"); + return; + } + new_reqest->req = *req; new_reqest->next = nullptr; diff --git a/src/modules/mc_nn_control/mc_nn_control.cpp b/src/modules/mc_nn_control/mc_nn_control.cpp index 360d63c416..99ad9e0393 100644 --- a/src/modules/mc_nn_control/mc_nn_control.cpp +++ b/src/modules/mc_nn_control/mc_nn_control.cpp @@ -72,6 +72,10 @@ MulticopterNeuralNetworkControl::MulticopterNeuralNetworkControl() : MulticopterNeuralNetworkControl::~MulticopterNeuralNetworkControl() { + delete _interpreter; + _interpreter = nullptr; + _input_tensor = nullptr; + _output_tensor = nullptr; perf_free(_loop_perf); } @@ -88,6 +92,14 @@ bool MulticopterNeuralNetworkControl::init() int MulticopterNeuralNetworkControl::InitializeNetwork() { + if (_interpreter != nullptr) { + delete _interpreter; + _interpreter = nullptr; + } + + _input_tensor = nullptr; + _output_tensor = nullptr; + // Initialize the neural network const tflite::Model *control_model = ::tflite::GetModel(control_net_tflite); @@ -103,11 +115,18 @@ int MulticopterNeuralNetworkControl::InitializeNetwork() static uint8_t tensor_arena[kTensorArenaSize]; _interpreter = new tflite::MicroInterpreter(control_model, resolver, tensor_arena, kTensorArenaSize); + if (_interpreter == nullptr) { + PX4_ERR("interpreter alloc failed"); + return -1; + } + // Allocate memory for the model's tensors TfLiteStatus allocate_status = _interpreter->AllocateTensors(); if (allocate_status != kTfLiteOk) { PX4_ERR("AllocateTensors() failed"); + delete _interpreter; + _interpreter = nullptr; return -1; } @@ -115,6 +134,8 @@ int MulticopterNeuralNetworkControl::InitializeNetwork() if (_input_tensor == nullptr) { PX4_ERR("Input tensor is null"); + delete _interpreter; + _interpreter = nullptr; return -1; } diff --git a/src/modules/mc_nn_control/mc_nn_control.hpp b/src/modules/mc_nn_control/mc_nn_control.hpp index 112cfe9555..cb8fd6049e 100644 --- a/src/modules/mc_nn_control/mc_nn_control.hpp +++ b/src/modules/mc_nn_control/mc_nn_control.hpp @@ -149,9 +149,9 @@ private: uint8 _mode_request_id{231}; //Random value int8 _arming_check_id{-1}; int8 _mode_id{-1}; - tflite::MicroInterpreter *_interpreter; - TfLiteTensor *_input_tensor; - TfLiteTensor *_output_tensor; + tflite::MicroInterpreter *_interpreter{nullptr}; + TfLiteTensor *_input_tensor{nullptr}; + TfLiteTensor *_output_tensor{nullptr}; float _input_data[15]; trajectory_setpoint_s _trajectory_setpoint; vehicle_angular_velocity_s _angular_velocity; diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 174506577b..040eb4bc3c 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -527,6 +527,12 @@ Replay::readAndAddSubscription(std::ifstream &file, uint16_t msg_size) } Subscription *subscription = new Subscription(); + + if (subscription == nullptr) { + delete compat; + return ReadAndAndAddSubResult::kFailure; + } + subscription->orb_meta = orb_meta; subscription->multi_id = multi_id; subscription->compat = compat; diff --git a/src/modules/sensors/data_validator/DataValidatorGroup.cpp b/src/modules/sensors/data_validator/DataValidatorGroup.cpp index eabf61788c..d908a1622b 100644 --- a/src/modules/sensors/data_validator/DataValidatorGroup.cpp +++ b/src/modules/sensors/data_validator/DataValidatorGroup.cpp @@ -54,6 +54,11 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) for (unsigned i = 0; i < siblings; i++) { next = new DataValidator(); + if (next == nullptr) { + PX4_ERR("alloc failed"); + break; + } + if (i == 0) { _first = next; @@ -64,7 +69,7 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings) prev = next; } - _last = next; + _last = prev; if (_first) { _timeout_interval_us = _first->get_timeout(); @@ -89,8 +94,14 @@ DataValidator *DataValidatorGroup::add_new_validator() return nullptr; } - _last->setSibling(validator); - _last = validator; + if (_last) { + _last->setSibling(validator); + _last = validator; + + } else { + _first = _last = validator; + } + _last->set_timeout(_timeout_interval_us); return _last; } diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index c579a9ba4b..5a603af33c 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -483,6 +483,11 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) if (_sensor_gps_pubs[i] == nullptr) { _sensor_gps_pubs[i] = new uORB::PublicationMulti {ORB_ID(sensor_gps)}; + + if (_sensor_gps_pubs[i] == nullptr) { + break; + } + _gps_ids[i] = hil_gps.id; device::Device::DeviceId device_id; @@ -1588,6 +1593,11 @@ int SimulatorMavlink::publish_distance_topic(const mavlink_distance_sensor_t *di if (_dist_pubs[i] == nullptr) { _dist_pubs[i] = new uORB::PublicationMulti {ORB_ID(distance_sensor)}; + + if (_dist_pubs[i] == nullptr) { + break; + } + _dist_sensor_ids[i] = dist.device_id; _dist_pubs[i]->publish(dist); break; diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index 26628a486b..64601a3254 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -884,12 +884,12 @@ bool UxrceddsClient::setBaudrate(int fd, unsigned baud) bool UxrceddsClient::add_replier(SrvBase *replier) { - if (_num_of_repliers < MAX_NUM_REPLIERS) { - _repliers[_num_of_repliers] = replier; - - _num_of_repliers++; + if (replier == nullptr || _num_of_repliers >= MAX_NUM_REPLIERS) { + return true; } + _repliers[_num_of_repliers] = replier; + _num_of_repliers++; return false; } 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 43f37b3c60..f1db4a0b58 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -79,6 +79,12 @@ VtolAttitudeControl::VtolAttitudeControl() : exit_and_cleanup(desc); } + if (_vtol_type == nullptr) { + PX4_ERR("vtol type allocation failed"); + exit_and_cleanup(desc); + return; + } + _flaps_setpoint_pub.advertise(); _spoilers_setpoint_pub.advertise(); _vtol_vehicle_status_pub.advertise(); @@ -302,6 +308,12 @@ VtolAttitudeControl::Run() #endif // !ENABLE_LOCKSTEP_SCHEDULER + if (_vtol_type == nullptr) { + PX4_ERR("vtol type unavailable"); + exit_and_cleanup(desc); + return; + } + if (!_initialized) { if (_vtol_type->init()) {