diff --git a/.gitignore b/.gitignore index b435816a7575c3..2769d317904195 100644 --- a/.gitignore +++ b/.gitignore @@ -156,8 +156,6 @@ build.tmp.binaries/ tasklist.json modules/esp_idf -/libraries/AC_Simulink -/libraries/AC_Simulink* # Ignore Python virtual environments # from: https://github.com/github/gitignore/blob/4488915eec0b3a45b5c63ead28f286819c0917de/Python.gitignore#L125 .env diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 867f1d7ef38cb8..559291fd4f895e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -73,6 +73,11 @@ #include #include + +#if MODE_LAB_ENABLED +#include +#endif + // Configuration #include "defines.h" #include "config.h" @@ -316,6 +321,10 @@ class Copter : public AP_Vehicle { // Arming/Disarming management class AP_Arming_Copter arming; +#if MODE_LAB_ENABLED + AC_Simulink simulinkController; +#endif + // Optical flow sensor #if AP_OPTICALFLOW_ENABLED AP_OpticalFlow optflow; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index cb67811a9c4e2e..0502722c7358af 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -4,6 +4,7 @@ #include #include + MAV_TYPE GCS_Copter::frame_type() const { /* @@ -1506,7 +1507,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg) #endif #if MODE_LAB_ENABLED == ENABLED case MAVLINK_MSG_ID_LAB_FROM_DASHBOARD: - copter.mode_lab.handle_message(msg); + copter.simulinkController.handle_message(msg); break; #endif default: diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 264a9982272f81..67fd6207e57eae 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -396,8 +396,6 @@ class ModeLab : public Mode { void change_motor_direction(bool reverse); void output_to_motors() override; - void handle_message(const mavlink_message_t &msg); - protected: const char *name() const override { return "LAB"; } const char *name4() const override { return "LAB"; } @@ -405,16 +403,11 @@ class ModeLab : public Mode { private: void arm_motors(); void disarm_motors(); - void check_if_received_message_from_dashboard(float threshold_ms); + void monitorDashboardMessage(float threshold_ms); - float motor_out_1,motor_out_2,motor_out_3,motor_out_4; + float *motor_out; uint32_t last_throttle_warning_output_ms; - - u_int8_t master_switch; - float ref_pos_x,ref_pos_y,ref_pos_z; - float ref_orient_yaw, ref_orient_pitch, ref_orient_roll; - float ref_power_gain; - + uint32_t last_dashboard_msg_ms; uint32_t start_time; diff --git a/ArduCopter/mode_lab.cpp b/ArduCopter/mode_lab.cpp index 18cf1e713d1fd7..7a6e14cee04493 100644 --- a/ArduCopter/mode_lab.cpp +++ b/ArduCopter/mode_lab.cpp @@ -1,207 +1,67 @@ +// Description: ModeLab class is a child class of the Mode class. It is used to run a simulink controller. + #include "Copter.h" -#include -FCS_model labController; -#include #include #include - +#include #if MODE_LAB_ENABLED == ENABLED -/* - * Init and run calls for lab flight mode - */ - // lab_init - initialise lab controller bool ModeLab::init(bool ignore_checks) -{ - // initialise the lab controller - labController.initialize(); +{ + // Initialise the controller + copter.simulinkController.inititalize(); - // arm motors + // Arm motors. Overrides and disables failsafes. arm_motors(); - // Start timers - start_time = AP_HAL::millis(); - last_dashboard_msg_ms = AP_HAL::millis(); - - // set the home for the ahrs TODO: returns zeros - copter.set_home_to_current_location_inflight(); - - // turn on notify leds + // Turn on notify leds AP_Notify::flags.esc_calibration = true; - // set the initial motor out values - motor_out_1 = 0.0f; - motor_out_2 = 0.0f; - motor_out_3 = 0.0f; - motor_out_4 = 0.0f; - - // set the initial reference values - master_switch = 0; - ref_power_gain = 0.0f; - ref_pos_x = 0.0f; - ref_pos_y = 0.0f; - ref_pos_z = 0.0f; - ref_orient_yaw = 0.0f; - ref_orient_pitch = 0.0f; - ref_orient_roll = 0.0f; - - gcs().send_text(MAV_SEVERITY_INFO, "LAB: intitialised"); + // Debug message to the GCS + gcs().send_text(MAV_SEVERITY_INFO, "LAB: intitialised"); // Reaches this point return true; } -void ModeLab::check_if_received_message_from_dashboard(float threshold_ms) -{ - uint32_t dashboard_msg_time = AP_HAL::millis() - last_dashboard_msg_ms; - if (dashboard_msg_time > threshold_ms) { - // Send a warning message to the GCS - gcs().send_text(MAV_SEVERITY_WARNING, "LAB: no message from dashboard in: %f ms" , double(dashboard_msg_time)); - // Run internal exit function - exit(); - // Switch back to the default mode - set_mode(Mode::Number::STABILIZE, ModeReason::GCS_COMMAND); - } -} - // lab_run - runs the lab controller // should be called at 100hz to match the controller rate void ModeLab::run() { - // check if we have received a message from the dashboard within a given time, otherwise exit - check_if_received_message_from_dashboard(2000); // 2 seconds - - // Prepare the arguments for the controller, given by the generated ert_main ------------ + // check if we have received a message from the dashboard + monitorDashboardMessage(2000); - // '/accel' ------------------------------------- - Vector3f accel_vals = ahrs.get_accel(); - float arg_accel[3]{ accel_vals.x, accel_vals.y, accel_vals.z }; + // run the controller + copter.simulinkController.runController(motor_out); - // '/gyro' -------------------------------------- - Vector3f gyro_vals = ahrs.get_gyro(); - float arg_gyro[3]{ gyro_vals.x, gyro_vals.y, gyro_vals.z }; + float *logging_data = copter.simulinkController.get_logging_data(); - // '/bat_V' ------------------------------------- - float arg_bat_V{ 0.0F }; - - // '/pos_est' ----------------------------------- - Vector3f position; - float arg_pos_est[3]; - if (ahrs.get_relative_position_NED_origin(position)) { - arg_pos_est[0] = position.x; - arg_pos_est[1] = position.y; - arg_pos_est[2] = position.z; - } else { - // This should instead return the previous value - // Assign the previous values to arg_pos_est - arg_pos_est[0] = 0.0F; - arg_pos_est[1] = 0.0F; - arg_pos_est[2] = 0.0F; - } + gcs().send_text(MAV_SEVERITY_INFO, "LAB: logging data %f %f %f %f", logging_data[0], logging_data[1], logging_data[2], logging_data[3]); + // send the logging data to the dashboard + // gcs().send_message(MSG_LAB_TO_DASHBOARD); // TODO: Implement this message /home/joseph/ardupilot_DroneLab/libraries/GCS_MAVLink/GCS_Common.cpp:5445 + +} - // '/vel_est' ----------------------------------- - Vector3f velocity; - float arg_vel_est[3]; - if (ahrs.get_velocity_NED(velocity)) { - arg_vel_est[0] = velocity.x; - arg_vel_est[1] = velocity.y; - arg_vel_est[2] = velocity.z; - } else { - // This should instead return the previous value - // Assign the previous values to arg_vel_est - arg_vel_est[0] = 0.0F; - arg_vel_est[1] = 0.0F; - arg_vel_est[2] = 0.0F; - } +void ModeLab::monitorDashboardMessage(float threshold_ms) +{ + if(!copter.simulinkController.recentDashboardMessage(threshold_ms)) { + // Send a warning message to the GCS + gcs().send_text(MAV_SEVERITY_WARNING, "LAB: no message from dashboard in: %f ms" , threshold_ms); - // '/yaw_opticalfow' ---------------------------- - // const AP_OpticalFlow *optflow = AP::opticalflow(); - const Vector2f &flowRate = Vector2f(0.0F, 0.0F); //optflow->flowRate(); - float arg_yaw_opticalfow{ atan2f(flowRate.y, flowRate.x) }; - - // '/pos_ref' ----------------------------------- - float arg_pos_ref[3]{ ref_pos_x, ref_pos_y, ref_pos_z}; - - // '/orient_ref' -------------------------------- - float arg_orient_ref[3]{ ref_orient_yaw, ref_orient_pitch, ref_orient_roll}; - - // Return variables from the controller --------------- - // '/motors_refout' - float arg_motors_refout[4]; - - // '/logging_refout' !! The array size is modified during the build process. See also common.xml !! - float arg_logging_refout[23]; - - labController.step(arg_accel, arg_gyro, &arg_bat_V, arg_pos_est, arg_vel_est, - &arg_yaw_opticalfow, arg_pos_ref, arg_orient_ref, arg_motors_refout, arg_logging_refout); - - // update the motor output, if the master switch is off, set all motors to 0 - // otherwise set the motors to the output from the controller times the power gain - if (master_switch != 0) { - // Check if the power gain is within the range (0-1) - if (ref_power_gain > 1.0f || ref_power_gain < 0.0f) { - ref_power_gain = 0.0f; - gcs().send_text(MAV_SEVERITY_WARNING, "LAB: power gain out of (0-1) range: %f", ref_power_gain); - } - // PWM output is between 1000 and 2000 (0% - 100%) - motor_out_1 = arg_motors_refout[0] * ref_power_gain * 1000 + 1000; - motor_out_2 = arg_motors_refout[1] * ref_power_gain * 1000 + 1000; - motor_out_3 = arg_motors_refout[2] * ref_power_gain * 1000 + 1000; - motor_out_4 = arg_motors_refout[3] * ref_power_gain * 1000 + 1000; - } - else { - motor_out_1 = 0.0F; - motor_out_2 = 0.0F; - motor_out_3 = 0.0F; - motor_out_4 = 0.0F; - } - - // Define a new array logging_full with timeSinceStart as the first element - float timeSinceStart_s = float(AP_HAL::millis() - start_time) / 1000.0f; // in seconds - - size_t size_controller_logging = sizeof(arg_logging_refout) / sizeof(arg_logging_refout[23]); - - float logging_full[size_controller_logging+1]; - - logging_full[0] = timeSinceStart_s; - - for (int i = 0; i < size_controller_logging; ++i) { - logging_full[i + 1] = arg_logging_refout[i]; + // Switch back to the default mode + set_mode(Mode::Number::STABILIZE, ModeReason::GCS_COMMAND); } - // send logging data to the dashboard - mavlink_channel_t chan = MAVLINK_COMM_0; - mavlink_msg_lab_to_dashboard_send(chan, logging_full); } void ModeLab::exit() { disarm_motors(); - + gcs().send_text(MAV_SEVERITY_WARNING, "LAB:EXITING"); // turn off notify leds AP_Notify::flags.esc_calibration = false; } -void ModeLab::disarm_motors() -{ - if (!hal.util->get_soft_armed()) { - return; - } - - // disarm - motors->armed(false); - hal.util->set_soft_armed(false); - - // un-reverse the motors - change_motor_direction(false); - hal.rcout->enable_channel_mask_updates(); - - // re-enable failsafes - g.failsafe_throttle.load(); - g.failsafe_gcs.load(); - g.fs_ekf_action.load(); -} - void ModeLab::change_motor_direction(bool reverse) { AP_HAL::RCOutput::BLHeliDshotCommand direction = reverse ? AP_HAL::RCOutput::DSHOT_REVERSE : AP_HAL::RCOutput::DSHOT_NORMAL; @@ -247,6 +107,25 @@ void ModeLab::arm_motors() hal.util->set_soft_armed(true); } +void ModeLab::disarm_motors() +{ + if (!hal.util->get_soft_armed()) { + return; + } + + // disarm + motors->armed(false); + hal.util->set_soft_armed(false); + + // un-reverse the motors + change_motor_direction(false); + hal.rcout->enable_channel_mask_updates(); + + // re-enable failsafes + g.failsafe_throttle.load(); + g.failsafe_gcs.load(); + g.fs_ekf_action.load(); +} // actually write values to the motors void ModeLab::output_to_motors() @@ -254,43 +133,22 @@ void ModeLab::output_to_motors() // check if motor are allowed to spin const bool allow_output = motors->armed() && motors->get_interlock(); - if (allow_output) { - - float motor_outputs[] = {motor_out_1, motor_out_2, motor_out_3, motor_out_4}; - gcs().send_text(MAV_SEVERITY_INFO, "PWM:: %f,%f,%f,%f", motor_out_1, motor_out_2, motor_out_3, motor_out_4); + if (allow_output && copter.simulinkController.get_master_switch()) { + + // get power gain from the controller - between 0 and 1 + float powerGain = copter.simulinkController.get_power_gain(); // convert output to PWM and send to each motor int8_t i; for (i = 0; i <= 3; i++) { if (motors->is_motor_enabled(i)) { - motors->rc_write(i, motor_outputs[i]); + float motor_pwm = motor_out[i] * powerGain * 1000 + 1000;; + motors->rc_write(i, motor_pwm); } } } } -// handle a mavlink message coming in from the dashboard -void ModeLab::handle_message(const mavlink_message_t &msg) -{ - // keep track of the last time we received a message from the dashboard - last_dashboard_msg_ms = AP_HAL::millis(); - - if (msg.msgid != MAVLINK_MSG_ID_LAB_FROM_DASHBOARD) { - return; - } - mavlink_lab_from_dashboard_t m; - mavlink_msg_lab_from_dashboard_decode(&msg, &m); - - master_switch = m.master_switch; - ref_power_gain = m.power; - ref_pos_x = m.ref_x; - ref_pos_y = m.ref_y; - ref_pos_z = m.ref_z; - ref_orient_yaw = m.ref_yaw; - ref_orient_pitch = m.ref_pitch; - ref_orient_roll = m.ref_roll; - -} #endif diff --git a/libraries/AC_Simulink/AC_Simulink.cpp b/libraries/AC_Simulink/AC_Simulink.cpp new file mode 100644 index 00000000000000..69a6672fd9b76b --- /dev/null +++ b/libraries/AC_Simulink/AC_Simulink.cpp @@ -0,0 +1,166 @@ +# include "AC_Simulink.h" + +AC_Simulink::AC_Simulink() { + // _singleton = this; +} + +AC_Simulink *AC_Simulink::_singleton; + +void AC_Simulink::inititalize() { + + // Initialize the Simulink controller + simulinkModel.initialize(); + + // Initialize the variables from the dashboard + master_switch = 0; + ref_power_gain = 0.0f; + ref_pos_x = 0.0f; + ref_pos_y = 0.0f; + ref_pos_z = 0.0f; + ref_orient_yaw = 0.0f; + ref_orient_pitch = 0.0f; + ref_orient_roll = 0.0f; + + // Initialize the time variables + last_dashboard_msg_ms = AP_HAL::millis(); + start_time = AP_HAL::millis(); +} + +// Method to wrap the Simulink controller +void AC_Simulink::runController(float* arg_motors_refout) { + // Prepare the arguments for the controller, given by the generated ert_main ------------ + + // '/accel' ------------------------------------- + const AP_AHRS &ahrs = AP::ahrs(); + Vector3f accel_vals = ahrs.get_accel(); + float arg_accel[3]{ accel_vals.x, accel_vals.y, accel_vals.z }; + + // '/gyro' -------------------------------------- + Vector3f gyro_vals = ahrs.get_gyro(); + float arg_gyro[3]{ gyro_vals.x, gyro_vals.y, gyro_vals.z }; + + // '/bat_V' ------------------------------------- + AP_BattMonitor &battery = AP::battery(); + float arg_bat_V{ battery.voltage() }; + + // '/pos_est' ----------------------------------- + Vector3f position; + float arg_pos_est[3]; + if (ahrs.get_relative_position_NED_origin(position)) { + arg_pos_est[0] = position.x; + arg_pos_est[1] = position.y; + arg_pos_est[2] = position.z; + } else { + // This should instead return the previous value + // Assign the previous values to arg_pos_est + arg_pos_est[0] = 0.0F; + arg_pos_est[1] = 0.0F; + arg_pos_est[2] = 0.0F; + } + + // '/vel_est' ----------------------------------- + Vector3f velocity; + float arg_vel_est[3]; + if (ahrs.get_velocity_NED(velocity)) { + arg_vel_est[0] = velocity.x; + arg_vel_est[1] = velocity.y; + arg_vel_est[2] = velocity.z; + } else { + // This should instead return the previous value + // Assign the previous values to arg_vel_est + arg_vel_est[0] = 0.0F; + arg_vel_est[1] = 0.0F; + arg_vel_est[2] = 0.0F; + } + + // '/yaw_opticalfow' ---------------------------- + // const AP_OpticalFlow *optflow = AP::opticalflow(); + // const Vector2f &flowRate = optflow->flowRate(); + + Vector2f flowRate = {0.0f, 0.0f}; + float arg_yaw_opticalfow{ atan2f(flowRate.y, flowRate.x) }; + + // '/pos_ref' ----------------------------------- + float arg_pos_ref[3]{ ref_pos_x, ref_pos_y, ref_pos_z}; + + // '/orient_ref' -------------------------------- + float arg_orient_ref[3]{ ref_orient_yaw, ref_orient_pitch, ref_orient_roll}; + + // Return variables from the controller --------------- + // '/motors_refout' + // float arg_motors_refout[4]; + + // '/logging_refout' !! The array size is modified during the build process. See also common.xml !! + // float arg_logging_refout[23]; + + simulinkModel.step(arg_accel, arg_gyro, &arg_bat_V, arg_pos_est, arg_vel_est, + &arg_yaw_opticalfow, arg_pos_ref, arg_orient_ref, arg_motors_refout, arg_logging_refout); + + + gcs().send_text(MAV_SEVERITY_INFO, "SIMULINK: logging data %f %f %f %f", double( AP_HAL::millis()) , arg_logging_refout[1], arg_logging_refout[2], arg_logging_refout[3]); +} + +// TODO: change the build process to change this instead +static int lengthOfLogging_Simulink = 23; +static int lengthOfLogging_Internal = 1; +float* arg_logging_refout = new float[lengthOfLogging_Simulink]; +float* logging_data = new float[lengthOfLogging_Internal + lengthOfLogging_Simulink]; + +// Method to get the logging data from the controller. Used for mavlink message +float* AC_Simulink::get_logging_data() { + + // log internal messages first + float timeSinceStart_s = float(AP_HAL::millis() - start_time) / 1000.0f; // in seconds + logging_data[0] = timeSinceStart_s; + + // log the data from the controller + for (int i = 0; i < lengthOfLogging_Simulink; ++i) { + logging_data[i + 1] = arg_logging_refout[i]; + } + + return logging_data; +} + +float AC_Simulink::get_power_gain() { + return ref_power_gain; +} + +bool AC_Simulink::get_master_switch() { + return master_switch; +} + +bool AC_Simulink::recentDashboardMessage(float threshold_ms) { + uint32_t dashboard_msg_time = AP_HAL::millis() - last_dashboard_msg_ms; + if (dashboard_msg_time > threshold_ms) { + return false; + } + return true; +} + +// handle a mavlink message coming in from the dashboard +void AC_Simulink::handle_message(const mavlink_message_t &msg) +{ + // keep track of the last time we received a message from the dashboard + last_dashboard_msg_ms = AP_HAL::millis(); + + if (msg.msgid != MAVLINK_MSG_ID_LAB_FROM_DASHBOARD) { + return; + } + mavlink_lab_from_dashboard_t m; + mavlink_msg_lab_from_dashboard_decode(&msg, &m); + + master_switch = m.master_switch; + ref_power_gain = m.power; + ref_pos_x = m.ref_x; + ref_pos_y = m.ref_y; + ref_pos_z = m.ref_z; + ref_orient_yaw = m.ref_yaw; + ref_orient_pitch = m.ref_pitch; + ref_orient_roll = m.ref_roll; + +} + + + + + diff --git a/libraries/AC_Simulink/AC_Simulink.h b/libraries/AC_Simulink/AC_Simulink.h new file mode 100644 index 00000000000000..be71e0ada466a8 --- /dev/null +++ b/libraries/AC_Simulink/AC_Simulink.h @@ -0,0 +1,74 @@ +// Description: This is the header file for the AC_Simulink class. +// This class wraps the Simulink controller and handles the communication between the controller, dashboard, and the rest of the ArduPilot code. +// The class includes methods to: +// 1. Wrap a simulink based controller, +// 2. Package logging data from simulink into a mavlink message for the dashboard, +// 3. Process mavlink messages coming in from the dashboard for the controller, +// 4. House safety checks for the controller, like checking if a message was received from the dashboard within a certain threshold, + + +#pragma once + +#ifndef MODE_LAB_ENABLED +#define MODE_LAB_ENABLED 1 +#endif + +#include +#include +#include +#include +#include +#include +#include +#include + + +class AC_Simulink { +public: + // Constructor + AC_Simulink(); + + void inititalize(); + + // Method to run the wrapped Simulink controller + void runController(float* motor_out); + + // Method to handle a mavlink message coming in from the dashboard + void handle_message(const mavlink_message_t &msg); + + // Method to check if a message was received from the dashboard within a certain threshold + bool recentDashboardMessage(float threshold_ms); + + // Method to get the logging data from the controller. Used for mavlink message + float* get_logging_data(); + + // Methods to get the power gain and master switch from the dashboard. Used for motor output + float get_power_gain(); + bool get_master_switch(); + + // get singleton instance + static AC_Simulink *get_singleton() { return _singleton; } + +private: + static AC_Simulink *_singleton; + + // Simulink controller + FCS_model simulinkModel; + + // Variables from the dashboard + bool master_switch; + float ref_power_gain; + float ref_pos_x,ref_pos_y,ref_pos_z; + float ref_orient_yaw, ref_orient_pitch, ref_orient_roll; + + // Counter for the last time a message was received from the dashboard + uint32_t last_dashboard_msg_ms; + + // Time value for the logging data + uint32_t start_time; + + // Pointers for variable length logging data + float* logging_data; + float* arg_logging_refout; + +}; diff --git a/libraries/AC_Simulink/FCS_model.cpp b/libraries/AC_Simulink/FCS_model.cpp index 8eb594a7ff5fae..c4c4ee8d6df0e7 100755 --- a/libraries/AC_Simulink/FCS_model.cpp +++ b/libraries/AC_Simulink/FCS_model.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -20,6 +20,7 @@ #include "rtwtypes.h" #include "FCS_model_private.h" #include +#include "rt_defines.h" extern "C" { @@ -28,8 +29,6 @@ extern "C" } -#include "rt_defines.h" - real32_T look1_iflf_binlx(real32_T u0, const real32_T bp0[], const real32_T table[], uint32_T maxIndex) { @@ -136,18 +135,16 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T real_T rtb_Sum1_jt; real_T rtb_DataTypeConversion6[3]; real_T rtb_DataTypeConversion7[3]; - real_T rtb_Product5; + real_T rtb_roll; + real_T Sum; real_T numAccum; - real_T numAccum_0; - real_T rtb_Gain4; real_T rtb_Product3; - real_T rtb_Product4; - real_T rtb_pitch; - int32_T iU; + real_T rtb_TrigonometricFunction4; + real_T rtb_pitchrate; + real_T rtb_rollrate; real32_T rtb_DataTypeConversion2_i; real32_T rtb_DataTypeConversion6_b; real32_T rtb_On1Off0forthrust; - real32_T u0; UNUSED_PARAMETER(arg_bat_V); // If: '/If1' incorporates: @@ -198,23 +195,24 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T // End of If: '/If1' // DiscreteTransferFcn: '/Discrete Transfer Fcn' - numAccum = FCS_model_P.DiscreteTransferFcn_NumCoef_n[1] * + Sum = FCS_model_P.DiscreteTransferFcn_NumCoef_n[1] * FCS_model_DW.DiscreteTransferFcn_states; // DiscreteTransferFcn: '/Discrete Transfer Fcn' - numAccum_0 = FCS_model_P.DiscreteTransferFcn_NumCoef_a[1] * + numAccum = FCS_model_P.DiscreteTransferFcn_NumCoef_a[1] * FCS_model_DW.DiscreteTransferFcn_states_e; // DiscreteStateSpace: '/Internal' { - rtb_Product5 = (FCS_model_P.Internal_C[0])*FCS_model_DW.Internal_DSTATE[0] + rtb_roll = (FCS_model_P.Internal_C[0])*FCS_model_DW.Internal_DSTATE[0] + (FCS_model_P.Internal_C[1])*FCS_model_DW.Internal_DSTATE[1]; } - // Product: '/Product5' incorporates: + // Trigonometry: '/Trigonometric Function' incorporates: // Constant: '/Constant1' // DataTypeConversion: '/Data Type Conversion4' // DataTypeConversion: '/Data Type Conversion5' + // Gain: '/Gain' // Gain: '/KDz' // Gain: '/zDz' // Inport: '/pos_est' @@ -223,35 +221,34 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T // Sum: '/Sum2' // Sum: '/Sum3' - rtb_Product5 -= arg_pos_est[2]; - rtb_Product5 *= FCS_model_P.zDz; - rtb_Product5 -= arg_vel_est[2]; - rtb_Product5 = -FCS_model_P.Vehicle.Airframe.mass * FCS_model_P.g + - FCS_model_P.KDz * rtb_Product5; + rtb_roll -= arg_pos_est[2]; + rtb_roll *= FCS_model_P.zDz; + rtb_roll -= arg_vel_est[2]; + rtb_roll = FCS_model_P.KDz * rtb_roll * FCS_model_P.Gain_Gain + + -FCS_model_P.Vehicle.Airframe.mass * FCS_model_P.g; // Saturate: '/SaturationThrust' - if (rtb_Product5 > FCS_model_P.SaturationThrust_UpperSat) { - rtb_Gain4 = FCS_model_P.SaturationThrust_UpperSat; - } else if (rtb_Product5 < FCS_model_P.SaturationThrust_LowerSat) { - rtb_Gain4 = FCS_model_P.SaturationThrust_LowerSat; + if (rtb_roll > FCS_model_P.SaturationThrust_UpperSat) { + rtb_rollrate = FCS_model_P.SaturationThrust_UpperSat; + } else if (rtb_roll < FCS_model_P.SaturationThrust_LowerSat) { + rtb_rollrate = FCS_model_P.SaturationThrust_LowerSat; } else { - rtb_Gain4 = rtb_Product5; + rtb_rollrate = rtb_roll; } // Gain: '/On=1//Off=0 for thrust' incorporates: // Saturate: '/SaturationThrust' rtb_On1Off0forthrust = FCS_model_P.On1Off0forthrust_Gain * - static_cast(rtb_Gain4); + static_cast(rtb_rollrate); // DiscreteStateSpace: '/Internal' { - rtb_Product5 = (FCS_model_P.Internal_C_a[0])*FCS_model_DW.Internal_DSTATE_h - [0] + rtb_roll = (FCS_model_P.Internal_C_a[0])*FCS_model_DW.Internal_DSTATE_h[0] + (FCS_model_P.Internal_C_a[1])*FCS_model_DW.Internal_DSTATE_h[1]; } - // Product: '/Product5' incorporates: + // Trigonometry: '/Trigonometric Function' incorporates: // Constant: '/yaw equilibrium' // DataTypeConversion: '/Data Type Conversion1' // Gain: '/KDpsi' @@ -263,36 +260,34 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T // Sum: '/Sum3' // Sum: '/Sum3' - rtb_Product5 = FCS_model_P.zDpsi * 2.0 * (rtb_Product5 - + rtb_roll = FCS_model_P.zDpsi * 2.0 * (rtb_roll - FCS_model_DW.Memory_PreviousInput); - rtb_Product5 -= arg_gyro[2]; - rtb_Product5 = (FCS_model_P.KDpsi * 1.3 * rtb_Product5 * - FCS_model_P.ChangingofJzz_Gain + - FCS_model_P.yawequilibrium_Value) * - FCS_model_P.On1Off1foryaw_Gain; + rtb_roll -= arg_gyro[2]; + rtb_roll = (FCS_model_P.KDpsi * 1.3 * rtb_roll * + FCS_model_P.ChangingofJzz_Gain + FCS_model_P.yawequilibrium_Value) + * FCS_model_P.On1Off1foryaw_Gain; // DataTypeConversion: '/Data Type Conversion2' - rtb_DataTypeConversion2_i = static_cast(rtb_Product5); + rtb_DataTypeConversion2_i = static_cast(rtb_roll); // DiscreteStateSpace: '/Internal' { - rtb_Product5 = (FCS_model_P.Internal_C_i[0])*FCS_model_DW.Internal_DSTATE_a - [0] + rtb_roll = (FCS_model_P.Internal_C_i[0])*FCS_model_DW.Internal_DSTATE_a[0] + (FCS_model_P.Internal_C_i[1])*FCS_model_DW.Internal_DSTATE_a[1]; } // Sum: '/Sum1' incorporates: // DiscreteTransferFcn: '/Discrete Transfer Fcn' - rtb_Sum1_j = rtb_Product5 - numAccum; + rtb_Sum1_j = rtb_roll - Sum; // DiscreteStateSpace: '/Internal' { - rtb_Product5 = FCS_model_P.Internal_C_ai*FCS_model_DW.Internal_DSTATE_g; - rtb_Product5 += FCS_model_P.Internal_D_e*rtb_Sum1_j; + rtb_roll = FCS_model_P.Internal_C_ai*FCS_model_DW.Internal_DSTATE_g; + rtb_roll += FCS_model_P.Internal_D_e*rtb_Sum1_j; } - // Product: '/Product5' incorporates: + // Trigonometry: '/Trigonometric Function' incorporates: // Constant: '/pitch equilibrium' // DataTypeConversion: '/Data Type Conversion1' // Gain: '/Changing of Jyy' @@ -303,35 +298,34 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T // Sum: '/Sum1' // Sum: '/Sum3' - rtb_Product5 *= FCS_model_P.zDtheta * 7.0; - rtb_Product5 -= arg_gyro[1]; - rtb_Product5 = (FCS_model_P.KDtheta * 1.4 * rtb_Product5 * - FCS_model_P.ChangingofJyy_Gain + - FCS_model_P.pitchequilibrium_Value) * + rtb_roll *= FCS_model_P.zDtheta * 7.0; + rtb_roll -= arg_gyro[1]; + rtb_roll = (FCS_model_P.KDtheta * 1.4 * rtb_roll * + FCS_model_P.ChangingofJyy_Gain + + FCS_model_P.pitchequilibrium_Value) * FCS_model_P.On1Off2forpitch_Gain; // DataTypeConversion: '/Data Type Conversion6' - rtb_DataTypeConversion6_b = static_cast(rtb_Product5); + rtb_DataTypeConversion6_b = static_cast(rtb_roll); // DiscreteStateSpace: '/Internal' { - rtb_Product5 = (FCS_model_P.Internal_C_c[0])*FCS_model_DW.Internal_DSTATE_l - [0] + rtb_roll = (FCS_model_P.Internal_C_c[0])*FCS_model_DW.Internal_DSTATE_l[0] + (FCS_model_P.Internal_C_c[1])*FCS_model_DW.Internal_DSTATE_l[1]; } // Sum: '/Sum1' incorporates: // DiscreteTransferFcn: '/Discrete Transfer Fcn' - rtb_Sum1_jt = rtb_Product5 - numAccum_0; + rtb_Sum1_jt = rtb_roll - numAccum; // DiscreteStateSpace: '/Internal' { - rtb_Product5 = FCS_model_P.Internal_C_g*FCS_model_DW.Internal_DSTATE_gs; - rtb_Product5 += FCS_model_P.Internal_D_h*rtb_Sum1_jt; + rtb_roll = FCS_model_P.Internal_C_g*FCS_model_DW.Internal_DSTATE_gs; + rtb_roll += FCS_model_P.Internal_D_h*rtb_Sum1_jt; } - // Product: '/Product5' incorporates: + // Trigonometry: '/Trigonometric Function' incorporates: // Constant: '/roll equilibrium' // DataTypeConversion: '/Data Type Conversion1' // Gain: '/Changing of Jxx' @@ -342,31 +336,33 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T // Sum: '/Sum2' // Sum: '/Sum3' - rtb_Product5 *= FCS_model_P.zDphi * 7.0; - rtb_Product5 -= arg_gyro[0]; - rtb_Product5 = (FCS_model_P.KDphi * 1.4 * rtb_Product5 * - FCS_model_P.ChangingofJxx_Gain + - FCS_model_P.rollequilibrium_Value) * - FCS_model_P.On1Off1forroll_Gain; - for (iU = 0; iU < 4; iU++) { - // Gain: '/Gain' incorporates: + rtb_roll *= FCS_model_P.zDphi * 7.0; + rtb_roll -= arg_gyro[0]; + rtb_roll = (FCS_model_P.KDphi * 1.4 * rtb_roll * + FCS_model_P.ChangingofJxx_Gain + FCS_model_P.rollequilibrium_Value) + * FCS_model_P.On1Off1forroll_Gain; + for (int32_T iU{0}; iU < 4; iU++) { + real32_T u0; + + // Product: '/Product' incorporates: // Constant: '/TorqueTotalThrustToThrustPerMotor' // DataTypeConversion: '/Data Type Conversion1' - // Product: '/Product' // SignalConversion generated from: '/Product' - arg_motors_refout[iU] = (((FCS_model_P.TorqueTotalThrustToThrustPerMot[iU + - 4] * rtb_DataTypeConversion2_i + - FCS_model_P.TorqueTotalThrustToThrustPerMot[iU] * rtb_On1Off0forthrust) + - FCS_model_P.TorqueTotalThrustToThrustPerMot[iU + 8] * - rtb_DataTypeConversion6_b) + - FCS_model_P.TorqueTotalThrustToThrustPerMot[iU + 12] * - static_cast(rtb_Product5)) * FCS_model_P.Gain_Gain_n; + arg_motors_refout[iU] = 0.0F; + arg_motors_refout[iU] += FCS_model_P.TorqueTotalThrustToThrustPerMot[iU] * + rtb_On1Off0forthrust; + arg_motors_refout[iU] += FCS_model_P.TorqueTotalThrustToThrustPerMot[iU + 4] + * rtb_DataTypeConversion2_i; + arg_motors_refout[iU] += FCS_model_P.TorqueTotalThrustToThrustPerMot[iU + 8] + * rtb_DataTypeConversion6_b; + arg_motors_refout[iU] += FCS_model_P.TorqueTotalThrustToThrustPerMot[iU + 12] + * static_cast(rtb_roll); // Lookup_n-D: '/1-D Lookup Table' arg_motors_refout[iU] = look1_iflf_binlx(arg_motors_refout[iU], FCS_model_P.uDLookupTable_bp01Data, FCS_model_P.uDLookupTable_tableData, - 8U); + 9U); // Saturate: '/Saturation' u0 = arg_motors_refout[iU]; @@ -374,34 +370,35 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T arg_motors_refout[iU] = FCS_model_P.Saturation_UpperSat; } else if (u0 < FCS_model_P.Saturation_LowerSat) { arg_motors_refout[iU] = FCS_model_P.Saturation_LowerSat; + } else { + arg_motors_refout[iU] = u0; } // End of Saturate: '/Saturation' } - // Product: '/Product5' incorporates: + // Trigonometry: '/Trigonometric Function' incorporates: // DataTypeConversion: '/Data Type Conversion' // Gain: '/Gain2' - // Inport: '/accel' - - rtb_Product5 = FCS_model_P.Gain2_Gain * arg_accel[1]; - - // Gain: '/Gain4' incorporates: - // DataTypeConversion: '/Data Type Conversion' // Gain: '/Gain3' // Inport: '/accel' - // Trigonometry: '/Trigonometric Function' - rtb_Gain4 = rt_atan2d_snf(rtb_Product5, FCS_model_P.Gain3_Gain * arg_accel[2]) - * FCS_model_P.Gain4_Gain; + rtb_roll = FCS_model_P.Gain2_Gain * arg_accel[1]; + rtb_roll = rt_atan2d_snf(rtb_roll, FCS_model_P.Gain3_Gain * arg_accel[2]); - // Product: '/Product5' incorporates: - // Trigonometry: '/Trigonometric Function2' + // Trigonometry: '/Trigonometric Function2' + rtb_rollrate = std::sin(rtb_roll); + rtb_Product3 = std::cos(rtb_roll); - rtb_Product5 = std::sin(rtb_Gain4); + // Sum: '/Sum1' incorporates: + // DataTypeConversion: '/Data Type Conversion1' + // Gain: '/Gain' + // Inport: '/gyro' + // Product: '/Product4' + // Product: '/Product5' - // Trigonometry: '/Trigonometric Function2' - rtb_Product4 = std::cos(rtb_Gain4); + rtb_pitchrate = FCS_model_P.Gain_Gain_i * rtb_rollrate * arg_gyro[2] + + rtb_Product3 * arg_gyro[1]; // Product: '/Divide' incorporates: // Constant: '/Constant' @@ -409,33 +406,20 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T // Gain: '/Gain1' // Inport: '/accel' - rtb_pitch = FCS_model_P.Gain1_Gain * arg_accel[0] / FCS_model_P.g; + rtb_TrigonometricFunction4 = FCS_model_P.Gain1_Gain * arg_accel[0] / + FCS_model_P.g; // Trigonometry: '/Trigonometric Function1' - if (rtb_pitch > 1.0) { - rtb_pitch = 1.0; - } else if (rtb_pitch < -1.0) { - rtb_pitch = -1.0; + if (rtb_TrigonometricFunction4 > 1.0) { + rtb_TrigonometricFunction4 = 1.0; + } else if (rtb_TrigonometricFunction4 < -1.0) { + rtb_TrigonometricFunction4 = -1.0; } - rtb_pitch = std::asin(rtb_pitch); + rtb_TrigonometricFunction4 = std::asin(rtb_TrigonometricFunction4); // End of Trigonometry: '/Trigonometric Function1' - // Trigonometry: '/Trigonometric Function4' - rtb_Product3 = std::tan(rtb_pitch); - - // Sum: '/Sum' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // Inport: '/gyro' - // Product: '/Product' - // Product: '/Product1' - // Product: '/Product2' - // Product: '/Product3' - - rtb_Product3 = (rtb_Product5 * rtb_Product3 * arg_gyro[1] + arg_gyro[0]) + - rtb_Product4 * rtb_Product3 * arg_gyro[2]; - // Outport: '/logging_out' incorporates: // DataTypeConversion: '/Data Type Conversion13' // DataTypeConversion: '/Data Type Conversion6' @@ -453,8 +437,8 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T arg_logging_refout[0] = static_cast (FCS_model_DW.Memory_PreviousInput); - arg_logging_refout[1] = static_cast(numAccum); - arg_logging_refout[2] = static_cast(numAccum_0); + arg_logging_refout[1] = static_cast(Sum); + arg_logging_refout[2] = static_cast(numAccum); arg_logging_refout[9] = *arg_yaw_opticalfow; arg_logging_refout[3] = arg_accel[0]; arg_logging_refout[6] = arg_gyro[0]; @@ -472,16 +456,16 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T arg_logging_refout[17] = arg_motors_refout[1]; arg_logging_refout[18] = arg_motors_refout[2]; arg_logging_refout[19] = arg_motors_refout[3]; - arg_logging_refout[20] = static_cast(rtb_Product3); - arg_logging_refout[21] = static_cast(rtb_Gain4); + arg_logging_refout[20] = static_cast(rtb_pitchrate); + arg_logging_refout[21] = static_cast(rtb_TrigonometricFunction4); - // Product: '/Product5' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // Gain: '/Gain' - // Inport: '/gyro' + // Sum: '/Sum' incorporates: + // Gain: '/Gain' + + Sum = FCS_model_P.w_c_pitch * rtb_TrigonometricFunction4 + rtb_pitchrate; - rtb_Product5 *= FCS_model_P.Gain_Gain; - rtb_Product5 *= arg_gyro[2]; + // Trigonometry: '/Trigonometric Function4' + rtb_TrigonometricFunction4 = std::tan(rtb_TrigonometricFunction4); // DataTypeConversion: '/Data Type Conversion6' incorporates: // Inport: '/pos_ref' @@ -513,26 +497,27 @@ void FCS_model::step(real32_T arg_accel[3], real32_T arg_gyro[3], real32_T rtb_DataTypeConversion7[2] = arg_orient_ref[2]; - // Update for DiscreteTransferFcn: '/Discrete Transfer Fcn' incorporates: - // DataTypeConversion: '/Data Type Conversion1' - // Gain: '/Gain' - // Inport: '/gyro' - // Product: '/Product4' - // Sum: '/Sum' - // Sum: '/Sum1' - - FCS_model_DW.DiscreteTransferFcn_states = (((rtb_Product4 * arg_gyro[1] + - rtb_Product5) + FCS_model_P.w_c_pitch * rtb_pitch) - + // Update for DiscreteTransferFcn: '/Discrete Transfer Fcn' + FCS_model_DW.DiscreteTransferFcn_states = (Sum - FCS_model_P.DiscreteTransferFcn_DenCoef_n[1] * FCS_model_DW.DiscreteTransferFcn_states) / FCS_model_P.DiscreteTransferFcn_DenCoef_n[0]; // Update for DiscreteTransferFcn: '/Discrete Transfer Fcn' incorporates: + // DataTypeConversion: '/Data Type Conversion1' // Gain: '/Gain' + // Inport: '/gyro' + // Product: '/Product' + // Product: '/Product1' + // Product: '/Product2' + // Product: '/Product3' // Sum: '/Sum' + // Sum: '/Sum' - FCS_model_DW.DiscreteTransferFcn_states_e = ((FCS_model_P.w_c_roll * rtb_Gain4 - + rtb_Product3) - FCS_model_P.DiscreteTransferFcn_DenCoef_o[1] * + FCS_model_DW.DiscreteTransferFcn_states_e = ((((rtb_rollrate * + rtb_TrigonometricFunction4 * arg_gyro[1] + arg_gyro[0]) + rtb_Product3 * + rtb_TrigonometricFunction4 * arg_gyro[2]) + FCS_model_P.w_c_roll * rtb_roll) + - FCS_model_P.DiscreteTransferFcn_DenCoef_o[1] * FCS_model_DW.DiscreteTransferFcn_states_e) / FCS_model_P.DiscreteTransferFcn_DenCoef_o[0]; @@ -665,8 +650,10 @@ FCS_model::FCS_model() : } // Destructor -// Currently there is no destructor body generated. -FCS_model::~FCS_model() = default; +FCS_model::~FCS_model() +{ + // Currently there is no destructor body generated. +} // Real-Time Model get method FCS_model::RT_MODEL_FCS_model_T * FCS_model::getRTM() diff --git a/libraries/AC_Simulink/FCS_model.h b/libraries/AC_Simulink/FCS_model.h index a8c5a5ba1816b9..0a36e6c831ce1b 100755 --- a/libraries/AC_Simulink/FCS_model.h +++ b/libraries/AC_Simulink/FCS_model.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -25,19 +25,19 @@ extern "C" { -#include "rt_nonfinite.h" +#include "rtGetInf.h" } +#include "rt_defines.h" + extern "C" { -#include "rtGetNaN.h" +#include "rt_nonfinite.h" } -#include "rt_defines.h" - // Macros for accessing real-time model data structure #ifndef rtmGetErrorStatus #define rtmGetErrorStatus(rtm) ((rtm)->errorStatus) @@ -157,6 +157,9 @@ class FCS_model final real_T Internal_InitialCondition; // Expression: 0 // Referenced by: '/Internal' + real_T Gain_Gain; // Expression: 0 + // Referenced by: '/Gain' + real_T yawequilibrium_Value; // Expression: 0 // Referenced by: '/yaw equilibrium' @@ -211,7 +214,7 @@ class FCS_model final real_T ChangingofJyy_Gain; // Expression: 0.6 // Referenced by: '/Changing of Jyy' - real_T On1Off2forpitch_Gain; // Expression: 1 + real_T On1Off2forpitch_Gain; // Expression: 0 // Referenced by: '/On=1//Off=2 for pitch' real_T rollequilibrium_Value; // Expression: 0 @@ -256,15 +259,12 @@ class FCS_model final real_T Gain3_Gain; // Expression: -1 // Referenced by: '/Gain3' - real_T Gain4_Gain; // Expression: -1 - // Referenced by: '/Gain4' + real_T Gain_Gain_i; // Expression: -1 + // Referenced by: '/Gain' real_T Gain1_Gain; // Expression: -1 // Referenced by: '/Gain1' - real_T Gain_Gain; // Expression: -1 - // Referenced by: '/Gain' - real32_T TorqueTotalThrustToThrustPerMot[16];// Expression: Controller.Q2Ts // Referenced by: '/TorqueTotalThrustToThrustPerMotor' @@ -279,14 +279,11 @@ class FCS_model final real32_T On1Off0forthrust_Gain; // Computed Parameter: On1Off0forthrust_Gain // Referenced by: '/On=1//Off=0 for thrust' - real32_T Gain_Gain_n; // Computed Parameter: Gain_Gain_n - // Referenced by: '/Gain' - - real32_T uDLookupTable_tableData[9]; + real32_T uDLookupTable_tableData[10]; // Computed Parameter: uDLookupTable_tableData // Referenced by: '/1-D Lookup Table' - real32_T uDLookupTable_bp01Data[9]; + real32_T uDLookupTable_bp01Data[10]; // Computed Parameter: uDLookupTable_bp01Data // Referenced by: '/1-D Lookup Table' diff --git a/libraries/AC_Simulink/FCS_model_data.cpp b/libraries/AC_Simulink/FCS_model_data.cpp index e7cadfa419111b..d7c38c2e25f974 100755 --- a/libraries/AC_Simulink/FCS_model_data.cpp +++ b/libraries/AC_Simulink/FCS_model_data.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -38,7 +38,7 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ }, { - 0.068, + 0.0769, { 6.86e-5, 0.0, 0.0, 0.0, 9.2e-5, 0.0, 0.0, 0.0, 0.0001366 }, 0.0624, @@ -117,12 +117,12 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ // Variable: w_c_pitch // Referenced by: '/Gain' - 3.0, + 0.03, // Variable: w_c_roll // Referenced by: '/Gain' - 3.0, + 0.3, // Variable: w_c_yaw // Referenced by: '/Gain' @@ -177,12 +177,12 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ // Expression: [num_pitch_disc] // Referenced by: '/Discrete Transfer Fcn' - { 0.0, 0.0098514888171639427 }, + { 0.0, 0.009998500149988751 }, // Expression: [den_pitch_disc] // Referenced by: '/Discrete Transfer Fcn' - { 1.0, -0.97044553354850815 }, + { 1.0, -0.99970004499550036 }, // Expression: 0 // Referenced by: '/Discrete Transfer Fcn' @@ -192,12 +192,12 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ // Expression: [num_roll_disc] // Referenced by: '/Discrete Transfer Fcn' - { 0.0, 0.0098514888171639427 }, + { 0.0, 0.0099850149887567487 }, // Expression: [den_roll_disc] // Referenced by: '/Discrete Transfer Fcn' - { 1.0, -0.97044553354850815 }, + { 1.0, -0.997004495503373 }, // Expression: 0 // Referenced by: '/Discrete Transfer Fcn' @@ -224,6 +224,11 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ 0.0, + // Expression: 0 + // Referenced by: '/Gain' + + 0.0, + // Expression: 0 // Referenced by: '/yaw equilibrium' @@ -314,10 +319,10 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ 0.6, - // Expression: 1 + // Expression: 0 // Referenced by: '/On=1//Off=2 for pitch' - 1.0, + 0.0, // Expression: 0 // Referenced by: '/roll equilibrium' @@ -390,7 +395,7 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ -1.0, // Expression: -1 - // Referenced by: '/Gain4' + // Referenced by: '/Gain' -1.0, @@ -399,11 +404,6 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ -1.0, - // Expression: -1 - // Referenced by: '/Gain' - - -1.0, - // Expression: Controller.Q2Ts // Referenced by: '/TorqueTotalThrustToThrustPerMotor' @@ -424,22 +424,17 @@ FCS_model::P_FCS_model_T FCS_model::FCS_model_P{ // Computed Parameter: On1Off0forthrust_Gain // Referenced by: '/On=1//Off=0 for thrust' - 0.0F, - - // Computed Parameter: Gain_Gain_n - // Referenced by: '/Gain' - - 101.936798F, + 1.0F, // Computed Parameter: uDLookupTable_tableData // Referenced by: '/1-D Lookup Table' - { 0.0F, 0.1F, 0.2F, 0.3F, 0.4F, 0.5F, 0.6F, 0.7F, 0.8F }, + { 0.1F, 0.2F, 0.3F, 0.4F, 0.5F, 0.6F, 0.7F, 0.8F, 0.9F, 1.0F }, // Computed Parameter: uDLookupTable_bp01Data // Referenced by: '/1-D Lookup Table' - { 0.0F, 36.6F, 50.3F, 62.5F, 74.6F, 83.5F, 93.2F, 102.0F, 112.7F }, + { 0.0F, 0.005F, 0.01F, 0.02F, 0.03F, 0.04F, 0.05F, 0.06F, 0.07F, 0.08F }, // Computed Parameter: Saturation_UpperSat // Referenced by: '/Saturation' diff --git a/libraries/AC_Simulink/FCS_model_private.h b/libraries/AC_Simulink/FCS_model_private.h index 27dd39b1816fd1..de0c8a2c4e7a35 100755 --- a/libraries/AC_Simulink/FCS_model_private.h +++ b/libraries/AC_Simulink/FCS_model_private.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/libraries/AC_Simulink/FCS_model_types.h b/libraries/AC_Simulink/FCS_model_types.h index 4c0ae79f250ea5..501cb582e36d89 100755 --- a/libraries/AC_Simulink/FCS_model_types.h +++ b/libraries/AC_Simulink/FCS_model_types.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -124,6 +124,30 @@ struct struct_OSJpyIZcrpXqReVWwh9iuG struct_p3FXZIgqtjF2uqDpmYjb6C Motor; }; +#endif + +#ifndef DEFINED_TYPEDEF_FOR_struct_sfjjhK32Dt7MmV2O18UsO_ +#define DEFINED_TYPEDEF_FOR_struct_sfjjhK32Dt7MmV2O18UsO_ + +struct struct_sfjjhK32Dt7MmV2O18UsO +{ + real_T Ts2Q[16]; + real_T Q2Ts[16]; + real_T totalThrustMaxRelative; + real_T motorsThrustPerMotorMax; +}; + +#endif + +#ifndef DEFINED_TYPEDEF_FOR_struct_dNTl6UkWY4GnjER3gqbKpG_ +#define DEFINED_TYPEDEF_FOR_struct_dNTl6UkWY4GnjER3gqbKpG_ + +struct struct_dNTl6UkWY4GnjER3gqbKpG +{ + real_T pwm_percentage[10]; + real_T thrust_N[10]; +}; + #endif #endif // RTW_HEADER_FCS_model_types_h_ diff --git a/libraries/AC_Simulink/buildInfo.mat b/libraries/AC_Simulink/buildInfo.mat index ef5d95d519bdb6..7728becb500823 100755 Binary files a/libraries/AC_Simulink/buildInfo.mat and b/libraries/AC_Simulink/buildInfo.mat differ diff --git a/libraries/AC_Simulink/rtGetInf.cpp b/libraries/AC_Simulink/rtGetInf.cpp index db87d971b279bf..9e9549042d9586 100755 --- a/libraries/AC_Simulink/rtGetInf.cpp +++ b/libraries/AC_Simulink/rtGetInf.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/libraries/AC_Simulink/rtGetInf.h b/libraries/AC_Simulink/rtGetInf.h index 250a7205e595d0..1b8b4bc1ca63fa 100755 --- a/libraries/AC_Simulink/rtGetInf.h +++ b/libraries/AC_Simulink/rtGetInf.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -18,14 +18,6 @@ // #ifndef RTW_HEADER_rtGetInf_h_ #define RTW_HEADER_rtGetInf_h_ - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - #include "rtwtypes.h" #ifdef __cplusplus diff --git a/libraries/AC_Simulink/rtGetNaN.cpp b/libraries/AC_Simulink/rtGetNaN.cpp index 473c576b3949a2..2beca995b794e9 100755 --- a/libraries/AC_Simulink/rtGetNaN.cpp +++ b/libraries/AC_Simulink/rtGetNaN.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/libraries/AC_Simulink/rtGetNaN.h b/libraries/AC_Simulink/rtGetNaN.h index 350b0e1c240816..93d87a58c71ef5 100755 --- a/libraries/AC_Simulink/rtGetNaN.h +++ b/libraries/AC_Simulink/rtGetNaN.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M @@ -18,14 +18,6 @@ // #ifndef RTW_HEADER_rtGetNaN_h_ #define RTW_HEADER_rtGetNaN_h_ - -extern "C" -{ - -#include "rt_nonfinite.h" - -} - #include "rtwtypes.h" #ifdef __cplusplus diff --git a/libraries/AC_Simulink/rt_defines.h b/libraries/AC_Simulink/rt_defines.h index 3a6dd259225739..c95ef1de8e497b 100755 --- a/libraries/AC_Simulink/rt_defines.h +++ b/libraries/AC_Simulink/rt_defines.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/libraries/AC_Simulink/rt_nonfinite.cpp b/libraries/AC_Simulink/rt_nonfinite.cpp index eb5283865cb9bb..034ccad473bc16 100755 --- a/libraries/AC_Simulink/rt_nonfinite.cpp +++ b/libraries/AC_Simulink/rt_nonfinite.cpp @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/libraries/AC_Simulink/rt_nonfinite.h b/libraries/AC_Simulink/rt_nonfinite.h index f84f91903e7818..98b72661ca9a90 100755 --- a/libraries/AC_Simulink/rt_nonfinite.h +++ b/libraries/AC_Simulink/rt_nonfinite.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/libraries/AC_Simulink/rtwtypes.h b/libraries/AC_Simulink/rtwtypes.h index b0e7753b1ab55c..32fcc0fb526abe 100755 --- a/libraries/AC_Simulink/rtwtypes.h +++ b/libraries/AC_Simulink/rtwtypes.h @@ -7,9 +7,9 @@ // // Code generated for Simulink model 'FCS_model'. // -// Model version : 5.21 -// Simulink Coder version : 23.2 (R2023b) 01-Aug-2023 -// C/C++ source code generated on : Thu Apr 18 15:33:46 2024 +// Model version : 5.19 +// Simulink Coder version : 9.8 (R2022b) 13-May-2022 +// C/C++ source code generated on : Sun Apr 28 12:32:03 2024 // // Target selection: ert.tlc // Embedded hardware selection: ARM Compatible->ARM Cortex-M diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0fbd891253e192..479400fede3b25 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -106,6 +106,10 @@ extern AP_IOMCU iomcu; #include +#if MODE_LAB_ENABLED +#include +#endif + extern const AP_HAL::HAL& hal; struct GCS_MAVLINK::LastRadioStatus GCS_MAVLINK::last_radio_status; @@ -5440,25 +5444,14 @@ void GCS_MAVLINK::send_extended_sys_state() const void GCS_MAVLINK::send_lab_to_dashboard() const { - // gcs().send_text(MAV_SEVERITY_INFO, "GCS_MAVLINK::send_lab_to_dashboard"); - // float logging1 = 69; - // float logging2 = 69; - // float logging3 = 69; - // float logging4 = 69; - // float logging5 = 69; - // float logging6 = 69; - // float logging7 = 69; - // float logging8 = 69; - // float logging9 = 69; - // mavlink_msg_lab_to_dashboard_send(chan, logging1, - // logging2, - // logging3, - // logging4, - // logging5, - // logging6, - // logging7, - // logging8, - // logging9); + AC_Simulink simulink_Controller; + + float *logging_data = simulink_Controller.get_logging_data(); + + // hal.console->printf("logging_data[0]: (%f)", logging_data[0]); + // gcs().send_text(MAV_SEVERITY_INFO, "GCS_MAVLINK::send_lab_to_dashboard, example: %f",logging_data[0]); + + mavlindk_msg_lab_to_dashboard_send(chan, logging_data); } void GCS_MAVLINK::send_attitude() const diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index 8960a881ab45cb..f8b758fdf8fc88 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -98,3 +98,9 @@ #ifndef AP_MAVLINK_COMMAND_LONG_ENABLED #define AP_MAVLINK_COMMAND_LONG_ENABLED 1 #endif + +////////////////////////////////////////////////////////////////////////////// +// Lab - fly vehicle in custom advanced control lab mode +#ifndef MODE_LAB_ENABLED +# define MODE_LAB_ENABLED 1 +#endif \ No newline at end of file