Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Clang format changes #429

Merged
merged 1 commit into from
Jul 12, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion .clang-format
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ AllowAllParametersOfDeclarationOnNextLine: false
AllowShortBlocksOnASingleLine: Always
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortIfStatementsOnASingleLine: Always
AllowShortIfStatementsOnASingleLine: AllIfsAndElse
AllowShortLambdasOnASingleLine: All
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterReturnType: None
Expand Down
26 changes: 15 additions & 11 deletions comms/mavlink/mavlink.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}
}
}

Expand Down Expand Up @@ -334,16 +335,17 @@ 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)
{
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)
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -451,15 +455,15 @@ 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)
{
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)
Expand Down Expand Up @@ -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)
Expand All @@ -507,15 +511,15 @@ 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)
{
// 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()
Expand Down
6 changes: 4 additions & 2 deletions include/sensors.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down
2 changes: 1 addition & 1 deletion include/util.h
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand Down
52 changes: 30 additions & 22 deletions lib/turbomath/turbomath.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<float>(sin_num_entries);
int16_t index = static_cast<int16_t>(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)
Expand All @@ -370,13 +372,15 @@ float atan(float x)
int16_t index = static_cast<int16_t>(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)
Expand All @@ -400,9 +404,7 @@ float atan2(float y, float x)
} else {
return arctan + M_PI;
}
}

else {
} else {
return arctan;
}
}
Expand All @@ -415,13 +417,15 @@ float asin(float x)
int16_t index = static_cast<int16_t>(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)
Expand All @@ -432,25 +436,29 @@ float alt(float press)
int16_t index = static_cast<int16_t>(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)
Expand Down
45 changes: 23 additions & 22 deletions src/comm_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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; }
Expand All @@ -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<uint16_t>(param_index);

if (id < PARAMS_COUNT) send_param_value(id);
if (id < PARAMS_COUNT) { send_param_value(id); }
}
}

Expand Down Expand Up @@ -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<int64_t>(now_us) * 1000, ts1);
}
}

void CommManager::offboard_control_callback(const CommLinkInterface::OffboardControl & control)
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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;
}
Expand Down
Loading
Loading