From 8591bcb7ca448ad80beee6379b87258a5576c5b2 Mon Sep 17 00:00:00 2001 From: Phil Tokumaru Date: Fri, 15 Nov 2024 10:27:21 -0800 Subject: [PATCH] added even more dependencies from rosflight_firmware :( --- test/comm_manager.h | 169 ++++++++++++++++++ test/command_manager.h | 170 ++++++++++++++++++ test/controller.h | 111 ++++++++++++ test/estimator.h | 127 ++++++++++++++ test/mixer.h | 275 +++++++++++++++++++++++++++++ test/param.h | 381 +++++++++++++++++++++++++++++++++++++++++ test/rosflight.h | 93 ++++++++++ test/state_manager.h | 193 +++++++++++++++++++++ test/util.h | 83 +++++++++ 9 files changed, 1602 insertions(+) create mode 100644 test/comm_manager.h create mode 100644 test/command_manager.h create mode 100644 test/controller.h create mode 100644 test/estimator.h create mode 100644 test/mixer.h create mode 100644 test/param.h create mode 100644 test/rosflight.h create mode 100644 test/state_manager.h create mode 100644 test/util.h diff --git a/test/comm_manager.h b/test/comm_manager.h new file mode 100644 index 0000000..3160c33 --- /dev/null +++ b/test/comm_manager.h @@ -0,0 +1,169 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_COMM_MANAGER_H +#define ROSFLIGHT_FIRMWARE_COMM_MANAGER_H + +#include "interface/comm_link.h" +#include "interface/param_listener.h" + +#include +#include + +#include +#include + +namespace rosflight_firmware +{ +class ROSflight; + +class CommManager : public ParamListenerInterface +{ +private: + enum StreamId + { + STREAM_ID_HEARTBEAT, + STREAM_ID_STATUS, + + STREAM_ID_ATTITUDE, + + STREAM_ID_IMU, + STREAM_ID_DIFF_PRESSURE, + STREAM_ID_BARO, + STREAM_ID_SONAR, + STREAM_ID_MAG, + STREAM_ID_BATTERY_STATUS, + + STREAM_ID_SERVO_OUTPUT_RAW, + STREAM_ID_GNSS, + STREAM_ID_GNSS_FULL, + STREAM_ID_RC_RAW, + STREAM_ID_LOW_PRIORITY, + STREAM_COUNT + }; + + enum OffboardControlMode + { + MODE_PASS_THROUGH, + MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE, + MODE_ROLL_PITCH_YAWRATE_THROTTLE, + MODE_ROLL_PITCH_YAWRATE_ALTITUDE, + }; + + uint8_t sysid_; + uint64_t offboard_control_time_; + ROSflight & RF_; + CommLinkInterface & comm_link_; + uint8_t send_params_index_; + bool initialized_ = false; + bool connected_ = false; + + static constexpr int LOG_MSG_SIZE = 50; + class LogMessageBuffer + { + public: + static constexpr int LOG_BUF_SIZE = 25; + LogMessageBuffer(); + + struct LogMessage + { + char msg[LOG_MSG_SIZE]; + CommLinkInterface::LogSeverity severity; + }; + void add_message(CommLinkInterface::LogSeverity severity, char msg[LOG_MSG_SIZE]); + size_t size() const { return length_; } + bool empty() const { return length_ == 0; } + bool full() const { return length_ == LOG_BUF_SIZE; } + const LogMessage & oldest() const { return buffer_[oldest_]; } + void pop(); + + private: + LogMessage buffer_[LOG_BUF_SIZE]; + size_t oldest_ = 0; + size_t newest_ = 0; + size_t length_ = 0; + }; + LogMessageBuffer log_buffer_; + + StateManager::BackupData backup_data_buffer_; + bool have_backup_data_ = false; + + void update_system_id(uint16_t param_id); + + void send_heartbeat(void); + void send_status(void); + void send_attitude(void); + void send_imu(void); + void send_output_raw(void); + + void send_diff_pressure(void); + void send_baro(void); + void send_sonar(void); + void send_mag(void); + void send_battery_status(void); + void send_gnss(void); + void send_gnss_full(void); + void send_rosflignt_cmd_ack(void); + void send_named_value_int(const char * const name, int32_t value); + void send_named_value_float(const char * const name, float value); + + void send_next_param(void); + + void receive_msg_offboard_control(CommLinkInterface::CommMessage * message); + void receive_msg_param_request_list(CommLinkInterface::CommMessage * message); + void receive_msg_param_request_read(CommLinkInterface::CommMessage * message); + void receive_msg_param_set(CommLinkInterface::CommMessage * message); + void receive_msg_rosflight_cmd(CommLinkInterface::CommMessage * message); + void receive_msg_rosflight_aux_cmd(CommLinkInterface::CommMessage * message); + void receive_msg_timesync(CommLinkInterface::CommMessage * message); + void receive_msg_external_attitude(CommLinkInterface::CommMessage * message); + void receive_msg_heartbeat(CommLinkInterface::CommMessage * message); + +public: + CommManager(ROSflight & rf, CommLinkInterface & comm_link); + + void init(); + void param_change_callback(uint16_t param_id) override; + void receive(void); + void transmit(got_flags got); + void send_param_value(uint16_t param_id); + void update_status(); + void log(CommLinkInterface::LogSeverity severity, const char * fmt, ...); + void log_message(CommLinkInterface::LogSeverity severity, char * text); + + + void send_rc_raw(void); + void send_backup_data(const StateManager::BackupData & backup_data); +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_COMM_MANAGER_H diff --git a/test/command_manager.h b/test/command_manager.h new file mode 100644 index 0000000..89a50c0 --- /dev/null +++ b/test/command_manager.h @@ -0,0 +1,170 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H +#define ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H + +#include "interface/param_listener.h" + +#include "rc.h" + +#include +#include + +namespace rosflight_firmware +{ +class ROSflight; + +typedef enum +{ + RATE, // Channel is is in rate mode (mrad/s) + ANGLE, // Channel command is in angle mode (mrad) + THROTTLE, // Channel is direcly controlling throttle max/1000 + PASSTHROUGH, // Channel directly passes PWM input to the mixer +} control_type_t; + +typedef struct +{ + bool active; // Whether or not the channel is active + control_type_t type; // What type the channel is + float value; // The value of the channel +} control_channel_t; + +typedef struct +{ + uint32_t stamp_ms; + control_channel_t x; + control_channel_t y; + control_channel_t z; + control_channel_t F; +} control_t; + +class CommandManager : public ParamListenerInterface +{ +private: + typedef struct + { + control_channel_t * rc; + control_channel_t * onboard; + control_channel_t * combined; + } mux_t; + + mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x}, + {&rc_command_.y, &offboard_command_.y, &combined_command_.y}, + {&rc_command_.z, &offboard_command_.z, &combined_command_.z}, + {&rc_command_.F, &offboard_command_.F, &combined_command_.F}}; + + // clang-format off + control_t rc_command_ = {0, + {false, ANGLE, 0.0}, + {false, ANGLE, 0.0}, + {false, RATE, 0.0}, + {false, THROTTLE, 0.0}}; + control_t offboard_command_ = {0, + {false, ANGLE, 0.0}, + {false, ANGLE, 0.0}, + {false, RATE, 0.0}, + {false, THROTTLE, 0.0}}; + control_t combined_command_ = {0, + {false, ANGLE, 0.0}, + {false, ANGLE, 0.0}, + {false, RATE, 0.0}, + {false, THROTTLE, 0.0}}; + + control_t multirotor_failsafe_command_ = {0, + {true, ANGLE, 0.0}, + {true, ANGLE, 0.0}, + {true, RATE, 0.0}, + {true, THROTTLE, 0.0}}; + control_t fixedwing_failsafe_command_ = {0, + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}, + {true, PASSTHROUGH, 0.0}, + {true, THROTTLE, 0.0}}; + // clang-format on + + typedef enum + { + ATT_MODE_RATE, + ATT_MODE_ANGLE + } att_mode_t; + + enum MuxChannel + { + MUX_X, + MUX_Y, + MUX_Z, + MUX_F, + }; + + typedef struct + { + RC::Stick rc_channel; + uint32_t last_override_time; + } rc_stick_override_t; + + rc_stick_override_t rc_stick_override_[3] = {{RC::STICK_X, 0}, + {RC::STICK_Y, 0}, + {RC::STICK_Z, 0}}; + + ROSflight & RF_; + + bool new_command_; + bool rc_override_; + + control_t & failsafe_command_; + + void init_failsafe(); + + bool do_roll_pitch_yaw_muxing(MuxChannel channel); + bool do_throttle_muxing(void); + void do_min_throttle_muxing(); + + void interpret_rc(void); + bool stick_deviated(MuxChannel channel); + +public: + void param_change_callback(uint16_t param_id) override; + CommandManager(ROSflight & _rf); + void init(); + bool run(); + bool rc_override_active(); + bool offboard_control_active(); + void set_new_offboard_command(control_t new_offboard_command); + void set_new_rc_command(control_t new_rc_command); + void override_combined_command_with_rc(); + inline const control_t & combined_control() const { return combined_command_; } + inline const control_t & rc_control() const { return rc_command_; } +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_COMMAND_MANAGER_H diff --git a/test/controller.h b/test/controller.h new file mode 100644 index 0000000..b8a9e9d --- /dev/null +++ b/test/controller.h @@ -0,0 +1,111 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_CONTROLLER_H +#define ROSFLIGHT_FIRMWARE_CONTROLLER_H + +#include "interface/param_listener.h" + +#include "command_manager.h" +#include "estimator.h" + +#include + +#include +#include + +namespace rosflight_firmware +{ +class ROSflight; + +class Controller : public ParamListenerInterface +{ +public: + struct Output + { + float F; + float x; + float y; + float z; + }; + + Controller(ROSflight & rf); + + inline const Output & output() const { return output_; } + + void init(); + void run(); + + void calculate_equilbrium_torque_from_rc(); + void param_change_callback(uint16_t param_id) override; + +private: + class PID + { + public: + PID(); + void init(float kp, float ki, float kd, float max, float min, float tau); + float run(float dt, float x, float x_c, bool update_integrator); + float run(float dt, float x, float x_c, bool update_integrator, float xdot); + + private: + float kp_; + float ki_; + float kd_; + + float max_; + float min_; + + float integrator_; + float differentiator_; + float prev_x_; + float tau_; + }; + + ROSflight & RF_; + + turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State & state, + const control_t & command, bool update_integrators); + + Output output_; + + PID roll_; + PID roll_rate_; + PID pitch_; + PID pitch_rate_; + PID yaw_rate_; + + uint64_t prev_time_us_; +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_CONTROLLER_H diff --git a/test/estimator.h b/test/estimator.h new file mode 100644 index 0000000..2e603ec --- /dev/null +++ b/test/estimator.h @@ -0,0 +1,127 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_ESTIMATOR_H +#define ROSFLIGHT_FIRMWARE_ESTIMATOR_H + +#include "interface/param_listener.h" + +#include + +#include +#include +#include + +namespace rosflight_firmware +{ +class ROSflight; + +typedef struct //__attribute__((__packed__)) +{ + uint64_t timestamp; // us, time of data read complete + float q[4]; // quaternions + float rate[3]; +} AttitudeStruct; + +class Estimator : public ParamListenerInterface +{ + +public: + + struct State + { + turbomath::Vector angular_velocity; + turbomath::Quaternion attitude; + float roll; + float pitch; + float yaw; + uint64_t timestamp_us; + }; + + Estimator(ROSflight & _rf); + + inline const State & state() const { return state_; } + + inline const turbomath::Vector & bias() { return bias_; } + + inline const turbomath::Vector & accLPF() { return accel_LPF_; } + + inline const turbomath::Vector & gyroLPF() { return gyro_LPF_; } + + void init(); + void param_change_callback(uint16_t param_id) override; + void run(); + void reset_state(); + void reset_adaptive_bias(); + void set_external_attitude_update(const turbomath::Quaternion & q); + + AttitudeStruct * get_attitude(void) { return &attitude_; } + +private: + AttitudeStruct attitude_; + const turbomath::Vector g_ = {0.0f, 0.0f, -1.0f}; + + ROSflight & RF_; + State state_; + + uint64_t last_time_; + uint64_t last_acc_update_us_; + uint64_t last_extatt_update_us_; + + turbomath::Vector w1_; + turbomath::Vector w2_; + + turbomath::Vector bias_; + + turbomath::Vector accel_LPF_; + turbomath::Vector gyro_LPF_; + + turbomath::Vector w_acc_; + + bool extatt_update_next_run_; + turbomath::Quaternion q_extatt_; + + void run_LPF(); + + bool can_use_accel() const; + bool can_use_extatt() const; + turbomath::Vector accel_correction() const; + turbomath::Vector extatt_correction() const; + turbomath::Vector smoothed_gyro_measurement(); + void integrate_angular_rate(turbomath::Quaternion & quat, const turbomath::Vector & omega, + const float dt) const; + void quaternion_to_dcm(const turbomath::Quaternion & q, turbomath::Vector & X, + turbomath::Vector & Y, turbomath::Vector & Z) const; +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_ESTIMATOR_H diff --git a/test/mixer.h b/test/mixer.h new file mode 100644 index 0000000..706a142 --- /dev/null +++ b/test/mixer.h @@ -0,0 +1,275 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_MIXER_H +#define ROSFLIGHT_FIRMWARE_MIXER_H + +#include "interface/param_listener.h" + +#include +#include + +#include +#include + +namespace rosflight_firmware +{ +class ROSflight; + +typedef RcStruct MixerStruct; + +class Mixer : public ParamListenerInterface +{ + +public: + static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14; + static constexpr uint8_t NUM_MIXER_OUTPUTS = 10; + + enum + { + ESC_CALIBRATION = 0, + QUADCOPTER_PLUS = 1, + QUADCOPTER_X = 2, + HEX_PLUS = 3, + HEX_X = 4, + OCTO_PLUS = 5, + OCTO_X = 6, + Y6 = 7, + X8 = 8, + TRICOPTER = 9, + FIXEDWING = 10, + PASSTHROUGH = 11, + VTAIL = 12, + QUADPLANE = 13, + CUSTOM = 14, + NUM_MIXERS, + INVALID_MIXER = 255 + }; + + typedef enum + { + NONE, // None + S, // Servo + M, // Motor + G // GPIO + } output_type_t; + + typedef struct + { + output_type_t output_type[NUM_MIXER_OUTPUTS]; + float default_pwm_rate[NUM_MIXER_OUTPUTS]; + float F[NUM_MIXER_OUTPUTS]; + float x[NUM_MIXER_OUTPUTS]; + float y[NUM_MIXER_OUTPUTS]; + float z[NUM_MIXER_OUTPUTS]; + } mixer_t; + + typedef struct + { + output_type_t type; + float value; + } aux_channel_t; + + typedef struct + { + aux_channel_t channel[NUM_TOTAL_OUTPUTS]; + } aux_command_t; + +private: + ROSflight & RF_; + + MixerStruct output_raw_ = {}; + + //float raw_outputs_[NUM_TOTAL_OUTPUTS]; + float *raw_outputs_ = output_raw_.chan; + float outputs_[NUM_TOTAL_OUTPUTS]; + aux_command_t aux_command_; + output_type_t combined_output_type_[NUM_TOTAL_OUTPUTS]; + + + // clang-format off + + const mixer_t esc_calibration_mixing = { + { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output type + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // X Mix + + const mixer_t quadcopter_plus_mixing = { + {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE}, // output_type + { 490, 490, 490, 490, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + { 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + { 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + { 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + { 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t quadcopter_x_mixing = { + { M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE}, // output_type + { 490, 490, 490, 490, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, -1.0f, -1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t hex_plus_mixing = { + { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type + { 490, 490, 490, 490, 490, 490, 50, 50, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.5f, -0.5f, -1.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t hex_x_mixing = { + { M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type + { 490, 490, 490, 490, 490, 490, 50, 50, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-0.5f, -1.0f, -0.5f, 0.5f, 1.0f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.866025f, 0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix + + const mixer_t octocopter_plus_mixing = { + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + { 490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 1.0f, 0.707f, 0.0f, 0.0f}, // X Mix + {1.0f, 0.707f, 0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t octocopter_x_mixing = { + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + { 490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0f, 1.0f, 0.414, 0.0f, 0.0f}, // X Mix + {1.0f, 0.414f, -0.414f, -1.0f, -1.0f, -0.414f, 0.414f, 1.0, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t Y6_mixing = { + {M, M, M, M, M, M, NONE, NONE, NONE, NONE}, // output_type + { 490, 490, 490, 490, 490, 490, 50, 50, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t X8_mixing = { + {M, M, M, M, M, M, M, M, NONE, NONE}, // output_type + { 490, 490, 490, 490, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {-1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // X Mix + {1.0f, 1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // Y Mix + {1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f} };// Z Mix + + const mixer_t tricopter_mixing = { + { M, M, M, NONE, S, NONE, NONE, NONE, NONE, NONE}, // output_type + { 490, 490, 490, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + { 1.000f, 0.000f, 1.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {-1.000f, 0.000f, 0.000f, 0.000f, 1.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + { 0.667f, 0.000f, 0.667f, 0.000f, -1.333f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + { 0.000f, 1.000f, 0.000f, 0.000f, 0.000f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f} }; // Z Mix + + const mixer_t fixedwing_mixing = { + { S, S, S, NONE, M, NONE, NONE, NONE, NONE, NONE}, // output type + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t fixedwing_vtail_mixing = { + { S, NONE, NONE, S, S, M, NONE, NONE, NONE, NONE}, // Ailerons, LRuddervator, RRuddervator, Motor + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz) + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 0.0f, 0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t quadplane_mixing = { + { S, S, S, M, M, M, M, M, NONE, NONE}, // Ailerons, Rudder, Elevator, Tractor Motor, Quadrotors + { 50, 50, 50, 50, 490, 490, 490, 490, 50, 50}, // Rate (Hz) + {0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f}, // F Mix + {1.0f, 0.0f, 0.0f, 0.0f, 0.0f,-1.0f, 0.0f, 1.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,-1.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 1.0f, 0.0f, 1.0f,-1.0f, 1.0f,-1.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t passthrough_mixing = { + {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t custom_mixing = { + {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE}, // output type + { 50, 50, 50, 50, 50, 50, 50, 50, 50, 50}, // Rate (Hz or kHz) + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix + {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix + + const mixer_t * mixer_to_use_; + + const mixer_t* array_of_mixers_[NUM_MIXERS] = { + &esc_calibration_mixing, + &quadcopter_plus_mixing, + &quadcopter_x_mixing, + &hex_plus_mixing, + &hex_x_mixing, + &octocopter_plus_mixing, + &octocopter_x_mixing, + &Y6_mixing, + &X8_mixing, + &tricopter_mixing, + &fixedwing_mixing, + &passthrough_mixing, + &fixedwing_vtail_mixing, + &quadplane_mixing, + &custom_mixing + }; + // clang-format on + +public: + Mixer(ROSflight & _rf); + void init(); + void init_PWM(); + void init_mixing(); + void mix_output(); + void param_change_callback(uint16_t param_id) override; + void set_new_aux_command(aux_command_t new_aux_command); + float * raw_outputs() { return raw_outputs_; } + MixerStruct * get_output_raw(void) { return &output_raw_; } +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_MIXER_H diff --git a/test/param.h b/test/param.h new file mode 100644 index 0000000..27bee5d --- /dev/null +++ b/test/param.h @@ -0,0 +1,381 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_PARAM_H +#define ROSFLIGHT_FIRMWARE_PARAM_H + +#ifndef GIT_VERSION_HASH +#define GIT_VERSION_HASH 0x00 +#pragma message "GIT_VERSION_HASH Undefined, setting to 0x00!" +#endif +#ifndef GIT_VERSION_STRING +#define GIT_VERSION_STRING "empty" +#pragma message "GIT_VERSION_STRING Undefined, setting to \"empty\"!" +#endif + +// Uncomment to view contents of GIT_VERSION_HASH and GIT_VERSION STRING +// #define STRINGIFY(s) XSTRINGIFY(s) +// #define XSTRINGIFY(s) #s +// #pragma message( "GIT_VERSION_HASH: " STRINGIFY(GIT_VERSION_HASH)) +// #pragma message( "GIT_VERSION_STRING: " GIT_VERSION_STRING) + +#include "interface/param_listener.h" + +#include +#include + +namespace rosflight_firmware +{ +enum : uint16_t +{ + /******************************/ + /*** HARDWARE CONFIGURATION ***/ + /******************************/ + PARAM_BAUD_RATE = 0, + PARAM_SERIAL_DEVICE, + + /*****************************/ + /*** MAVLINK CONFIGURATION ***/ + /*****************************/ + PARAM_SYSTEM_ID, + + /********************************/ + /*** CONTROLLER CONFIGURATION ***/ + /********************************/ + PARAM_MAX_COMMAND, + + PARAM_PID_ROLL_RATE_P, + PARAM_PID_ROLL_RATE_I, + PARAM_PID_ROLL_RATE_D, + + PARAM_PID_PITCH_RATE_P, + PARAM_PID_PITCH_RATE_I, + PARAM_PID_PITCH_RATE_D, + + PARAM_PID_YAW_RATE_P, + PARAM_PID_YAW_RATE_I, + PARAM_PID_YAW_RATE_D, + + PARAM_PID_ROLL_ANGLE_P, + PARAM_PID_ROLL_ANGLE_I, + PARAM_PID_ROLL_ANGLE_D, + + PARAM_PID_PITCH_ANGLE_P, + PARAM_PID_PITCH_ANGLE_I, + PARAM_PID_PITCH_ANGLE_D, + + 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_SPIN_MOTORS_WHEN_ARMED, + + /*******************************/ + /*** ESTIMATOR CONFIGURATION ***/ + /*******************************/ + PARAM_INIT_TIME, + PARAM_FILTER_KP_ACC, + PARAM_FILTER_KI, + PARAM_FILTER_KP_EXT, + PARAM_FILTER_ACCEL_MARGIN, + + PARAM_FILTER_USE_QUAD_INT, + PARAM_FILTER_USE_MAT_EXP, + PARAM_FILTER_USE_ACC, + + PARAM_CALIBRATE_GYRO_ON_ARM, + + PARAM_GYRO_XY_ALPHA, + PARAM_GYRO_Z_ALPHA, + PARAM_ACC_ALPHA, + + 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, + + PARAM_BARO_BIAS, + PARAM_GROUND_LEVEL, + + PARAM_DIFF_PRESS_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, + + PARAM_FC_ROLL, + PARAM_FC_PITCH, + PARAM_FC_YAW, + + /********************/ + /*** ARMING SETUP ***/ + /********************/ + PARAM_ARM_THRESHOLD, + + /************************/ + /*** OFFBOARD CONTROL ***/ + /************************/ + PARAM_OFFBOARD_TIMEOUT, + + /***********************/ + /*** BATTERY MONITOR ***/ + /***********************/ + PARAM_BATTERY_VOLTAGE_MULTIPLIER, + PARAM_BATTERY_CURRENT_MULTIPLIER, + PARAM_BATTERY_VOLTAGE_ALPHA, + PARAM_BATTERY_CURRENT_ALPHA, + + // keep track of size of params array + PARAMS_COUNT +}; + +typedef enum +{ + PARAM_TYPE_INT32, + PARAM_TYPE_FLOAT, + PARAM_TYPE_INVALID +} param_type_t; + +typedef union +{ + float fvalue; + int32_t ivalue; +} param_value_t; + +class ROSflight; +class Params +{ +public: + static constexpr uint8_t PARAMS_NAME_LENGTH = 16; + +private: + typedef struct + { + uint32_t version; + uint16_t size; + uint8_t magic_be; // magic number, should be 0xBE + + param_value_t values[PARAMS_COUNT]; + char names[PARAMS_COUNT][PARAMS_NAME_LENGTH]; + param_type_t types[PARAMS_COUNT]; + + uint8_t magic_ef; // magic number, should be 0xEF + uint8_t chk; // XOR checksum + } params_t; + + params_t params; + ROSflight & RF_; + + void init_param_int(uint16_t id, const char name[PARAMS_NAME_LENGTH], int32_t value); + void init_param_float(uint16_t id, const char name[PARAMS_NAME_LENGTH], float value); + uint8_t compute_checksum(void); + + ParamListenerInterface * const * listeners_; + size_t num_listeners_; + +public: + Params(ROSflight & _rf); + + // function declarations + + /** + * @brief Initialize parameter values + */ + void init(); + + /** + * @brief Set all parameters to default values + */ + void set_defaults(void); + + /** + * @brief Specify listeners for parameter changes + * @param listeners An array of pointers to objects that implement the ParamListenerInterface + * interface + * @param num_listeners The length of the array passed as the listeners parameter + */ + void set_listeners(ParamListenerInterface * const listeners[], size_t num_listeners); + + /** + * @brief Read parameter values from non-volatile memory + * @return True if successful, false otherwise + */ + bool read(void); + + /** + * @brief Write current parameter values to non-volatile memory + * @return True if successful, false otherwise + */ + bool write(void); + + /** + * @brief Callback for executing actions that need to be taken when a parameter value changes + * @param id The ID of the parameter that was changed + */ + void change_callback(uint16_t id); + + /** + * @brief Gets the id of a parameter from its name + * @param name The name of the parameter + * @return The ID of the parameter if the name is valid, PARAMS_COUNT otherwise (invalid ID) + */ + uint16_t lookup_param_id(const char name[PARAMS_NAME_LENGTH]); + + /** + * @brief Get the value of an integer parameter by id + * @param id The ID of the parameter + * @return The value of the parameter + */ + inline int get_param_int(uint16_t id) const { return params.values[id].ivalue; } + + /** + * @brief Get the value of a floating point parameter by id + * @param id The ID of the parameter + * @return The value of the parameter + */ + inline float get_param_float(uint16_t id) const { return params.values[id].fvalue; } + + /** + * @brief Get the name of a parameter + * @param id The ID of the parameter + * @return The name of the parameter + */ + inline const char * get_param_name(uint16_t id) const { return params.names[id]; } + + /** + * @brief Get the type of a parameter + * @param id The ID of the parameter + * @return The type of the parameter + * This returns one of three possible types + * PARAM_TYPE_INT32, PARAM_TYPE_FLOAT, or PARAM_TYPE_INVALID + * See line 165 + */ + inline param_type_t get_param_type(uint16_t id) const { return params.types[id]; } + + /** + * @brief Sets the value of a parameter by ID and calls the parameter change callback + * @param id The ID of the parameter + * @param value The new value + * @return True if a parameter value was changed, false otherwise + */ + bool set_param_int(uint16_t id, int32_t value); + + /** + * @brief Sets the value of a floating point parameter by ID and calls the parameter callback + * @param id The ID of the parameter + * @param value The new value + * @return True if a parameter was changed, false otherwise + */ + bool set_param_float(uint16_t id, float value); + + /** + * @brief Sets the value of a parameter by name and calls the parameter change callback + * @param name The name of the parameter + * @param value The new value + * @return True if a parameter value was changed, false otherwise + */ + bool set_param_by_name_int(const char name[PARAMS_NAME_LENGTH], int32_t value); + + /** + * @brief Sets the value of a floating point parameter by name and calls the parameter change + * callback + * @param name The name of the parameter + * @param value The new value + * @return True if a parameter value was changed, false otherwise + */ + bool set_param_by_name_float(const char name[PARAMS_NAME_LENGTH], float value); +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_PARAM_H diff --git a/test/rosflight.h b/test/rosflight.h new file mode 100644 index 0000000..778c651 --- /dev/null +++ b/test/rosflight.h @@ -0,0 +1,93 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_ROSFLIGHT_H +#define ROSFLIGHT_FIRMWARE_ROSFLIGHT_H + +#include "interface/comm_link.h" +#include "interface/param_listener.h" + +#include "board.h" +#include "comm_manager.h" +#include "command_manager.h" +#include "controller.h" +#include "estimator.h" +#include "mixer.h" +#include "param.h" +#include "rc.h" +#include "sensors.h" +#include "state_manager.h" + +#include + +namespace rosflight_firmware +{ +class ROSflight +{ +public: + ROSflight(Board & board, CommLinkInterface & comm_link); + + Board & board_; + CommManager comm_manager_; + + Params params_; + + CommandManager command_manager_; + Controller controller_; + Estimator estimator_; + Mixer mixer_; + RC rc_; + Sensors sensors_; + StateManager state_manager_; + + uint32_t loop_time_us; + + /** + * @brief Main initialization routine for the ROSflight autopilot flight stack + */ + void init(); + + /** + * @brief Main loop for the ROSflight autopilot flight stack + */ + void run(); + + uint32_t get_loop_time_us(); + +private: + static constexpr size_t num_param_listeners_ = 7; + ParamListenerInterface * const param_listeners_[num_param_listeners_] = { + &comm_manager_, &command_manager_, &controller_, &estimator_, &mixer_, &rc_, &sensors_}; +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_ROSFLIGHT_H diff --git a/test/state_manager.h b/test/state_manager.h new file mode 100644 index 0000000..9283aa2 --- /dev/null +++ b/test/state_manager.h @@ -0,0 +1,193 @@ +/* + * 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_STATE_MANAGER_H +#define ROSFLIGHT_FIRMWARE_STATE_MANAGER_H + +#include "util.h" + +#include +#include + +namespace rosflight_firmware +{ +class ROSflight; + +class StateManager +{ +public: + struct State + { + bool armed; + bool failsafe; + bool error; + uint16_t error_codes; + }; + + enum Event + { + EVENT_INITIALIZED, + EVENT_REQUEST_ARM, + EVENT_REQUEST_DISARM, + EVENT_RC_LOST, + EVENT_RC_FOUND, + EVENT_ERROR, + EVENT_NO_ERROR, + EVENT_CALIBRATION_COMPLETE, + EVENT_CALIBRATION_FAILED, + }; + + enum : uint16_t + { + ERROR_NONE = 0x0000, + ERROR_INVALID_MIXER = 0x0001, + ERROR_IMU_NOT_RESPONDING = 0x0002, + ERROR_RC_LOST = 0x0004, + ERROR_UNHEALTHY_ESTIMATOR = 0x0008, + ERROR_TIME_GOING_BACKWARDS = 0x0010, + ERROR_UNCALIBRATED_IMU = 0x0020, + ERROR_INVALID_FAILSAFE = 0x0040, + }; + + /** + * @brief Stores backup data for restoring the system state after a hard fault + * + * Also stores debugging information to be sent over serial after the connection + * is restored. + */ + struct __attribute__((packed)) BackupData + { + static constexpr uint32_t ARM_MAGIC = + 0xbad2fa11; //!< magic number to ensure we only arm on startup if we really intended to + + uint16_t reset_count = 0; //!< number of hard faults since normal system startup + uint16_t error_code = 0; //!< state manager error codes + uint32_t arm_flag = + 0; //!< set to ARM_MAGIC if the system was armed when the hard fault occured, 0 otherwise + + /** + * @brief Low-level debugging information for case of hard fault + * + * It is up to the board support layer to populate this data + */ + struct DebugInfo + { + uint32_t r0; //!< register 0 + uint32_t r1; //!< register 1 + uint32_t r2; //!< register 2 + uint32_t r3; //!< register 3 + uint32_t r12; //!< register 12 + uint32_t lr; //!< link register + uint32_t pc; //!< program counter + uint32_t psr; //!< program status register + } debug; + + uint32_t checksum = 0; //!< checksum for data in the struct, must be the last member + + /** + * @brief Computes checksum and prepares struct to be written to backup memory + * @pre All data fields have been set, no data will be changed between calling this function and + * writing to memory + * @post Checksum field is set and the struct is ready to be written to backup memory + */ + void finalize() + { + checksum = checksum_fletcher16(reinterpret_cast(this), + sizeof(BackupData) - sizeof(checksum)); + } + + /** + * @brief Checks whether the checksum of a struct read from backup memory is valid + * + * @return true The checksum is valid + * @return false The checksum is invalid + */ + bool valid_checksum() + { + return checksum + == checksum_fletcher16(reinterpret_cast(this), + sizeof(BackupData) - sizeof(checksum)); + } + }; + + StateManager(ROSflight & parent); + void init(); + void run(); + + inline const State & state() const { return state_; } + + void set_event(Event event); + void set_error(uint16_t error); + void clear_error(uint16_t error); + + /** + * @brief Write recovery data to backup memory in the case of a hard fault + * + * This function should only be called by the hardfault interrupt handler + * + * @pre Called from hardfault interrupt handler + * @post Recovery data has been written to backup RAM and the hardfault interrupt handler may now + * reset the system + * + * @param debug Low-level debugging data populated by the hardfault handler + */ + void write_backup_data(const BackupData::DebugInfo & debug); + + void check_backup_memory(); + +private: + ROSflight & RF_; + State state_; + + uint32_t next_led_blink_ms_ = 0; + uint32_t next_arming_error_msg_ms_ = 0; + + uint32_t hardfault_count_ = 0; + + enum FsmState + { + FSM_STATE_INIT, + FSM_STATE_PREFLIGHT, + FSM_STATE_ARMED, + FSM_STATE_ERROR, + FSM_STATE_FAILSAFE, + FSM_STATE_CALIBRATING + }; + + FsmState fsm_state_; + void process_errors(); + + void update_leds(); +}; + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_STATE_MANAGER_H diff --git a/test/util.h b/test/util.h new file mode 100644 index 0000000..ed75a5e --- /dev/null +++ b/test/util.h @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2020, James Jackson, Daniel Koch, and Trey Henrichsen, 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 + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef ROSFLIGHT_FIRMWARE_UTIL_H +#define ROSFLIGHT_FIRMWARE_UTIL_H + +#include +#include +#include + +namespace rosflight_firmware +{ +/** + * @brief Fletcher 16-bit checksum + * + * @param src Pointer to data on which to compute the checksum + * @param len Number of bytes in the data + * @param finalize Whether to finalize the checksum; set to false for intermediate chunks of + * non-contiguous data + * @param start Value at which to start the checksum, set for subsequent calls on chunks of + * non-contiguous data + * @return uint16_t Fletcher 16-bit checksum + */ +inline uint16_t checksum_fletcher16(const uint8_t * src, size_t len, bool finalize = true, + uint16_t start = 0) +{ + static constexpr size_t max_block_length = + 5800; // guarantee that no overflow will occur (reduce from standard value + // to account for values in 'start') + + uint32_t c1 = (start & 0xFF00) >> 8; + uint32_t c2 = start & 0x00FF; + + size_t block_length; + for (; len > 0; len -= block_length) { + block_length = len < max_block_length ? len : max_block_length; + for (size_t i = 0; i < block_length; i++) { + c1 += *(src++); + c2 += c1; + } + + c1 %= 255; + c2 %= 255; + } + + uint16_t checksum = c1 << 8 | c2; + + if (finalize && checksum == 0) { checksum = 0xFFFF; } + + return checksum; +} + +} // namespace rosflight_firmware + +#endif // ROSFLIGHT_FIRMWARE_UTIL_H