From 7d8fbcfbfd85ac61ea81ea4bdb5f5b8b342a87df Mon Sep 17 00:00:00 2001 From: Brandon Sutherland Date: Fri, 12 Jul 2024 09:08:45 -0600 Subject: [PATCH] added missing brackets and fixed weird formatting in .clang-format --- .clang-format | 2 +- comms/mavlink/mavlink.cpp | 26 +++++++++++-------- include/sensors.h | 6 +++-- include/util.h | 2 +- lib/turbomath/turbomath.cpp | 52 +++++++++++++++++++++---------------- src/comm_manager.cpp | 45 ++++++++++++++++---------------- src/command_manager.cpp | 20 +++++++------- src/controller.cpp | 22 +++++++++------- src/estimator.cpp | 4 +-- src/mixer.cpp | 2 +- src/param.cpp | 24 ++++++++++------- src/rc.cpp | 32 +++++++++++------------ src/rosflight.cpp | 2 +- src/sensors.cpp | 25 +++++++++--------- src/state_manager.cpp | 47 +++++++++++++++++++-------------- test/common.cpp | 10 ++++--- test/test_board.cpp | 2 +- test/turbotrig_test.cpp | 6 +++-- 18 files changed, 181 insertions(+), 148 deletions(-) diff --git a/.clang-format b/.clang-format index 536a1802..ae8019dc 100644 --- a/.clang-format +++ b/.clang-format @@ -11,7 +11,7 @@ AllowAllParametersOfDeclarationOnNextLine: false AllowShortBlocksOnASingleLine: Always AllowShortCaseLabelsOnASingleLine: false AllowShortFunctionsOnASingleLine: All -AllowShortIfStatementsOnASingleLine: Always +AllowShortIfStatementsOnASingleLine: AllIfsAndElse AllowShortLambdasOnASingleLine: All AllowShortLoopsOnASingleLine: true AlwaysBreakAfterReturnType: None diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp index aa18e516..9217c89b 100644 --- a/comms/mavlink/mavlink.cpp +++ b/comms/mavlink/mavlink.cpp @@ -49,8 +49,9 @@ void Mavlink::init(uint32_t baud_rate, uint32_t dev) void Mavlink::receive(void) { while (board_.serial_bytes_available()) { - if (mavlink_parse_char(MAVLINK_COMM_0, board_.serial_read(), &in_buf_, &status_)) + if (mavlink_parse_char(MAVLINK_COMM_0, board_.serial_read(), &in_buf_, &status_)) { handle_mavlink_message(); + } } } @@ -334,7 +335,7 @@ void Mavlink::handle_msg_param_request_list(const mavlink_message_t * const msg) mavlink_param_request_list_t list; mavlink_msg_param_request_list_decode(msg, &list); - if (listener_ != nullptr) listener_->param_request_list_callback(list.target_system); + if (listener_ != nullptr) { listener_->param_request_list_callback(list.target_system); } } void Mavlink::handle_msg_param_request_read(const mavlink_message_t * const msg) @@ -342,8 +343,9 @@ void Mavlink::handle_msg_param_request_read(const mavlink_message_t * const msg) mavlink_param_request_read_t read; mavlink_msg_param_request_read_decode(msg, &read); - if (listener_ != nullptr) + if (listener_ != nullptr) { listener_->param_request_read_callback(read.target_system, read.param_id, read.param_index); + } } void Mavlink::handle_msg_param_set(const mavlink_message_t * const msg) @@ -357,12 +359,14 @@ void Mavlink::handle_msg_param_set(const mavlink_message_t * const msg) switch (param.type) { case MAV_PARAM_TYPE_INT32: - if (listener_ != nullptr) + if (listener_ != nullptr) { listener_->param_set_int_callback(set.target_system, set.param_id, param.param_int32); + } break; case MAV_PARAM_TYPE_REAL32: - if (listener_ != nullptr) + if (listener_ != nullptr) { listener_->param_set_float_callback(set.target_system, set.param_id, param.param_float); + } break; default: // unsupported parameter type @@ -419,7 +423,7 @@ void Mavlink::handle_msg_rosflight_cmd(const mavlink_message_t * const msg) return; } - if (listener_ != nullptr) listener_->command_callback(command); + if (listener_ != nullptr) { listener_->command_callback(command); } } void Mavlink::handle_msg_rosflight_aux_cmd(const mavlink_message_t * const msg) @@ -451,7 +455,7 @@ void Mavlink::handle_msg_rosflight_aux_cmd(const mavlink_message_t * const msg) } // call callback after all channels have been repacked - if (listener_ != nullptr) listener_->aux_command_callback(command); + if (listener_ != nullptr) { listener_->aux_command_callback(command); } } void Mavlink::handle_msg_timesync(const mavlink_message_t * const msg) @@ -459,7 +463,7 @@ void Mavlink::handle_msg_timesync(const mavlink_message_t * const msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); - if (listener_ != nullptr) listener_->timesync_callback(tsync.tc1, tsync.ts1); + if (listener_ != nullptr) { listener_->timesync_callback(tsync.tc1, tsync.ts1); } } void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg) @@ -493,7 +497,7 @@ void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg) control.z.valid = !(ctrl.ignore & IGNORE_VALUE3); control.F.valid = !(ctrl.ignore & IGNORE_VALUE4); - if (listener_ != nullptr) listener_->offboard_control_callback(control); + if (listener_ != nullptr) { listener_->offboard_control_callback(control); } } void Mavlink::handle_msg_external_attitude(const mavlink_message_t * const msg) @@ -507,7 +511,7 @@ void Mavlink::handle_msg_external_attitude(const mavlink_message_t * const msg) q_extatt.y = q_msg.qy; q_extatt.z = q_msg.qz; - if (listener_ != nullptr) listener_->external_attitude_callback(q_extatt); + if (listener_ != nullptr) { listener_->external_attitude_callback(q_extatt); } } void Mavlink::handle_msg_heartbeat(const mavlink_message_t * const msg) @@ -515,7 +519,7 @@ void Mavlink::handle_msg_heartbeat(const mavlink_message_t * const msg) // none of the information from the heartbeat is used (void) msg; - if (listener_ != nullptr) listener_->heartbeat_callback(); + if (listener_ != nullptr) { listener_->heartbeat_callback(); } } void Mavlink::handle_mavlink_message() diff --git a/include/sensors.h b/include/sensors.h index 00914614..6f41f1e4 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -193,9 +193,11 @@ class Sensors : public ParamListenerInterface inline bool should_send_imu_data(void) { - if (imu_data_sent_) return false; - else + if (imu_data_sent_) { + return false; + } else { imu_data_sent_ = true; + } return true; } diff --git a/include/util.h b/include/util.h index 6cb4ac05..ed75a5e7 100644 --- a/include/util.h +++ b/include/util.h @@ -73,7 +73,7 @@ inline uint16_t checksum_fletcher16(const uint8_t * src, size_t len, bool finali uint16_t checksum = c1 << 8 | c2; - if (finalize && checksum == 0) checksum = 0xFFFF; + if (finalize && checksum == 0) { checksum = 0xFFFF; } return checksum; } diff --git a/lib/turbomath/turbomath.cpp b/lib/turbomath/turbomath.cpp index 68bbbba9..7064056c 100644 --- a/lib/turbomath/turbomath.cpp +++ b/lib/turbomath/turbomath.cpp @@ -335,28 +335,30 @@ float cos(float x) { return sin(M_PI / 2.0 - x); } float sin(float x) { // wrap down to +/x PI - while (x > M_PI) x -= 2.0 * M_PI; + while (x > M_PI) { x -= 2.0 * M_PI; } - while (x <= -M_PI) x += 2.0 * M_PI; + while (x <= -M_PI) { x += 2.0 * M_PI; } // sin is symmetric - if (x < 0) return -1.0 * sin(-x); + if (x < 0) { return -1.0 * sin(-x); } // wrap onto (0, PI) - if (x > M_PI) return -1.0 * sin(x - M_PI); + if (x > M_PI) { return -1.0 * sin(x - M_PI); } // Now, all we have left is the range 0 to PI, use the lookup table float t = (x - sin_min_x) / (sin_max_x - sin_min_x) * static_cast(sin_num_entries); int16_t index = static_cast(t); float delta_x = t - index; - if (index >= sin_num_entries) return sin_lookup_table[sin_num_entries - 1] / sin_scale_factor; - else if (index < sin_num_entries - 1) + if (index >= sin_num_entries) { + return sin_lookup_table[sin_num_entries - 1] / sin_scale_factor; + } else if (index < sin_num_entries - 1) { return sin_lookup_table[index] / sin_scale_factor + delta_x * (sin_lookup_table[index + 1] - sin_lookup_table[index]) / sin_scale_factor; - else + } else { return sin_lookup_table[index] / sin_scale_factor + delta_x * (sin_lookup_table[index] - sin_lookup_table[index - 1]) / sin_scale_factor; + } } float atan(float x) @@ -370,13 +372,15 @@ float atan(float x) int16_t index = static_cast(t); float delta_x = t - index; - if (index >= atan_num_entries) return atan_lookup_table[atan_num_entries - 1] / atan_scale_factor; - else if (index < atan_num_entries - 1) + if (index >= atan_num_entries) { + return atan_lookup_table[atan_num_entries - 1] / atan_scale_factor; + } else if (index < atan_num_entries - 1) { return atan_lookup_table[index] / atan_scale_factor + delta_x * (atan_lookup_table[index + 1] - atan_lookup_table[index]) / atan_scale_factor; - else + } else { return atan_lookup_table[index] / atan_scale_factor + delta_x * (atan_lookup_table[index] - atan_lookup_table[index - 1]) / atan_scale_factor; + } } float atan2(float y, float x) @@ -400,9 +404,7 @@ float atan2(float y, float x) } else { return arctan + M_PI; } - } - - else { + } else { return arctan; } } @@ -415,13 +417,15 @@ float asin(float x) int16_t index = static_cast(t); float delta_x = t - index; - if (index >= asin_num_entries) return asin_lookup_table[asin_num_entries - 1] / asin_scale_factor; - else if (index < asin_num_entries - 1) + if (index >= asin_num_entries) { + return asin_lookup_table[asin_num_entries - 1] / asin_scale_factor; + } else if (index < asin_num_entries - 1) { return asin_lookup_table[index] / asin_scale_factor + delta_x * (asin_lookup_table[index + 1] - asin_lookup_table[index]) / asin_scale_factor; - else + } else { return asin_lookup_table[index] / asin_scale_factor + delta_x * (asin_lookup_table[index] - asin_lookup_table[index - 1]) / asin_scale_factor; + } } float alt(float press) @@ -432,25 +436,29 @@ float alt(float press) int16_t index = static_cast(t); float delta_x = t - index; - if (index >= pressure_num_entries) + if (index >= pressure_num_entries) { return asin_lookup_table[pressure_num_entries - 1] / pressure_scale_factor; - else if (index < pressure_num_entries - 1) + } else if (index < pressure_num_entries - 1) { return pressure_lookup_table[index] / pressure_scale_factor + delta_x * (pressure_lookup_table[index + 1] - pressure_lookup_table[index]) / pressure_scale_factor; - else + } else { return pressure_lookup_table[index] / pressure_scale_factor + delta_x * (pressure_lookup_table[index] - pressure_lookup_table[index - 1]) / pressure_scale_factor; - } else + } + } else { return 0.0; + } } float fabs(float x) { - if (x < 0) return -x; - else + if (x < 0) { + return -x; + } else { return x; + } } float inv_sqrt(float x) diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp index 557c92c4..6f3092fb 100644 --- a/src/comm_manager.cpp +++ b/src/comm_manager.cpp @@ -126,7 +126,7 @@ void CommManager::send_param_value(uint16_t param_id) void CommManager::param_request_list_callback(uint8_t target_system) { - if (target_system == sysid_) send_params_index_ = 0; + if (target_system == sysid_) { send_params_index_ = 0; } } void CommManager::send_parameter_list() { send_params_index_ = 0; } @@ -138,7 +138,7 @@ void CommManager::param_request_read_callback(uint8_t target_system, const char uint16_t id = (param_index < 0) ? RF_.params_.lookup_param_id(param_name) : static_cast(param_index); - if (id < PARAMS_COUNT) send_param_value(id); + if (id < PARAMS_COUNT) { send_param_value(id); } } } @@ -227,8 +227,10 @@ void CommManager::timesync_callback(int64_t tc1, int64_t ts1) { uint64_t now_us = RF_.board_.clock_micros(); - if (tc1 == 0) // check that this is a request, not a response + if (tc1 == 0) { + // check that this is a request, not a response comm_link_.send_timesync(sysid_, static_cast(now_us) * 1000, ts1); + } } void CommManager::offboard_control_callback(const CommLinkInterface::OffboardControl & control) @@ -345,14 +347,16 @@ void CommManager::send_heartbeat(void) void CommManager::send_status(void) { - if (!initialized_) return; + if (!initialized_) { return; } uint8_t control_mode = 0; - if (RF_.params_.get_param_int(PARAM_FIXED_WING)) control_mode = MODE_PASS_THROUGH; - else if (RF_.command_manager_.combined_control().x.type == ANGLE) + if (RF_.params_.get_param_int(PARAM_FIXED_WING)) { + control_mode = MODE_PASS_THROUGH; + } else if (RF_.command_manager_.combined_control().x.type == ANGLE) { control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE; - else + } else { control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE; + } comm_link_.send_status( sysid_, RF_.state_manager_.state().armed, RF_.state_manager_.state().failsafe, @@ -471,40 +475,37 @@ void CommManager::stream(got_flags got) // Send out data - if (got.imu) // Nominally 400Hz - { + if (got.imu) { // Nominally 400Hz send_imu(); send_attitude(); static uint64_t ro_count = 0; - if (!((ro_count++) % 8)) send_output_raw(); // Raw output at 400Hz/8 = 50Hz + if (!((ro_count++) % 8)) { send_output_raw(); } // Raw output at 400Hz/8 = 50Hz } // Pitot sensor - if (got.diff_pressure) send_diff_pressure(); + if (got.diff_pressure) { send_diff_pressure(); } // Baro altitude - if (got.baro) send_baro(); + if (got.baro) { send_baro(); } // Magnetometer - if (got.mag) send_mag(); + if (got.mag) { send_mag(); } // Height above ground sensor (not enabled) - if (got.sonar) send_sonar(); + if (got.sonar) { send_sonar(); } // Battery V & I - if (got.battery) send_battery_status(); + if (got.battery) { send_battery_status(); } // GPS data (GNSS Packed) - if (got.gnss) send_gnss(); + if (got.gnss) { send_gnss(); } // GPS full data (not needed) - if (got.gnss_full) send_gnss_full(); - if (got.rc) send_rc_raw(); + if (got.gnss_full) { send_gnss_full(); } + if (got.rc) { send_rc_raw(); } { static uint64_t next_heartbeat = 0, next_status = 0; - if ((time_us) / 1000000 >= next_heartbeat) // 1 Hz - { + if ((time_us) / 1000000 >= next_heartbeat) { // 1 Hz send_heartbeat(); next_heartbeat = time_us / 1000000 + 1; } - if ((time_us) / 100000 >= next_status) // 10 Hz - { + if ((time_us) / 100000 >= next_status) { // 10 Hz send_status(); next_status = time_us / 100000 + 1; } diff --git a/src/command_manager.cpp b/src/command_manager.cpp index abab3aff..a66aaa64 100644 --- a/src/command_manager.cpp +++ b/src/command_manager.cpp @@ -83,10 +83,8 @@ void CommandManager::init_failsafe() { float failsafe_thr_param = RF_.params_.get_param_float(PARAM_FAILSAFE_THROTTLE); bool fixedwing = RF_.params_.get_param_int(PARAM_FIXED_WING); - if (!fixedwing - && (failsafe_thr_param < 0. - || failsafe_thr_param > 1.0)) // fixed wings always have 0 failsafe throttle - { + // fixed wings always have 0 failsafe throttle + if (!fixedwing && (failsafe_thr_param < 0. || failsafe_thr_param > 1.0)) { RF_.state_manager_.set_error(StateManager::ERROR_INVALID_FAILSAFE); failsafe_thr_param = 0.; } else { @@ -95,9 +93,11 @@ void CommandManager::init_failsafe() multirotor_failsafe_command_.F.value = failsafe_thr_param; - if (fixedwing) failsafe_command_ = fixedwing_failsafe_command_; - else + if (fixedwing) { + failsafe_command_ = fixedwing_failsafe_command_; + } else { failsafe_command_ = multirotor_failsafe_command_; + } } void CommandManager::interpret_rc(void) @@ -175,8 +175,7 @@ bool CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel) if ((RF_.rc_.switch_mapped(RC::SWITCH_ATT_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_ATT_OVERRIDE)) || stick_deviated(channel)) { override_this_channel = true; - } else // Otherwise only have RC override if the offboard channel is inactive - { + } else { // Otherwise only have RC override if the offboard channel is inactive if (muxes[channel].onboard->active) { override_this_channel = false; } else { @@ -195,8 +194,7 @@ bool CommandManager::do_throttle_muxing(void) if (RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE) && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) { override_this_channel = true; - } else // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override - { + } else { // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override if (muxes[MUX_F].onboard->active) { // Check if the parameter flag is set to have us always take the smaller throttle if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) { @@ -219,7 +217,7 @@ bool CommandManager::rc_override_active() { return rc_override_; } bool CommandManager::offboard_control_active() { for (int i = 0; i < 4; i++) { - if (muxes[i].onboard->active) return true; + if (muxes[i].onboard->active) { return true; } } return false; } diff --git a/src/controller.cpp b/src/controller.cpp index e04036fb..05e6c16f 100644 --- a/src/controller.cpp +++ b/src/controller.cpp @@ -187,28 +187,31 @@ turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, const Estimator::Sta float dt = 1e-6 * dt_us; // ROLL - if (command.x.type == RATE) + if (command.x.type == RATE) { out.x = roll_rate_.run(dt, state.angular_velocity.x, command.x.value, update_integrators); - else if (command.x.type == ANGLE) + } else if (command.x.type == ANGLE) { out.x = roll_.run(dt, state.roll, command.x.value, update_integrators, state.angular_velocity.x); - else + } else { out.x = command.x.value; + } // PITCH - if (command.y.type == RATE) + if (command.y.type == RATE) { out.y = pitch_rate_.run(dt, state.angular_velocity.y, command.y.value, update_integrators); - else if (command.y.type == ANGLE) + } else if (command.y.type == ANGLE) { out.y = pitch_.run(dt, state.pitch, command.y.value, update_integrators, state.angular_velocity.y); - else + } else { out.y = command.y.value; + } // YAW - if (command.z.type == RATE) + if (command.z.type == RATE) { out.z = yaw_rate_.run(dt, state.angular_velocity.z, command.z.value, update_integrators); - else + } else { out.z = command.z.value; + } return out; } @@ -280,8 +283,9 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator, // Integrator anti-windup //// Include reference to Dr. Beard's notes here float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u; - if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f) + if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f) { integrator_ = (u_sat - p_term + d_term) / ki_; + } // Set output return u_sat; diff --git a/src/estimator.cpp b/src/estimator.cpp index 4db72f0a..ddc3439c 100644 --- a/src/estimator.cpp +++ b/src/estimator.cpp @@ -232,7 +232,7 @@ void Estimator::run() bool Estimator::can_use_accel() const { // if we are not using accel, just bail - if (!RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)) return false; + if (!RF_.params_.get_param_int(PARAM_FILTER_USE_ACC)) { return false; } // current magnitude of LPF'd accelerometer const float a_sqrd_norm = accel_LPF_.sqrd_norm(); @@ -322,7 +322,7 @@ void Estimator::integrate_angular_rate(turbomath::Quaternion & quat, // only propagate if we've moved // TODO[PCL]: Will this ever be true? We should add a margin to this const float sqrd_norm_w = omega.sqrd_norm(); - if (sqrd_norm_w == 0.0f) return; + if (sqrd_norm_w == 0.0f) { return; } // for convenience const float &p = omega.x, &q = omega.y, &r = omega.z; diff --git a/src/mixer.cpp b/src/mixer.cpp index 1115eb7a..14f57821 100644 --- a/src/mixer.cpp +++ b/src/mixer.cpp @@ -148,7 +148,7 @@ void Mixer::mix_output() commands.z = 0.0; } - if (mixer_to_use_ == nullptr) return; + if (mixer_to_use_ == nullptr) { return; } for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) { if (mixer_to_use_->output_type[i] != NONE) { diff --git a/src/param.cpp b/src/param.cpp index c587fff1..6bd0eeb3 100644 --- a/src/param.cpp +++ b/src/param.cpp @@ -72,14 +72,17 @@ uint8_t Params::compute_checksum(void) const char * p; for (p = reinterpret_cast(¶ms.values); - p < reinterpret_cast(¶ms.values) + 4 * PARAMS_COUNT; p++) + p < reinterpret_cast(¶ms.values) + 4 * PARAMS_COUNT; p++) { chk ^= *p; + } for (p = reinterpret_cast(¶ms.names); - p < reinterpret_cast(¶ms.names) + PARAMS_COUNT * PARAMS_NAME_LENGTH; p++) + p < reinterpret_cast(¶ms.names) + PARAMS_COUNT * PARAMS_NAME_LENGTH; p++) { chk ^= *p; + } for (p = reinterpret_cast(¶ms.types); - p < reinterpret_cast(¶ms.types) + PARAMS_COUNT; p++) + p < reinterpret_cast(¶ms.types) + PARAMS_COUNT; p++) { chk ^= *p; + } return chk; } @@ -270,14 +273,15 @@ void Params::set_listeners(ParamListenerInterface * const listeners[], size_t nu bool Params::read(void) { - if (!RF_.board_.memory_read(¶ms, sizeof(params_t))) return false; + if (!RF_.board_.memory_read(¶ms, sizeof(params_t))) { return false; } - if (params.version != GIT_VERSION_HASH) return false; + if (params.version != GIT_VERSION_HASH) { return false; } - if (params.size != sizeof(params_t) || params.magic_be != 0xBE || params.magic_ef != 0xEF) + if (params.size != sizeof(params_t) || params.magic_be != 0xBE || params.magic_ef != 0xEF) { return false; + } - if (compute_checksum() != params.chk) return false; + if (compute_checksum() != params.chk) { return false; } return true; } @@ -290,7 +294,7 @@ bool Params::write(void) params.magic_ef = 0xEF; params.chk = compute_checksum(); - if (!RF_.board_.memory_write(¶ms, sizeof(params_t))) return false; + if (!RF_.board_.memory_write(¶ms, sizeof(params_t))) { return false; } return true; } @@ -314,10 +318,10 @@ uint16_t Params::lookup_param_id(const char name[PARAMS_NAME_LENGTH]) } // stop comparing if end of string is reached - if (params.names[id][i] == '\0') break; + if (params.names[id][i] == '\0') { break; } } - if (match) return id; + if (match) { return id; } } return PARAMS_COUNT; diff --git a/src/rc.cpp b/src/rc.cpp index d90364b8..a6ed15c1 100644 --- a/src/rc.cpp +++ b/src/rc.cpp @@ -151,13 +151,14 @@ void RC::init_switches() break; } - if (switches[chan].mapped) + if (switches[chan].mapped) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch mapped to RC channel %d", channel_name, switches[chan].channel); - else + } else { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "%s switch not mapped", channel_name); + } } } @@ -176,12 +177,13 @@ bool RC::check_rc_lost() } } - if (failsafe) + if (failsafe) { // set the RC Lost error flag RF_.state_manager_.set_event(StateManager::EVENT_RC_LOST); - else + } else { // Clear the RC Lost Error RF_.state_manager_.set_event(StateManager::EVENT_RC_FOUND); + } return failsafe; } @@ -193,8 +195,7 @@ void RC::look_for_arm_disarm_signal() prev_time_ms = now_ms; // check for arming switch if (!switch_mapped(SWITCH_ARM)) { - if (!RF_.state_manager_.state().armed) // we are DISARMED - { + if (!RF_.state_manager_.state().armed) { // we are DISARMED // if left stick is down and to the right if ((RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)) && (RF_.rc_.stick(STICK_Z) > (1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD)))) { @@ -205,8 +206,7 @@ void RC::look_for_arm_disarm_signal() if (time_sticks_have_been_in_arming_position_ms > 1000) { RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); } - } else // we are ARMED - { + } else { // we are ARMED // if left stick is down and to the left if (RF_.rc_.stick(STICK_F) < RF_.params_.get_param_float(PARAM_ARM_THRESHOLD) && RF_.rc_.stick(STICK_Z) < -(1.0f - RF_.params_.get_param_float(PARAM_ARM_THRESHOLD))) { @@ -219,12 +219,11 @@ void RC::look_for_arm_disarm_signal() time_sticks_have_been_in_arming_position_ms = 0; } } - } else // ARMING WITH SWITCH - { + } else { // ARMING WITH SWITCH if (RF_.rc_.switch_on(SWITCH_ARM)) { - if (!RF_.state_manager_.state().armed) + if (!RF_.state_manager_.state().armed) { RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_ARM); - ; + } } else { RF_.state_manager_.set_event(StateManager::EVENT_REQUEST_DISARM); } @@ -234,13 +233,12 @@ void RC::look_for_arm_disarm_signal() bool RC::run() { // Check for rc lost - if (check_rc_lost()) return false; + if (check_rc_lost()) { return false; } // read and normalize stick values for (uint8_t channel = 0; channel < static_cast(STICKS_COUNT); channel++) { float pwm = RF_.board_.rc_read(sticks[channel].channel); - if (sticks[channel].one_sided) // generally only F is one_sided - { + if (sticks[channel].one_sided) { // generally only F is one_sided stick_values[channel] = pwm; } else { stick_values[channel] = 2.0 * (pwm - 0.5); @@ -274,9 +272,9 @@ bool RC::new_command() if (new_command_) { new_command_ = false; return true; - } else + } else { return false; - ; + } } } // namespace rosflight_firmware diff --git a/src/rosflight.cpp b/src/rosflight.cpp index 017b1bd9..7254d11b 100644 --- a/src/rosflight.cpp +++ b/src/rosflight.cpp @@ -130,7 +130,7 @@ void ROSflight::run() state_manager_.run(); // get RC, synchronous with rc data acquisition - if (got.rc) rc_.run(); + if (got.rc) { rc_.run(); } // update commands (internal logic tells whether or not we should do anything or not) command_manager_.run(); diff --git a/src/sensors.cpp b/src/sensors.cpp index 9dea58be..fb491836 100644 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -127,8 +127,9 @@ got_flags Sensors::run() rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); last_imu_update_ms_ = rf_.board_.clock_millis(); - if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) + if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) { got.imu = false; + } // Move data into local copy data_.accel.x = accel_[0]; @@ -143,8 +144,8 @@ got_flags Sensors::run() data_.gyro = data_.fcu_orientation * data_.gyro; - if (calibrating_acc_flag_) calibrate_accel(); - if (calibrating_gyro_flag_) calibrate_gyro(); + if (calibrating_acc_flag_) { calibrate_accel(); } + if (calibrating_gyro_flag_) { calibrate_gyro(); } // Apply bias correction correct_imu(); @@ -279,7 +280,9 @@ bool Sensors::update_imu(void) if ((new_data = rf_.board_.imu_has_new_data()) > 0) { rf_.state_manager_.clear_error(StateManager::ERROR_IMU_NOT_RESPONDING); last_imu_update_ms_ = rf_.board_.clock_millis(); - if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) return false; + if (!rf_.board_.imu_read(accel_, &data_.imu_temperature, gyro_, &data_.imu_time)) { + return false; + } // Move data into local copy data_.accel.x = accel_[0]; @@ -294,8 +297,8 @@ bool Sensors::update_imu(void) data_.gyro = data_.fcu_orientation * data_.gyro; - if (calibrating_acc_flag_) calibrate_accel(); - if (calibrating_gyro_flag_) calibrate_gyro(); + if (calibrating_acc_flag_) { calibrate_accel(); } + if (calibrating_gyro_flag_) { calibrate_gyro(); } // Apply bias correction correct_imu(); @@ -468,9 +471,7 @@ void Sensors::calibrate_baro() baro_calibration_mean_ = 0.0f; baro_calibration_var_ = 0.0f; baro_calibration_count_ = 0; - } - - else if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) { + } else if (baro_calibration_count_ > SENSOR_CAL_DELAY_CYCLES) { float measurement = data_.baro_pressure - ground_pressure_; float delta = measurement - baro_calibration_mean_; baro_calibration_mean_ += delta / (baro_calibration_count_ - SENSOR_CAL_DELAY_CYCLES); @@ -549,7 +550,7 @@ void Sensors::correct_mag(void) void Sensors::correct_baro(void) { - if (!baro_calibrated_) calibrate_baro(); + if (!baro_calibrated_) { calibrate_baro(); } data_.baro_pressure -= rf_.params_.get_param_float(PARAM_BARO_BIAS); data_.baro_altitude = turbomath::alt(data_.baro_pressure) - rf_.params_.get_param_float(PARAM_GROUND_LEVEL); @@ -557,11 +558,11 @@ void Sensors::correct_baro(void) void Sensors::correct_diff_pressure() { - if (!diff_pressure_calibrated_) calibrate_diff_pressure(); + if (!diff_pressure_calibrated_) { calibrate_diff_pressure(); } data_.diff_pressure -= rf_.params_.get_param_float(PARAM_DIFF_PRESS_BIAS); float atm = 101325.0f; - if (data_.baro_present) atm = data_.baro_pressure; + if (data_.baro_present) { atm = data_.baro_pressure; } data_.diff_pressure_velocity = turbomath::fsign(data_.diff_pressure) * 24.574f / turbomath::inv_sqrt((turbomath::fabs(data_.diff_pressure) * data_.diff_pressure_temp / atm)); } diff --git a/src/state_manager.cpp b/src/state_manager.cpp index 3cfdbbaa..b7b6048b 100644 --- a/src/state_manager.cpp +++ b/src/state_manager.cpp @@ -157,27 +157,34 @@ void StateManager::set_event(StateManager::Event event) break; case EVENT_REQUEST_ARM: if (next_arming_error_msg_ms_ < RF_.board_.clock_millis()) { - if (state_.error_codes & StateManager::ERROR_INVALID_MIXER) + if (state_.error_codes & StateManager::ERROR_INVALID_MIXER) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Invalid mixer"); - if (state_.error_codes & StateManager::ERROR_IMU_NOT_RESPONDING) + } + if (state_.error_codes & StateManager::ERROR_IMU_NOT_RESPONDING) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not responding"); - if (state_.error_codes & StateManager::ERROR_RC_LOST) + } + if (state_.error_codes & StateManager::ERROR_RC_LOST) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: RC signal lost"); - if (state_.error_codes & StateManager::ERROR_UNHEALTHY_ESTIMATOR) + } + if (state_.error_codes & StateManager::ERROR_UNHEALTHY_ESTIMATOR) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Unhealthy estimator"); - if (state_.error_codes & StateManager::ERROR_TIME_GOING_BACKWARDS) + } + if (state_.error_codes & StateManager::ERROR_TIME_GOING_BACKWARDS) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Time going backwards"); - if (state_.error_codes & StateManager::ERROR_UNCALIBRATED_IMU) + } + if (state_.error_codes & StateManager::ERROR_UNCALIBRATED_IMU) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: IMU not calibrated"); - if (state_.error_codes & StateManager::ERROR_INVALID_FAILSAFE) + } + if (state_.error_codes & StateManager::ERROR_INVALID_FAILSAFE) { RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unable to arm: Invalid failsafe setting"); + } next_arming_error_msg_ms_ = RF_.board_.clock_millis() + 1000; // throttle messages to 1 Hz @@ -222,9 +229,11 @@ void StateManager::set_event(StateManager::Event event) break; case EVENT_REQUEST_DISARM: state_.armed = false; - if (state_.error) fsm_state_ = FSM_STATE_ERROR; - else + if (state_.error) { + fsm_state_ = FSM_STATE_ERROR; + } else { fsm_state_ = FSM_STATE_PREFLIGHT; + } break; case EVENT_ERROR: state_.error = true; @@ -260,8 +269,9 @@ void StateManager::set_event(StateManager::Event event) } // If there has been a change, then report it to the user - if (start_state != fsm_state_ || state_.error_codes != start_errors) + if (start_state != fsm_state_ || state_.error_codes != start_errors) { RF_.comm_manager_.update_status(); + } } void StateManager::write_backup_data(const BackupData::DebugInfo & debug) @@ -312,9 +322,11 @@ void StateManager::check_backup_memory() void StateManager::process_errors() { - if (state_.error_codes) set_event(EVENT_ERROR); - else + if (state_.error_codes) { + set_event(EVENT_ERROR); + } else { set_event(EVENT_NO_ERROR); + } } void StateManager::update_leds() @@ -325,19 +337,16 @@ void StateManager::update_leds() RF_.board_.led1_toggle(); next_led_blink_ms_ = RF_.board_.clock_millis() + 100; } - } - // blink slowly if in error - else if (state_.error) { + } else if (state_.error) { // blink slowly if in error if (next_led_blink_ms_ < RF_.board_.clock_millis()) { RF_.board_.led1_toggle(); next_led_blink_ms_ = RF_.board_.clock_millis() + 500; } - } - // off if disarmed, on if armed - else if (!state_.armed) + } else if (!state_.armed) { // off if disarmed, on if armed RF_.board_.led1_off(); - else + } else { RF_.board_.led1_on(); + } } } // namespace rosflight_firmware diff --git a/test/common.cpp b/test/common.cpp index 0acc4ab7..0e212b90 100644 --- a/test/common.cpp +++ b/test/common.cpp @@ -4,8 +4,9 @@ double quaternion_error(Eigen::Quaternionf q_eig, turbomath::Quaternion q) { Eigen::Quaternionf est_quat(q.w, q.x, q.y, q.z); Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse(); - if (q_tilde.vec().norm() < 0.000001) return 0; - else { + if (q_tilde.vec().norm() < 0.000001) { + return 0; + } else { Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); @@ -17,8 +18,9 @@ double quaternion_error(turbomath::Quaternion q0, turbomath::Quaternion q) Eigen::Quaternionf est_quat(q.w, q.x, q.y, q.z); Eigen::Quaternionf q_eig(q0.w, q0.x, q0.y, q0.z); Eigen::Quaternionf q_tilde = q_eig * est_quat.inverse(); - if (q_tilde.vec().norm() < 0.000001) return 0; - else { + if (q_tilde.vec().norm() < 0.000001) { + return 0; + } else { Eigen::Vector3f v_tilde = atan2(q_tilde.vec().norm(), q_tilde.w()) * q_tilde.vec() / q_tilde.vec().norm(); return v_tilde.norm(); diff --git a/test/test_board.cpp b/test/test_board.cpp index ba53dd90..0cbf1142 100644 --- a/test/test_board.cpp +++ b/test/test_board.cpp @@ -109,7 +109,7 @@ bool testBoard::backup_memory_read(void * dest, size_t len) void testBoard::backup_memory_write(const void * src, size_t len) { - if (len > BACKUP_MEMORY_SIZE) len = BACKUP_MEMORY_SIZE; + if (len > BACKUP_MEMORY_SIZE) { len = BACKUP_MEMORY_SIZE; } memcpy(backup_memory_, src, len); } void testBoard::backup_memory_clear(size_t len) { memset(backup_memory_, 0, len); } diff --git a/test/turbotrig_test.cpp b/test/turbotrig_test.cpp index 95a84971..3e0f2622 100644 --- a/test/turbotrig_test.cpp +++ b/test/turbotrig_test.cpp @@ -117,9 +117,11 @@ TEST(TurboMath, atan2) TEST(TurboMath, asin) { for (float i = -1.0; i <= 1.0; i += 0.001) { - if (fabs(i) < 0.95) EXPECT_NEAR(turbomath::asin(i), asin(i), 0.0001); - else + if (fabs(i) < 0.95) { + EXPECT_NEAR(turbomath::asin(i), asin(i), 0.0001); + } else { EXPECT_NEAR(turbomath::asin(i), asin(i), 0.2); + } } }