diff --git a/.astylerc b/.astylerc index e586d130..b19af1a5 100644 --- a/.astylerc +++ b/.astylerc @@ -7,4 +7,3 @@ align-pointer=name align-reference=name max-code-length=120 suffix=none ---ignore-exclude-errors-x diff --git a/boards/naze/board.c b/boards/naze/board.c index e31c177e..42d40ce6 100644 --- a/boards/naze/board.c +++ b/boards/naze/board.c @@ -1,22 +1,22 @@ /* BSD 3-Clause License - * + * * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -103,21 +103,21 @@ static float _gyro_scale; void sensors_init() { - // Initialize I2c - i2cInit(I2CDEV_2); + // Initialize I2c + i2cInit(I2CDEV_2); - while(millis() < 50); + while (millis() < 50); - i2cWrite(0,0,0); - _baro_present = ms5611_init(); - _mag_present = hmc5883lInit(_board_revision); - _sonar_present = mb1242_init(); - _diff_pressure_present = ms4525_init(); + i2cWrite(0,0,0); + _baro_present = ms5611_init(); + _mag_present = hmc5883lInit(_board_revision); + _sonar_present = mb1242_init(); + _diff_pressure_present = ms4525_init(); - // IMU - uint16_t acc1G; - mpu6050_init(true, &acc1G, &_gyro_scale, _board_revision); - _accel_scale = 9.80665f/acc1G; + // IMU + uint16_t acc1G; + mpu6050_init(true, &acc1G, &_gyro_scale, _board_revision); + _accel_scale = 9.80665f/acc1G; } void imu_register_callback(void (*callback)(void)) @@ -133,7 +133,7 @@ void imu_not_responding_error() sensors_init(); } -bool imu_read_all(float accel[3], float gyro[3], float* temperature) +bool imu_read_all(float accel[3], float gyro[3], float *temperature) { // Convert to NED int16_t accel_raw[3]; @@ -292,7 +292,7 @@ void pwm_write(uint8_t channel, uint16_t value) bool pwm_lost() { - return ((millis() - pwmLastUpdate()) > 40); + return ((millis() - pwmLastUpdate()) > 40); } // non-volatile memory @@ -302,22 +302,40 @@ void memory_init(void) initEEPROM(); } -bool memory_read(void * dest, size_t len) +bool memory_read(void *dest, size_t len) { return readEEPROM(dest, len); } -bool memory_write(const void * src, size_t len) +bool memory_write(const void *src, size_t len) { return writeEEPROM(src, len); } // LED -void led0_on(void) { LED0_ON; } -void led0_off(void) { LED0_OFF; } -void led0_toggle(void) { LED0_TOGGLE; } +void led0_on(void) +{ + LED0_ON; +} +void led0_off(void) +{ + LED0_OFF; +} +void led0_toggle(void) +{ + LED0_TOGGLE; +} -void led1_on(void) { LED1_ON; } -void led1_off(void) { LED1_OFF; } -void led1_toggle(void) { LED1_TOGGLE; } +void led1_on(void) +{ + LED1_ON; +} +void led1_off(void) +{ + LED1_OFF; +} +void led1_toggle(void) +{ + LED1_TOGGLE; +} diff --git a/fix-code-style.sh b/fix-code-style.sh index 333a6481..7bfefdd0 100755 --- a/fix-code-style.sh +++ b/fix-code-style.sh @@ -3,5 +3,6 @@ CMD="astyle --options=.astylerc" $CMD include/* -$CMD --exclude="param.c" src/* +$CMD src/* $CMD lib/turbotrig/* +$CMD boards/naze/* diff --git a/include/board.h b/include/board.h index 2a64d62f..98aba8c7 100644 --- a/include/board.h +++ b/include/board.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -55,7 +55,7 @@ void imu_register_callback(void (*callback)(void)); void imu_read_accel(float accel[3]); void imu_read_gyro(float gyro[3]); float imu_read_temperature(void); -bool imu_read_all(float accel[3], float gyro[3], float* temperature); +bool imu_read_all(float accel[3], float gyro[3], float *temperature); void imu_not_responding_error(); bool mag_check(void); @@ -87,8 +87,8 @@ void pwm_write(uint8_t channel, uint16_t value); // non-volatile memory void memory_init(void); -bool memory_read(void * dest, size_t len); -bool memory_write(const void * src, size_t len); +bool memory_read(void *dest, size_t len); +bool memory_write(const void *src, size_t len); // LEDs void led0_on(void); diff --git a/include/controller.h b/include/controller.h index a0b261d2..de1f5574 100644 --- a/include/controller.h +++ b/include/controller.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/estimator.h b/include/estimator.h index b1cb7793..edeefe16 100644 --- a/include/estimator.h +++ b/include/estimator.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mavlink.h b/include/mavlink.h index c5ec194e..bd343ead 100644 --- a/include/mavlink.h +++ b/include/mavlink.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mavlink_log.h b/include/mavlink_log.h index f9cd6c27..db01905b 100644 --- a/include/mavlink_log.h +++ b/include/mavlink_log.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mavlink_param.h b/include/mavlink_param.h index faa464e3..98985171 100644 --- a/include/mavlink_param.h +++ b/include/mavlink_param.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mavlink_receive.h b/include/mavlink_receive.h index 2b1b7fba..acaab462 100644 --- a/include/mavlink_receive.h +++ b/include/mavlink_receive.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mavlink_stream.h b/include/mavlink_stream.h index 6de5fddf..6018e154 100644 --- a/include/mavlink_stream.h +++ b/include/mavlink_stream.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mavlink_util.h b/include/mavlink_util.h index 6faafa9e..4a47f6bc 100644 --- a/include/mavlink_util.h +++ b/include/mavlink_util.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mixer.h b/include/mixer.h index da8ddc6f..a7e3b844 100644 --- a/include/mixer.h +++ b/include/mixer.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mode.h b/include/mode.h index 84eecf27..31ff1864 100644 --- a/include/mode.h +++ b/include/mode.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/mux.h b/include/mux.h index c69cde6b..4d3a1aed 100644 --- a/include/mux.h +++ b/include/mux.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -74,8 +74,8 @@ extern bool _new_command; * @brief Selects a combination of 3 possible control inputs for a single combined control output. * * Selects between failsafe, offboard, and rc input control, and based off current state, muxes them into - * the #_combined_control extern. - * + * the #_combined_control extern. + * * @return False if no new commands were triggered by the 3 control inputs, otherwise true. */ bool mux_inputs(); diff --git a/include/param.h b/include/param.h index 5ae41f30..932e9f5a 100644 --- a/include/param.h +++ b/include/param.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -43,169 +43,169 @@ extern "C" { typedef enum { - /******************************/ - /*** HARDWARE CONFIGURATION ***/ - /******************************/ - PARAM_BAUD_RATE, - - /*****************************/ - /*** MAVLINK CONFIGURATION ***/ - /*****************************/ - PARAM_SYSTEM_ID, - PARAM_STREAM_HEARTBEAT_RATE, - PARAM_STREAM_STATUS_RATE, - - PARAM_STREAM_ATTITUDE_RATE, - PARAM_STREAM_IMU_RATE, - PARAM_STREAM_MAG_RATE, - PARAM_STREAM_BARO_RATE, - PARAM_STREAM_AIRSPEED_RATE, - PARAM_STREAM_SONAR_RATE, - - PARAM_STREAM_OUTPUT_RAW_RATE, - PARAM_STREAM_RC_RAW_RATE, - - /********************************/ - /*** CONTROLLER CONFIGURATION ***/ - /********************************/ - PARAM_MAX_COMMAND, - - PARAM_PID_ROLL_RATE_P, - PARAM_PID_ROLL_RATE_I, - PARAM_PID_ROLL_RATE_D, - PARAM_ROLL_RATE_TRIM, - - PARAM_PID_PITCH_RATE_P, - PARAM_PID_PITCH_RATE_I, - PARAM_PID_PITCH_RATE_D, - PARAM_PITCH_RATE_TRIM, - - PARAM_PID_YAW_RATE_P, - PARAM_PID_YAW_RATE_I, - PARAM_PID_YAW_RATE_D, - PARAM_YAW_RATE_TRIM, - - PARAM_PID_ROLL_ANGLE_P, - PARAM_PID_ROLL_ANGLE_I, - PARAM_PID_ROLL_ANGLE_D, - PARAM_ROLL_ANGLE_TRIM, - - PARAM_PID_PITCH_ANGLE_P, - PARAM_PID_PITCH_ANGLE_I, - PARAM_PID_PITCH_ANGLE_D, - PARAM_PITCH_ANGLE_TRIM, - - PARAM_X_EQ_TORQUE, - PARAM_Y_EQ_TORQUE, - PARAM_Z_EQ_TORQUE, - - PARAM_PID_TAU, - - /*************************/ - /*** PWM CONFIGURATION ***/ - /*************************/ - PARAM_MOTOR_PWM_SEND_RATE, - PARAM_MOTOR_IDLE_THROTTLE, - PARAM_FAILSAFE_THROTTLE, - PARAM_MOTOR_MAX_PWM, - PARAM_MOTOR_MIN_PWM, - PARAM_SPIN_MOTORS_WHEN_ARMED, - - /*******************************/ - /*** ESTIMATOR CONFIGURATION ***/ - /*******************************/ - PARAM_INIT_TIME, - PARAM_FILTER_KP, - PARAM_FILTER_KI, - - PARAM_FILTER_USE_QUAD_INT, - PARAM_FILTER_USE_MAT_EXP, - PARAM_FILTER_USE_ACC, - - PARAM_CALIBRATE_GYRO_ON_ARM, - - PARAM_GYRO_ALPHA, - PARAM_ACC_ALPHA, - - PARAM_ACCEL_SCALE, - - PARAM_GYRO_X_BIAS, - PARAM_GYRO_Y_BIAS, - PARAM_GYRO_Z_BIAS, - PARAM_ACC_X_BIAS, - PARAM_ACC_Y_BIAS, - PARAM_ACC_Z_BIAS, - PARAM_ACC_X_TEMP_COMP, - PARAM_ACC_Y_TEMP_COMP, - PARAM_ACC_Z_TEMP_COMP, - - PARAM_MAG_A11_COMP, - PARAM_MAG_A12_COMP, - PARAM_MAG_A13_COMP, - PARAM_MAG_A21_COMP, - PARAM_MAG_A22_COMP, - PARAM_MAG_A23_COMP, - PARAM_MAG_A31_COMP, - PARAM_MAG_A32_COMP, - PARAM_MAG_A33_COMP, - PARAM_MAG_X_BIAS, - PARAM_MAG_Y_BIAS, - PARAM_MAG_Z_BIAS, - - /************************/ - /*** RC CONFIGURATION ***/ - /************************/ - PARAM_RC_TYPE, - PARAM_RC_X_CHANNEL, - PARAM_RC_Y_CHANNEL, - PARAM_RC_Z_CHANNEL, - PARAM_RC_F_CHANNEL, - PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, - PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, - PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, - PARAM_RC_ARM_CHANNEL, - PARAM_RC_NUM_CHANNELS, - - PARAM_RC_SWITCH_5_DIRECTION, - PARAM_RC_SWITCH_6_DIRECTION, - PARAM_RC_SWITCH_7_DIRECTION, - PARAM_RC_SWITCH_8_DIRECTION, - - PARAM_RC_OVERRIDE_DEVIATION, - PARAM_OVERRIDE_LAG_TIME, - PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, - - PARAM_RC_ATTITUDE_MODE, - PARAM_RC_MAX_ROLL, - PARAM_RC_MAX_PITCH, - PARAM_RC_MAX_ROLLRATE, - PARAM_RC_MAX_PITCHRATE, - PARAM_RC_MAX_YAWRATE, - - /***************************/ - /*** FRAME CONFIGURATION ***/ - /***************************/ - PARAM_MIXER, - - PARAM_FIXED_WING, - PARAM_ELEVATOR_REVERSE, - PARAM_AILERON_REVERSE, - PARAM_RUDDER_REVERSE, - - /********************/ - /*** ARMING SETUP ***/ - /********************/ - PARAM_ARM_THRESHOLD, - - // keep track of size of params array - PARAMS_COUNT + /******************************/ + /*** HARDWARE CONFIGURATION ***/ + /******************************/ + PARAM_BAUD_RATE, + + /*****************************/ + /*** MAVLINK CONFIGURATION ***/ + /*****************************/ + PARAM_SYSTEM_ID, + PARAM_STREAM_HEARTBEAT_RATE, + PARAM_STREAM_STATUS_RATE, + + PARAM_STREAM_ATTITUDE_RATE, + PARAM_STREAM_IMU_RATE, + PARAM_STREAM_MAG_RATE, + PARAM_STREAM_BARO_RATE, + PARAM_STREAM_AIRSPEED_RATE, + PARAM_STREAM_SONAR_RATE, + + PARAM_STREAM_OUTPUT_RAW_RATE, + PARAM_STREAM_RC_RAW_RATE, + + /********************************/ + /*** CONTROLLER CONFIGURATION ***/ + /********************************/ + PARAM_MAX_COMMAND, + + PARAM_PID_ROLL_RATE_P, + PARAM_PID_ROLL_RATE_I, + PARAM_PID_ROLL_RATE_D, + PARAM_ROLL_RATE_TRIM, + + PARAM_PID_PITCH_RATE_P, + PARAM_PID_PITCH_RATE_I, + PARAM_PID_PITCH_RATE_D, + PARAM_PITCH_RATE_TRIM, + + PARAM_PID_YAW_RATE_P, + PARAM_PID_YAW_RATE_I, + PARAM_PID_YAW_RATE_D, + PARAM_YAW_RATE_TRIM, + + PARAM_PID_ROLL_ANGLE_P, + PARAM_PID_ROLL_ANGLE_I, + PARAM_PID_ROLL_ANGLE_D, + PARAM_ROLL_ANGLE_TRIM, + + PARAM_PID_PITCH_ANGLE_P, + PARAM_PID_PITCH_ANGLE_I, + PARAM_PID_PITCH_ANGLE_D, + PARAM_PITCH_ANGLE_TRIM, + + PARAM_X_EQ_TORQUE, + PARAM_Y_EQ_TORQUE, + PARAM_Z_EQ_TORQUE, + + PARAM_PID_TAU, + + /*************************/ + /*** PWM CONFIGURATION ***/ + /*************************/ + PARAM_MOTOR_PWM_SEND_RATE, + PARAM_MOTOR_IDLE_THROTTLE, + PARAM_FAILSAFE_THROTTLE, + PARAM_MOTOR_MAX_PWM, + PARAM_MOTOR_MIN_PWM, + PARAM_SPIN_MOTORS_WHEN_ARMED, + + /*******************************/ + /*** ESTIMATOR CONFIGURATION ***/ + /*******************************/ + PARAM_INIT_TIME, + PARAM_FILTER_KP, + PARAM_FILTER_KI, + + PARAM_FILTER_USE_QUAD_INT, + PARAM_FILTER_USE_MAT_EXP, + PARAM_FILTER_USE_ACC, + + PARAM_CALIBRATE_GYRO_ON_ARM, + + PARAM_GYRO_ALPHA, + PARAM_ACC_ALPHA, + + PARAM_ACCEL_SCALE, + + PARAM_GYRO_X_BIAS, + PARAM_GYRO_Y_BIAS, + PARAM_GYRO_Z_BIAS, + PARAM_ACC_X_BIAS, + PARAM_ACC_Y_BIAS, + PARAM_ACC_Z_BIAS, + PARAM_ACC_X_TEMP_COMP, + PARAM_ACC_Y_TEMP_COMP, + PARAM_ACC_Z_TEMP_COMP, + + PARAM_MAG_A11_COMP, + PARAM_MAG_A12_COMP, + PARAM_MAG_A13_COMP, + PARAM_MAG_A21_COMP, + PARAM_MAG_A22_COMP, + PARAM_MAG_A23_COMP, + PARAM_MAG_A31_COMP, + PARAM_MAG_A32_COMP, + PARAM_MAG_A33_COMP, + PARAM_MAG_X_BIAS, + PARAM_MAG_Y_BIAS, + PARAM_MAG_Z_BIAS, + + /************************/ + /*** RC CONFIGURATION ***/ + /************************/ + PARAM_RC_TYPE, + PARAM_RC_X_CHANNEL, + PARAM_RC_Y_CHANNEL, + PARAM_RC_Z_CHANNEL, + PARAM_RC_F_CHANNEL, + PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, + PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, + PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, + PARAM_RC_ARM_CHANNEL, + PARAM_RC_NUM_CHANNELS, + + PARAM_RC_SWITCH_5_DIRECTION, + PARAM_RC_SWITCH_6_DIRECTION, + PARAM_RC_SWITCH_7_DIRECTION, + PARAM_RC_SWITCH_8_DIRECTION, + + PARAM_RC_OVERRIDE_DEVIATION, + PARAM_OVERRIDE_LAG_TIME, + PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, + + PARAM_RC_ATTITUDE_MODE, + PARAM_RC_MAX_ROLL, + PARAM_RC_MAX_PITCH, + PARAM_RC_MAX_ROLLRATE, + PARAM_RC_MAX_PITCHRATE, + PARAM_RC_MAX_YAWRATE, + + /***************************/ + /*** FRAME CONFIGURATION ***/ + /***************************/ + PARAM_MIXER, + + PARAM_FIXED_WING, + PARAM_ELEVATOR_REVERSE, + PARAM_AILERON_REVERSE, + PARAM_RUDDER_REVERSE, + + /********************/ + /*** ARMING SETUP ***/ + /********************/ + PARAM_ARM_THRESHOLD, + + // keep track of size of params array + PARAMS_COUNT } param_id_t; typedef enum uint8_t { - PARAM_TYPE_INT32, - PARAM_TYPE_FLOAT, - PARAM_TYPE_INVALID + PARAM_TYPE_INT32, + PARAM_TYPE_FLOAT, + PARAM_TYPE_INVALID } param_type_t; // function declarations diff --git a/include/rc.h b/include/rc.h index f1aafc0b..cdb87847 100644 --- a/include/rc.h +++ b/include/rc.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -93,9 +93,9 @@ bool rc_switch_mapped(rc_switch_t channel); /** * @brief Receive new RC data and update local data members. * - * Maps channeled inputs from the RC controller to their proper data member values within this + * Maps channeled inputs from the RC controller to their proper data member values within this * class every 20ms. Upon update, signals to the mux that a new command is waiting. - * + * * @return False if it hasn't been 20ms since the last update, otherwise true. */ bool receive_rc(); diff --git a/include/rosflight.h b/include/rosflight.h index fcb9f065..5c48c262 100644 --- a/include/rosflight.h +++ b/include/rosflight.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/include/sensors.h b/include/sensors.h index faf2563b..64e1b657 100644 --- a/include/sensors.h +++ b/include/sensors.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/lib/turbotrig/turbotrig.c b/lib/turbotrig/turbotrig.c index 734d5dd5..2a9f1c8f 100644 --- a/lib/turbotrig/turbotrig.c +++ b/lib/turbotrig/turbotrig.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/lib/turbotrig/turbotrig.h b/lib/turbotrig/turbotrig.h index 5a55e948..8f29964e 100644 --- a/lib/turbotrig/turbotrig.h +++ b/lib/turbotrig/turbotrig.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/lib/turbotrig/turbovec.c b/lib/turbotrig/turbovec.c index 0f8c0e52..fa0df727 100644 --- a/lib/turbotrig/turbovec.c +++ b/lib/turbotrig/turbovec.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/lib/turbotrig/turbovec.h b/lib/turbotrig/turbovec.h index 7ecdc406..a8a1a9ee 100644 --- a/lib/turbotrig/turbovec.h +++ b/lib/turbotrig/turbovec.h @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/src/controller.c b/src/controller.c index 5272cb99..daaa2995 100644 --- a/src/controller.c +++ b/src/controller.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -77,8 +77,8 @@ static pid_t pid_pitch; static pid_t pid_pitch_rate; static pid_t pid_yaw_rate; -static void init_pid(pid_t *pid, param_id_t kp_param_id, param_id_t ki_param_id, param_id_t kd_param_id, float *current_x, - float *current_xdot, float *commanded_x, float *output, float max, float min) +static void init_pid(pid_t *pid, param_id_t kp_param_id, param_id_t ki_param_id, param_id_t kd_param_id, + float *current_x, float *current_xdot, float *commanded_x, float *output, float max, float min) { pid->kp_param_id = kp_param_id; pid->ki_param_id = ki_param_id; @@ -129,8 +129,8 @@ static void run_pid(pid_t *pid, float dt) { if (dt > 0.0f) { - pid->differentiator = (2.0f*pid->tau-dt)/(2.0f*pid->tau+dt)*pid->differentiator + 2.0f/(2.0f*pid->tau+dt)*(( - *pid->current_x) - pid->prev_x); + pid->differentiator = (2.0f*pid->tau-dt)/(2.0f*pid->tau+dt)*pid->differentiator + + 2.0f/(2.0f*pid->tau+dt)*((*pid->current_x) - pid->prev_x); pid->prev_x = *pid->current_x; d_term = get_param_float(pid->kd_param_id)*pid->differentiator; } @@ -142,7 +142,7 @@ static void run_pid(pid_t *pid, float dt) } // If there is an integrator, we are armed, and throttle is high - if ( (pid->ki_param_id < PARAMS_COUNT) && (_armed_state == ARMED) && (_combined_control.F.value > 0.1)) + if ((pid->ki_param_id < PARAMS_COUNT) && (_armed_state == ARMED) && (_combined_control.F.value > 0.1)) { if (get_param_float(pid->ki_param_id) > 0.0) { diff --git a/src/estimator.c b/src/estimator.c index bc34721d..266a8f90 100644 --- a/src/estimator.c +++ b/src/estimator.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -186,7 +186,8 @@ void run_estimator() // add in accelerometer float a_sqrd_norm = _accel_LPF.x*_accel_LPF.x + _accel_LPF.y*_accel_LPF.y + _accel_LPF.z*_accel_LPF.z; - if (get_param_int(PARAM_FILTER_USE_ACC) && a_sqrd_norm < 1.15f*1.15f*9.80665f*9.80665f && a_sqrd_norm > 0.85f*0.85f*9.80665f*9.80665f) + if (get_param_int(PARAM_FILTER_USE_ACC) && a_sqrd_norm < 1.15f*1.15f*9.80665f*9.80665f + && a_sqrd_norm > 0.85f*0.85f*9.80665f*9.80665f) { // Keep track of the last time that the acc update ran last_acc_update_us = _current_state.now_us; diff --git a/src/mavlink.c b/src/mavlink.c index 1e2caefb..66bfdbe1 100644 --- a/src/mavlink.c +++ b/src/mavlink.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/src/mavlink_param.c b/src/mavlink_param.c index 014140c5..dcdaa4f2 100644 --- a/src/mavlink_param.c +++ b/src/mavlink_param.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/src/mavlink_receive.c b/src/mavlink_receive.c index a32555a6..b9e6f61c 100644 --- a/src/mavlink_receive.c +++ b/src/mavlink_receive.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/src/mavlink_stream.c b/src/mavlink_stream.c index 3f28b16c..b164931a 100644 --- a/src/mavlink_stream.c +++ b/src/mavlink_stream.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/src/mavlink_util.c b/src/mavlink_util.c index 15708b3a..65a79a7d 100644 --- a/src/mavlink_util.c +++ b/src/mavlink_util.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE diff --git a/src/mixer.c b/src/mixer.c index 9a3eb5cc..002577b7 100644 --- a/src/mixer.c +++ b/src/mixer.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -96,7 +96,7 @@ static mixer_t X8_mixing = { 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f} // Z Mix }; -static mixer_t* mixer_to_use; +static mixer_t *mixer_to_use; static mixer_t *array_of_mixers[NUM_MIXERS] = { @@ -172,7 +172,8 @@ void write_motor(uint8_t index, float value) value = 0.0; } _outputs[index] = value; - int32_t pwm_us = value * (get_param_int(PARAM_MOTOR_MAX_PWM) - get_param_int(PARAM_MOTOR_MIN_PWM)) + get_param_int(PARAM_MOTOR_MIN_PWM); + int32_t pwm_us = value * (get_param_int(PARAM_MOTOR_MAX_PWM) - get_param_int(PARAM_MOTOR_MIN_PWM)) + + get_param_int(PARAM_MOTOR_MIN_PWM); pwm_write(index, pwm_us); } diff --git a/src/mode.c b/src/mode.c index 201a54f7..0c507071 100644 --- a/src/mode.c +++ b/src/mode.c @@ -118,7 +118,7 @@ bool check_failsafe(void) // go into failsafe if we get an invalid RC command for any channel for (int8_t i = 0; i 2100) + if (pwm_read(i) < 900 || pwm_read(i) > 2100) { failsafe = true; } @@ -225,7 +225,7 @@ bool check_mode() { if (rc_switch(RC_SWITCH_ARM)) { - if ( !(_armed_state & ARMED)) + if (!(_armed_state & ARMED)) arm(); } else diff --git a/src/mux.c b/src/mux.c index 28629a7d..5e426681 100644 --- a/src/mux.c +++ b/src/mux.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -78,7 +78,8 @@ typedef struct uint32_t last_override_time; } rc_stick_override_t; -rc_stick_override_t rc_stick_override[] = { +rc_stick_override_t rc_stick_override[] = +{ { RC_STICK_X, 0 }, { RC_STICK_Y, 0 }, { RC_STICK_Z, 0 } @@ -86,9 +87,9 @@ rc_stick_override_t rc_stick_override[] = { typedef struct { - control_channel_t* rc; - control_channel_t* onboard; - control_channel_t* combined; + control_channel_t *rc; + control_channel_t *onboard; + control_channel_t *combined; } mux_t; mux_t muxes[4] = @@ -161,7 +162,8 @@ static bool stick_deviated(mux_channel_t channel) return true; } else - { //check if the RC value for this channel has moved from center enough to trigger a RC override + { + //check if the RC value for this channel has moved from center enough to trigger a RC override if (fabs(rc_stick(rc_stick_override[channel].rc_channel)) > get_param_float(PARAM_RC_OVERRIDE_DEVIATION)) { rc_stick_override[channel].last_override_time = now; @@ -208,7 +210,8 @@ static bool do_throttle_muxing(void) { if (muxes[MUX_F].onboard->active) { - if (get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) //Check if the parameter flag is set to have us always take the smaller throttle + //Check if the parameter flag is set to have us always take the smaller throttle + if (get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) { rc_override = (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value); } @@ -238,7 +241,7 @@ bool offboard_control_active() { for (int i = 0; i < 4; i++) { - if(muxes[i].onboard->active) + if (muxes[i].onboard->active) return true; } return false; diff --git a/src/param.c b/src/param.c index 7219374f..9813b750 100644 --- a/src/param.c +++ b/src/param.c @@ -80,7 +80,7 @@ static void init_param_float(param_id_t id, char name[PARAMS_NAME_LENGTH], float static uint8_t compute_checksum(void) { uint8_t chk = 0; - const uint8_t * p; + const uint8_t *p; for (p = (const uint8_t *)¶ms.values; p < ((const uint8_t *)¶ms.values + 4*PARAMS_COUNT); p++) chk ^= *p; diff --git a/src/rc.c b/src/rc.c index acd2f9e5..f26a90b9 100644 --- a/src/rc.c +++ b/src/rc.c @@ -1,22 +1,22 @@ -/* +/* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -114,8 +114,8 @@ static void init_switches() void init_rc() { - init_sticks(); - init_switches(); + init_sticks(); + init_switches(); } float rc_stick(rc_stick_t channel) @@ -135,52 +135,53 @@ bool rc_switch_mapped(rc_switch_t channel) bool receive_rc() { - static uint32_t last_rc_receive_time = 0; + static uint32_t last_rc_receive_time = 0; - uint32_t now = clock_millis(); + uint32_t now = clock_millis(); - // if it has been more than 20ms then look for new RC values and parse them - if (now - last_rc_receive_time < 20) + // if it has been more than 20ms then look for new RC values and parse them + if (now - last_rc_receive_time < 20) + { + return false; + } + last_rc_receive_time = now; + + // read and normalize stick values + for (rc_stick_t channel = 0; channel < RC_STICKS_COUNT; channel++) + { + uint16_t pwm = pwm_read(sticks[channel].channel); + if (sticks[channel].one_sided) //generally only F is one_sided { - return false; + stick_values[channel] = (float)(pwm - 1000) / (1000.0); } - last_rc_receive_time = now; - - // read and normalize stick values - for (rc_stick_t channel = 0; channel < RC_STICKS_COUNT; channel++) + else { - uint16_t pwm = pwm_read(sticks[channel].channel); - if (sticks[channel].one_sided) //generally only F is one_sided - { - stick_values[channel] = (float)(pwm - 1000) / (1000.0); - } - else - { - stick_values[channel] = (float)(2*(pwm - 1500) / (1000.0)); - } + stick_values[channel] = (float)(2*(pwm - 1500) / (1000.0)); } + } - // read and interpret switch values - for (rc_switch_t channel = 0; channel < RC_SWITCHES_COUNT; channel++) + // read and interpret switch values + for (rc_switch_t channel = 0; channel < RC_SWITCHES_COUNT; channel++) + { + if (switches[channel].mapped) { - if (switches[channel].mapped) + //switch is on/off dependent on its default direction as set in the params/init_switches + if (switches[channel].direction < 0) { - if (switches[channel].direction < 0) //switch is on/off dependent on its default direction as set in the params/init_switches - { - switch_values[channel] = pwm_read(switches[channel].channel) < 1500; - } - else - { - switch_values[channel] = pwm_read(switches[channel].channel) >= 1500; - } + switch_values[channel] = pwm_read(switches[channel].channel) < 1500; } else { - switch_values[channel] = false; + switch_values[channel] = pwm_read(switches[channel].channel) >= 1500; } } + else + { + switch_values[channel] = false; + } + } - // Signal to the mux that we need to compute a new combined command - _new_command = true; - return true; + // Signal to the mux that we need to compute a new combined command + _new_command = true; + return true; } diff --git a/src/sensors.c b/src/sensors.c index 2d4fe2c6..19f57c3f 100644 --- a/src/sensors.c +++ b/src/sensors.c @@ -1,22 +1,22 @@ /* * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab - * + * * All rights reserved. - * + * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions are met: - * + * * * Redistributions of source code must retain the above copyright notice, this * list of conditions and the following disclaimer. - * + * * * Redistributions in binary form must reproduce the above copyright notice, * this list of conditions and the following disclaimer in the documentation * and/or other materials provided with the distribution. - * + * * * Neither the name of the copyright holder nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. - * + * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE @@ -127,14 +127,14 @@ bool update_sensors() if (now > (last_time_look_for_disarmed_sensors + 500)) { last_time_look_for_disarmed_sensors = now; - if(!sonar_present()) + if (!sonar_present()) { // if(sonar_check()) // { // mavlink_log_info("FOUND SONAR", NULL); // } } - if(!diff_pressure_present()) + if (!diff_pressure_present()) { // if(diff_pressure_check()) // { @@ -152,14 +152,14 @@ bool update_sensors() } // Update whatever sensos are available - if(baro_present()) + if (baro_present()) { baro_read(&_baro_altitude, &_baro_pressure, &_baro_temperature); } - if(diff_pressure_present()) + if (diff_pressure_present()) { - if(baro_present()) + if (baro_present()) { diff_pressure_set_atm(_baro_pressure); } @@ -256,7 +256,7 @@ static bool update_imu(void) else { // if we have lost 1000 IMU messages then something is wrong - if(clock_millis() > last_imu_update_ms + 1000) + if (clock_millis() > last_imu_update_ms + 1000) { // Tell the board to fix it last_imu_update_ms = clock_millis(); @@ -309,7 +309,8 @@ static vector_t vector_max(vector_t a, vector_t b) { vector_t out = {a.x > b.x ? a.x : b.x, a.y > b.y ? a.y : b.y, - a.z > b.z ? a.z : b.z}; + a.z > b.z ? a.z : b.z + }; return out; } @@ -317,7 +318,8 @@ static vector_t vector_min(vector_t a, vector_t b) { vector_t out = {a.x < b.x ? a.x : b.x, a.y < b.y ? a.y : b.y, - a.z < b.z ? a.z : b.z}; + a.z < b.z ? a.z : b.z + }; return out; } @@ -354,7 +356,8 @@ static void calibrate_accel(void) // Which is why this line is so confusing. What we are doing, is first removing // the contribution of temperature to the measurements during the calibration, // Then we are dividing by the number of measurements. - vector_t accel_bias = scalar_multiply(1.0/(float)count, vector_sub(acc_sum, scalar_multiply(acc_temp_sum, accel_temp_bias))); + vector_t accel_bias = scalar_multiply(1.0/(float)count, + vector_sub(acc_sum, scalar_multiply(acc_temp_sum, accel_temp_bias))); // Sanity Check - // If the accelerometer is upside down or being spun around during the calibration, @@ -440,11 +443,11 @@ static void correct_mag(void) // correct according to known soft iron bias - converts to nT _mag.x = get_param_float(PARAM_MAG_A11_COMP)*mag_hard_x + get_param_float(PARAM_MAG_A12_COMP)*mag_hard_y + - get_param_float(PARAM_MAG_A13_COMP)*mag_hard_z; + get_param_float(PARAM_MAG_A13_COMP)*mag_hard_z; _mag.y = get_param_float(PARAM_MAG_A21_COMP)*mag_hard_x + get_param_float(PARAM_MAG_A22_COMP)*mag_hard_y + - get_param_float(PARAM_MAG_A23_COMP)*mag_hard_z; + get_param_float(PARAM_MAG_A23_COMP)*mag_hard_z; _mag.z = get_param_float(PARAM_MAG_A31_COMP)*mag_hard_x + get_param_float(PARAM_MAG_A32_COMP)*mag_hard_y + - get_param_float(PARAM_MAG_A33_COMP)*mag_hard_z; + get_param_float(PARAM_MAG_A33_COMP)*mag_hard_z; }