diff options
Diffstat (limited to 'src/modules')
140 files changed, 8518 insertions, 21803 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 191d20f30..33879892e 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -325,7 +325,7 @@ void KalmanNav::updatePublications() _pos.vx = vN; _pos.vy = vE; _pos.vz = vD; - _pos.hdg = psi; + _pos.yaw = psi; // attitude publication _att.timestamp = _pubTimeStamp; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 9e533ccdf..a70a14fe4 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -57,7 +57,7 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> @@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode*/ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } @@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); if (!initialized) { - - gyro_offsets[0] += raw.gyro_rad_s[0]; - gyro_offsets[1] += raw.gyro_rad_s[1]; - gyro_offsets[2] += raw.gyro_rad_s[2]; - offset_count++; - - if (hrt_absolute_time() - start_time > 3000000LL) { - initialized = true; - gyro_offsets[0] /= offset_count; - gyro_offsets[1] /= offset_count; - gyro_offsets[2] /= offset_count; - } + // XXX disabling init for now + initialized = true; + + // gyro_offsets[0] += raw.gyro_rad_s[0]; + // gyro_offsets[1] += raw.gyro_rad_s[1]; + // gyro_offsets[2] += raw.gyro_rad_s[2]; + // offset_count++; + + // if (hrt_absolute_time() - start_time > 3000000LL) { + // initialized = true; + // gyro_offsets[0] /= offset_count; + // gyro_offsets[1] /= offset_count; + // gyro_offsets[2] /= offset_count; + // } } else { diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 107c2dfb1..236052b56 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -35,7 +35,7 @@ #include <uORB/topics/debug_key_value.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <drivers/drv_hrt.h> @@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode */ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp index fbb73d997..ed6707f04 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file accelerometer_calibration.c + * @file accelerometer_calibration.cpp * * Implementation of accelerometer calibration. * @@ -104,32 +104,44 @@ */ #include "accelerometer_calibration.h" +#include "commander_helper.h" +#include <unistd.h> +#include <stdio.h> #include <poll.h> +#include <fcntl.h> +#include <sys/prctl.h> +#include <math.h> +#include <string.h> #include <drivers/drv_hrt.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_accel.h> -#include <systemlib/conversions.h> +#include <geo/geo.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> #include <mavlink/mavlink_log.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { +int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); - /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_scale[3]; - int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); + int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale); if (res == OK) { /* measurements complete successfully, set parameters */ @@ -165,24 +177,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + return OK; } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_error(); - sleep(2); + return ERROR; } /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); } -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { +int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { const int samples_num = 2500; float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; @@ -207,38 +212,52 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + unsigned done_count = 0; + while (true) { bool done = true; - char str[80]; - int str_ptr; - str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; + for (int i = 0; i < 6; i++) { if (!data_collected[i]) { - str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; } } + + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", + (!data_collected[0]) ? "x+ " : "", + (!data_collected[1]) ? "x- " : "", + (!data_collected[2]) ? "y+ " : "", + (!data_collected[3]) ? "y- " : "", + (!data_collected[4]) ? "z+ " : "", + (!data_collected[5]) ? "z- " : ""); + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + if (done) break; - mavlink_log_info(mavlink_fd, str); int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) return ERROR; if (data_collected[orient]) { - sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]); continue; } - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient], + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); + data_collected[orient] = true; - tune_confirm(); + tune_neutral(); } close(sensor_combined_sub); @@ -246,7 +265,7 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float float accel_T[3][3]; int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); return ERROR; } @@ -279,8 +298,10 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ - int64_t still_time = 2000000; - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + hrt_abstime still_time = 2000000; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ @@ -316,12 +337,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "still..."); + mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); t_still = t; t_timeout = t + timeout; } else { /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { + if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } @@ -331,7 +352,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving..."); + mavlink_log_info(mavlink_fd, "detected motion, please hold still..."); t_still = 0; } } @@ -343,34 +364,34 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: failed reading accel"); + mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); return -1; } } - if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 1; // [ -g, 0, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); @@ -382,7 +403,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { * Read specified number of accelerometer samples, calculate average and dispersion. */ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sensor_combined_sub; + fds[0].events = POLLIN; int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; @@ -416,7 +439,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) + if (det == 0.0f) return ERROR; // Singular matrix dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index f93a867ba..1cf9c0977 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -44,8 +44,7 @@ #define ACCELEROMETER_CALIBRATION_H_ #include <stdint.h> -#include <uORB/topics/vehicle_status.h> -void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +int do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp new file mode 100644 index 000000000..e414e5f70 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.cpp @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file airspeed_calibration.cpp + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include <stdio.h> +#include <poll.h> +#include <math.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/differential_pressure.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return ERROR; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + return ERROR; + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + return OK; + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + return ERROR; + } +} diff --git a/src/modules/mathlib/math/test/test.hpp b/src/modules/commander/airspeed_calibration.h index 2027bb827..71c701fc2 100644 --- a/src/modules/mathlib/math/test/test.hpp +++ b/src/modules/commander/airspeed_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,19 +32,15 @@ ****************************************************************************/ /** - * @file test.hpp - * - * Controller library code + * @file gyro_calibration.h + * Airspeed sensor calibration routine */ -#pragma once +#ifndef AIRSPEED_CALIBRATION_H_ +#define AIRSPEED_CALIBRATION_H_ + +#include <stdint.h> -//#include <assert.h> -//#include <time.h> -//#include <stdlib.h> +int do_airspeed_calibration(int mavlink_fd); -bool equal(float a, float b, float eps = 1e-5); -void float2SigExp( - const float &num, - float &sig, - int &exp); +#endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/modules/commander/baro_calibration.cpp index f19db15ec..3123c4087 100644 --- a/src/modules/mathlib/math/Matrix.hpp +++ b/src/modules/commander/baro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,30 +32,29 @@ ****************************************************************************/ /** - * @file Matrix.h - * - * matrix code + * @file baro_calibration.cpp + * Barometer calibration routine */ -#pragma once +#include "baro_calibration.h" -#include <nuttx/config.h> +#include <poll.h> +#include <math.h> +#include <fcntl.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_baro.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Matrix.hpp" -#else -#include "generic/Matrix.hpp" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR #endif +static const int ERROR = -1; -namespace math +int do_baro_calibration(int mavlink_fd) { -class Matrix; -int matrixTest(); -int matrixAddTest(); -int matrixSubTest(); -int matrixMultTest(); -int matrixInvTest(); -int matrixDivTest(); -int matrixArmTest(); -bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f); -} // namespace math + // TODO implement this + return ERROR; +} diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/modules/commander/baro_calibration.h index 21661622a..bc42bc6cf 100644 --- a/src/modules/mathlib/math/generic/Matrix.cpp +++ b/src/modules/commander/baro_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,15 @@ ****************************************************************************/ /** - * @file Matrix.cpp - * - * matrix code + * @file mag_calibration.h + * Barometer calibration routine */ -#include "Matrix.hpp" +#ifndef BARO_CALIBRATION_H_ +#define BARO_CALIBRATION_H_ + +#include <stdint.h> + +int do_baro_calibration(int mavlink_fd); + +#endif /* BARO_CALIBRATION_H_ */ diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp index a26938637..be38ea104 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file calibration_routines.c + * @file calibration_routines.cpp * Calibration routines implementations. * * @author Lorenz Meier <lm@inf.ethz.ch> @@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c deleted file mode 100644 index e9d1f3954..000000000 --- a/src/modules/commander/commander.c +++ /dev/null @@ -1,2097 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file commander.c - * Main system state machine implementation. - * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * - */ - -#include "commander.h" - -#include <nuttx/config.h> -#include <pthread.h> -#include <stdio.h> -#include <stdlib.h> -#include <stdbool.h> -#include <string.h> -#include <unistd.h> -#include <fcntl.h> -#include <errno.h> -#include <debug.h> -#include <sys/prctl.h> -#include <string.h> -#include <drivers/drv_led.h> -#include <drivers/drv_hrt.h> -#include <drivers/drv_tone_alarm.h> -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" -#include <math.h> -#include <poll.h> -#include <uORB/uORB.h> -#include <uORB/topics/sensor_combined.h> -#include <uORB/topics/battery_status.h> -#include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/offboard_control_setpoint.h> -#include <uORB/topics/home_position.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_local_position.h> -#include <uORB/topics/vehicle_gps_position.h> -#include <uORB/topics/vehicle_command.h> -#include <uORB/topics/subsystem_info.h> -#include <uORB/topics/actuator_controls.h> -#include <uORB/topics/parameter_update.h> -#include <uORB/topics/differential_pressure.h> -#include <mavlink/mavlink_log.h> - -#include <systemlib/param/param.h> -#include <systemlib/systemlib.h> -#include <systemlib/err.h> - -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ -#include <drivers/drv_accel.h> -#include <drivers/drv_gyro.h> -#include <drivers/drv_mag.h> -#include <drivers/drv_baro.h> - -#include "calibration_routines.h" -#include "accelerometer_calibration.h" - -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - -#include <systemlib/cpuload.h> -extern struct system_load_s system_load; - -/* Decouple update interval and hysteris counters, all depends on intervals */ -#define COMMANDER_MONITORING_INTERVAL 50000 -#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) -#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f -#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 -#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -/* File descriptors */ -static int leds; -static int buzzer; -static int mavlink_fd; -static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ -static orb_advert_t stat_pub; - -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -static unsigned int failsafe_lowlevel_timeout_ms; - -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ -static int daemon_task; /**< Handle of daemon task / thread */ - -/* pthread loops */ -static void *orb_receive_loop(void *arg); - -__EXPORT int commander_main(int argc, char *argv[]); - -/** - * Mainloop of commander. - */ -int commander_thread_main(int argc, char *argv[]); - -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); - -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); - - - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -/** - * Sort calibration values. - * - * Sorts the calibration values with bubble sort. - * - * @param a The array to sort - * @param n The number of entries in the array - */ -// static void cal_bsort(float a[], int n); - -static int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -static void buzzer_deinit() -{ - close(buzzer); -} - - -static int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -static void led_deinit() -{ - close(leds); -} - -static int led_toggle(int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -static int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -static int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; - -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) -{ - - /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } - - /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } - - /* Trigger Tetris on being bored */ - - return 0; -} - -void tune_confirm(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_error(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) -{ - if (current_status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) -{ - - /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - tune_confirm(); - sleep(2); - tune_confirm(); - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_confirm(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(sub_mag); -} - -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - int errcount = 0; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - errcount++; - } - - if (errcount > 1000) { - /* any persisting poll error is a reason to abort */ - mavlink_log_info(mavlink_fd, "permanent gyro error, aborted."); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it still"); - /* set to accel calibration mode */ - status->flag_preflight_airspeed_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - /* exit airspeed calibration mode */ - status->flag_preflight_airspeed_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(diff_pres_sub); -} - - - -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) -{ - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - - /* announce command handling */ - tune_confirm(); - - - /* supported command handling start */ - - /* request to set different system mode */ - switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - break; - - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - /* request to disarm */ - - } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } - } - break; - - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - } - } - } - break; - -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status, mavlink_fd); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { - mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); - - } else { - - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - - if (((int)(cmd->param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - } - break; - - default: { - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); - } - break; - } - - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); - - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); - } - - /* send any requested ACKs */ - if (cmd->confirmation > 0) { - /* send acknowledge command */ - // XXX TODO - } - -} - -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander orb rcv", getpid()); - - /* Subscribe to command topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - - struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - vstatus->onboard_control_sensors_present |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - vstatus->onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - vstatus->onboard_control_sensors_health |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_health &= ~info.subsystem_type; - } - } - } - - close(subsys_sub); - - return NULL; -} - -/* - * Provides a coarse estimate of remaining battery power. - * - * The estimate is very basic and based on decharging voltage curves. - * - * @return the estimated remaining capacity in 0..1 - */ -float battery_remaining_estimate_voltage(float voltage); - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); - exit(1); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - -int commander_thread_main(int argc, char *argv[]) -{ - /* not yet initialized */ - commander_initialized = false; - bool home_position_set = false; - - /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - - param_t _param_sys_type = param_find("MAV_TYPE"); - param_t _param_system_id = param_find("MAV_SYS_ID"); - param_t _param_component_id = param_find("MAV_COMP_ID"); - - /* welcome user */ - warnx("I am in command now!\n"); - - /* pthreads for command and subsystem info handling */ - // pthread_t command_handling_thread; - pthread_t subsystem_info_thread; - - /* initialize */ - if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); - } - - if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); - } - - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); - } - - /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; - current_status.flag_system_armed = false; - /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; - /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; - /* allow manual override initially */ - current_status.flag_external_manual_override_ok = true; - /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; - /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - - /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); - /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); - - /* home position */ - orb_advert_t home_pub = -1; - struct home_position_s home; - memset(&home, 0, sizeof(home)); - - if (stat_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); - warnx("exiting."); - exit(ERROR); - } - - mavlink_log_info(mavlink_fd, "system is running"); - - /* create pthreads */ - pthread_attr_t subsystem_info_attr; - pthread_attr_init(&subsystem_info_attr); - pthread_attr_setstacksize(&subsystem_info_attr, 2048); - pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); - - /* Start monitoring loop */ - uint16_t counter = 0; - uint8_t flight_env; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - bool critical_battery_voltage_actions_done = false; - uint8_t low_voltage_counter = 0; - uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; - float bat_remain = 1.0f; - - uint16_t stick_off_counter = 0; - uint16_t stick_on_counter = 0; - - /* Subscribe to manual control data */ - int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp_man; - memset(&sp_man, 0, sizeof(sp_man)); - - /* Subscribe to offboard control data */ - int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; - memset(&sp_offboard, 0, sizeof(sp_offboard)); - - int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - struct vehicle_global_position_s global_position; - memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; - - int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; - - /* - * The home position is set based on GPS only, to prevent a dependency between - * position estimator and commander. RAW GPS is more than good enough for a - * non-flying vehicle. - */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - struct vehicle_gps_position_s gps_position; - memset(&gps_position, 0, sizeof(gps_position)); - - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s sensors; - memset(&sensors, 0, sizeof(sensors)); - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; - - /* Subscribe to command topic */ - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - - /* Subscribe to parameters changed topic */ - int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); - struct parameter_update_s param_changed; - memset(¶m_changed, 0, sizeof(param_changed)); - - /* subscribe to battery topic */ - int battery_sub = orb_subscribe(ORB_ID(battery_status)); - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - battery.voltage_v = 0.0f; - - // uint8_t vehicle_state_previous = current_status.state_machine; - float voltage_previous = 0.0f; - - uint64_t last_idle_time = 0; - - /* now initialized */ - commander_initialized = true; - thread_running = true; - - uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; - - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; - bool state_changed = true; - bool param_init_forced = true; - - while (!thread_should_exit) { - - /* Get current values */ - bool new_data; - orb_check(sp_man_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); - } - - orb_check(sp_offboard_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); - } - - orb_check(sensor_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); - } - - orb_check(diff_pres_sub, &new_data); - - if (new_data) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; - } - - orb_check(cmd_sub, &new_data); - - if (new_data) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); - } - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - - /* update parameters */ - if (!current_status.flag_system_armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - - } - } - - /* update global position estimate */ - orb_check(global_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; - } - - /* update local position estimate */ - orb_check(local_position_sub, &new_data); - - if (new_data) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; - } - - /* update battery status */ - orb_check(battery_sub, &new_data); - if (new_data) { - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } - } - - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - /* toggle GPS led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_BLUE); - - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle arming (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); - - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); - - // } else { - // led_off(LED_AMBER); - // } - } - - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - - last_idle_time = system_load.tasks[0].total_runtime; - } - } - - // // XXX Export patterns and threshold to parameters - /* Trigger audio event for low battery */ - if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { - /* For less than 10%, start be really annoying at 5 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 3); - - } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { - /* For less than 20%, start be slightly annoying at 1 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - } - - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - - if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { - low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - } - - low_voltage_counter++; - } - - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ - else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { - if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { - critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); - } - - critical_voltage_counter++; - - } else { - low_voltage_counter = 0; - critical_voltage_counter = 0; - } - - /* End battery voltage check */ - - - /* - * Check for valid position information. - * - * If the system has a valid position source from an onboard - * position estimator, it is safe to operate it autonomously. - * The flag_vector_flight_mode_ok flag indicates that a minimum - * set of position measurements is available. - */ - - /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; - bool airspeed_valid = current_status.flag_airspeed_valid; - - /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.flag_global_position_valid = false; - } - - if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; - // XXX check for controller status and home position as well - - } else { - current_status.flag_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000) { - current_status.flag_airspeed_valid = true; - - } else { - current_status.flag_airspeed_valid = false; - } - - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; - - } else { - current_status.flag_vector_flight_mode_ok = false; - } - - /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid || - airspeed_valid != current_status.flag_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ - - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } - - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { - - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - - /* check if gps fix is ok */ - // XXX magic number - float hdop_threshold_m = 4.0f; - float vdop_threshold_m = 8.0f; - - /* - * If horizontal dilution of precision (hdop / eph) - * and vertical diluation of precision (vdop / epv) - * are below a certain threshold (e.g. 4 m), AND - * home position is not yet set AND the last GPS - * GPS measurement is not older than two seconds AND - * the system is currently not armed, set home - * position to the current position. - */ - - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { - warnx("setting home position"); - - /* copy position data to uORB home message, store it locally as well */ - home.lat = gps_position.lat; - home.lon = gps_position.lon; - home.alt = gps_position.alt; - - home.eph_m = gps_position.eph_m; - home.epv_m = gps_position.epv_m; - - home.s_variance_m_s = gps_position.s_variance_m_s; - home.p_variance_m = gps_position.p_variance_m; - - /* announce new home position */ - if (home_pub > 0) { - orb_publish(ORB_ID(home_position), home_pub, &home); - } else { - home_pub = orb_advertise(ORB_ID(home_position), &home); - } - - /* mark home position as set */ - home_position_set = true; - tune_confirm(); - } - } - - /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); - - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { - - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { - - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } - - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { - - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } - - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); - - /* - * Check if manual stability control modes have to be switched - */ - if (!isfinite(sp_man.manual_sas_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; - - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set default */ - /* this MUST be mapped to extremal position to switch easy in case of emergency */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; - - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; - - } else { - /* center stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; - } - - if (current_status.manual_sas_mode != manual_sas_mode) { - /* publish SAS mode changes immediately */ - manual_sas_mode = current_status.manual_sas_mode; - state_changed = true; - } - - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) && - current_status.flag_control_manual_enabled && - current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && - sp_man.yaw < -STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; - - } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* check if left stick is in lower right position --> arm */ - if (current_status.flag_control_manual_enabled && - current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && - sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - stick_on_counter = 0; - - } else { - stick_on_counter++; - stick_off_counter = 0; - } - } - - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; - - } else { - static uint64_t last_print_time = 0; - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; - state_changed = true; - } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } - } - } - - - - - /* End mode switch */ - - /* END RC state check */ - - - /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; - - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } - - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } - - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_confirm(); - - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_confirm(); - } - } - - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; - - /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - - } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); - } - - } else { - static uint64_t last_print_time = 0; - - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); - - /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; - state_changed = true; - } - } - } - } - - - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - - - /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); - state_changed = false; - } - - /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; - - fflush(stdout); - counter++; - usleep(COMMANDER_MONITORING_INTERVAL); - } - - /* wait for threads to complete */ - // pthread_join(command_handling_thread, NULL); - pthread_join(subsystem_info_thread, NULL); - - /* close fds */ - led_deinit(); - buzzer_deinit(); - close(sp_man_sub); - close(sp_offboard_sub); - close(global_position_sub); - close(sensor_sub); - close(cmd_sub); - - warnx("exiting..\n"); - fflush(stdout); - - thread_running = false; - - return 0; -} diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp new file mode 100644 index 000000000..45e0307e6 --- /dev/null +++ b/src/modules/commander/commander.cpp @@ -0,0 +1,1768 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * Lorenz Meier <lm@inf.ethz.ch> + * Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file commander.cpp + * Main system state machine implementation. + * + */ + +#include <nuttx/config.h> +#include <pthread.h> +#include <stdio.h> +#include <stdlib.h> +#include <stdbool.h> +#include <string.h> +#include <unistd.h> +#include <fcntl.h> +#include <errno.h> +#include <debug.h> +#include <sys/prctl.h> +#include <sys/stat.h> +#include <string.h> +#include <math.h> +#include <poll.h> + +#include <uORB/uORB.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/battery_status.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/offboard_control_setpoint.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/vehicle_command.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/subsystem_info.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/differential_pressure.h> +#include <uORB/topics/safety.h> + +#include <drivers/drv_led.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/systemlib.h> +#include <systemlib/err.h> +#include <systemlib/cpuload.h> +#include <systemlib/rc_check.h> + +#include "px4_custom_mode.h" +#include "commander_helper.h" +#include "state_machine_helper.h" +#include "calibration_routines.h" +#include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +extern struct system_load_s system_load; + +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define RC_TIMEOUT 100000 +#define DIFFPRESS_TIMEOUT 2000000 + +#define PRINT_INTERVAL 5000000 +#define PRINT_MODE_REJECT_INTERVAL 2000000 + +enum MAV_MODE_FLAG { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END = 129, /* | */ +}; + +/* Mavlink file descriptors */ +static int mavlink_fd; + +/* flags */ +static bool commander_initialized = false; +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +static unsigned int leds_counter; +/* To remember when last notification was sent */ +static uint64_t last_print_mode_reject_time = 0; +/* if connected via USB */ +static bool on_usb_power = false; + +static float takeoff_alt = 5.0f; + +/* tasks waiting for low prio thread */ +typedef enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task_t; + +static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE; + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + * + * @ingroup apps + */ +extern "C" __EXPORT int commander_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +void usage(const char *reason); + +/** + * React to commands that are sent e.g. from the mavlink module. + */ +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); + +void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); + +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); + +transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); + +void print_reject_mode(const char *msg); + +void print_reject_arm(const char *msg); + +void print_status(); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); + +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result); + + +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running\n"); + print_status(); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n"); + exit(1); +} + +void print_status() +{ + warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + + /* read all relevant states */ + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + struct vehicle_status_s state; + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + + const char *armed_str; + + switch (state.arming_state) { + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + + default: + armed_str = "ERR: UNKNOWN STATE"; + break; + } + + + warnx("arming: %s", armed_str); +} + +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* only handle high-priority commands here */ + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: { + uint8_t base_mode = (uint8_t) cmd->param1; + uint8_t custom_main_mode = (uint8_t) cmd->param2; + + // TODO remove debug code + //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + /* set arming state */ + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); + } + + } else { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); + } + + } else { + arming_res = TRANSITION_NOT_CHANGED; + } + } + + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; + + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } + + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } + } + } + + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; + } + + case VEHICLE_CMD_NAV_TAKEOFF: { + if (armed->armed) { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + /* reject TAKEOFF not armed */ + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; + } + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + // XXX implement later + result = VEHICLE_CMD_RESULT_DENIED; + break; + + default: + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* we do not care in the high prio loop about commands we don't know */ + } else { + tune_negative(); + + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + + } + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* send acknowledge command */ + // XXX TODO + } + +} + +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + bool battery_tune_played = false; + bool arm_tune_played = false; + + /* set parameters */ + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); + + /* welcome user */ + warnx("starting"); + + /* pthread for slow low prio thread */ + pthread_t commander_low_prio_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds"); + } + + if (buzzer_init() != OK) { + warnx("ERROR: Failed to initialize buzzer"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + /* try again later */ + usleep(20000); + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); + } + } + + /* Main state machine */ + orb_advert_t status_pub; + /* make sure we are in preflight state */ + memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); + + status.main_state = MAIN_STATE_MANUAL; + status.navigation_state = NAVIGATION_STATE_DIRECT; + status.arming_state = ARMING_STATE_INIT; + status.hil_state = HIL_STATE_OFF; + + /* neither manual nor offboard control commands have been received */ + status.offboard_control_signal_found_once = false; + status.rc_signal_found_once = false; + + /* mark all signals lost as long as they haven't been found */ + status.rc_signal_lost = true; + status.offboard_control_signal_lost = true; + + /* allow manual override initially */ + control_mode.flag_external_manual_override_ok = true; + + /* set battery warning flag */ + status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; + + // XXX for now just set sensors as initialized + status.condition_system_sensors_initialized = true; + + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + + /* advertise to ORB */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + /* publish current state machine */ + + /* publish initial state */ + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + mavlink_log_info(mavlink_fd, "[cmd] started"); + + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + + /* Start monitoring loop */ + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; + + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; + + uint64_t last_idle_time = 0; + + uint64_t start_time = 0; + + bool status_changed = true; + bool param_init_forced = true; + + bool updated = false; + + bool rc_calibration_ok = (OK == rc_calibration_check()); + + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + memset(&safety, 0, sizeof(safety)); + safety.safety_switch_available = false; + safety.safety_off = false; + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + /* Subscribe to global position */ + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + + /* Subscribe to local position data */ + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + + /* Subscribe to GPS topic */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + /* Subscribe to sensor topic */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + /* Subscribe to differential pressure topic */ + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* Subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + + toggle_status_leds(&status, &armed, true); + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + start_time = hrt_absolute_time(); + + while (!thread_should_exit) { + + /* update parameters */ + orb_check(param_changed_sub, &updated); + + if (updated || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!armed.armed) { + if (param_get(_param_sys_type, &(status.system_type)) != OK) { + warnx("failed getting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (status.system_type == VEHICLE_TYPE_COAXIAL || + status.system_type == VEHICLE_TYPE_HELICOPTER || + status.system_type == VEHICLE_TYPE_TRICOPTER || + status.system_type == VEHICLE_TYPE_QUADROTOR || + status.system_type == VEHICLE_TYPE_HEXAROTOR || + status.system_type == VEHICLE_TYPE_OCTOROTOR) { + control_mode.flag_external_manual_override_ok = false; + status.is_rotary_wing = true; + + } else { + control_mode.flag_external_manual_override_ok = true; + status.is_rotary_wing = false; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(status.system_id)); + param_get(_param_component_id, &(status.component_id)); + status_changed = true; + + /* re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check()); + + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); + } + } + + orb_check(sp_man_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(diff_pres_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + } + + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); + + orb_check(cmd_sub, &updated); + + if (updated) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(&status, &safety, &control_mode, &cmd, &armed); + } + + /* update safety topic */ + orb_check(safety_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(safety), safety_sub, &safety); + } + + /* update global position estimate */ + orb_check(global_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + } + + /* update condition_global_position_valid */ + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + + /* update local position estimate */ + orb_check(local_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + } + + /* update condition_local_position_valid and condition_local_altitude_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + + if (status.condition_local_altitude_valid) { + if (status.condition_landed != local_position.landed) { + status.condition_landed = local_position.landed; + status_changed = true; + + if (status.condition_landed) { + mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); + } + } + } + + /* update battery status */ + orb_check(battery_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + // warnx("bat v: %2.2f", battery.voltage_v); + + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } + } + + /* update subsystem */ + orb_check(subsys_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("subsystem changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + status.onboard_control_sensors_health &= ~info.subsystem_type; + } + + status_changed = true; + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + + /* check if board is connected via USB */ + struct stat statbuf; + //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); + } + + // XXX remove later + //warnx("bat remaining: %2.2f", status.battery_remaining); + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { + //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; + status_changed = true; + battery_tune_played = false; + } + + low_voltage_counter++; + + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + /* critical battery voltage, this is rather an emergency, change state machine */ + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; + battery_tune_played = false; + + if (armed.armed) { + // XXX not sure what should happen when voltage is low in flight + //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + } else { + // XXX should we still allow to arm with critical battery? + //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + } + + status_changed = true; + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + /* If in INIT state, try to proceed to STANDBY state */ + if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { + // XXX check for sensors + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + + } else { + // XXX: Add emergency stuff if sensors are lost + } + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + orb_check(gps_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + /* check if GPS fix is ok */ + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + if (!home_position_set && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { + /* copy position data to uORB home message, store it locally as well */ + // TODO use global position estimate + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + double home_lat_d = home.lat * 1e-7; + double home_lon_d = home.lon * 1e-7; + warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + home_position_set = true; + tune_positive(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* start RC input check */ + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + status_changed = true; + + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + status_changed = true; + } + } + + status.rc_signal_lost = false; + + transition_result_t res; // store all transitions results here + + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; + + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; + + } else { + stick_off_counter++; + } + + } else { + stick_off_counter = 0; + } + + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + status.main_state == MAIN_STATE_MANUAL && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; + + } else { + stick_on_counter++; + } + + } else { + stick_on_counter = 0; + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates safety switch not pressed */ + + if (!(!safety.safety_switch_available || safety.safety_off)) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine */ + res = check_main_state_machine(&status); + + if (res == TRANSITION_CHANGED) { + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + } + } + + /* evaluate the navigation state machine */ + transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); + + if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + } + + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = check_arming_state_changed(); + bool main_state_changed = check_main_state_changed(); + bool navigation_state_changed = check_navigation_state_changed(); + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; + + } else { + status_changed = false; + } + + hrt_abstime t1 = hrt_absolute_time(); + + /* publish arming state */ + if (arming_state_changed) { + armed.timestamp = t1; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } + + /* publish control mode */ + if (navigation_state_changed || arming_state_changed) { + /* publish new navigation state */ + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic + control_mode.counter++; + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } + + /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ + if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + status.counter++; + status.timestamp = t1; + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + armed.timestamp = t1; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } + + /* play arming and battery warning tunes */ + if (!arm_tune_played && armed.armed) { + /* play tune when armed */ + if (tune_arm() == OK) + arm_tune_played = true; + + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + /* play tune on battery warning */ + if (tune_low_bat() == OK) + battery_tune_played = true; + + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { + /* play tune on battery critical */ + if (tune_critical_bat() == OK) + battery_tune_played = true; + + } else if (battery_tune_played) { + tune_stop(); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + arm_tune_played = false; + } + + fflush(stdout); + counter++; + + toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + pthread_join(commander_low_prio_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(local_position_sub); + close(global_position_sub); + close(gps_sub); + close(sensor_sub); + close(safety_sub); + close(cmd_sub); + close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); + + warnx("exiting"); + fflush(stdout); + + thread_running = false; + + return 0; +} + +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + +void +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_BLUE); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 1Hz */ + if (leds_counter % 20 == 0) + led_toggle(LED_BLUE); + + } else { + /* not ready to arm, blink at 10Hz */ + if (leds_counter % 2 == 0) + led_toggle(LED_BLUE); + } + +#endif + + if (changed) { + /* XXX TODO blink fast when armed and serious error occurs */ + + if (armed->armed) { + rgbled_set_mode(RGBLED_MODE_ON); + } else if (armed->ready_to_arm) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + } else { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + } + } + + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + switch (status->battery_warning) { + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + default: + break; + } + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + default: + break; + } + } + + /* give system warnings on error LED, XXX maybe add memory usage warning too */ + if (status->load > 0.95f) { + if (leds_counter % 2 == 0) + led_toggle(LED_AMBER); + + } else { + led_off(LED_AMBER); + } + + leds_counter++; +} + +void +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +{ + /* main mode switch */ + if (!isfinite(sp_man->mode_switch)) { + warnx("mode sw not finite"); + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_AUTO; + + } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else { + current_status->mode_switch = MODE_SWITCH_ASSISTED; + } + + /* land switch */ + if (!isfinite(sp_man->return_switch)) { + current_status->return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { + current_status->return_switch = RETURN_SWITCH_RETURN; + + } else { + current_status->return_switch = RETURN_SWITCH_NONE; + } + + /* assisted switch */ + if (!isfinite(sp_man->assisted_switch)) { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { + current_status->assisted_switch = ASSISTED_SWITCH_EASY; + + } else { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + + /* mission switch */ + if (!isfinite(sp_man->mission_switch)) { + current_status->mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { + current_status->mission_switch = MISSION_SWITCH_NONE; + + } else { + current_status->mission_switch = MISSION_SWITCH_MISSION; + } +} + +transition_result_t +check_main_state_machine(struct vehicle_status_s *current_status) +{ + /* evaluate the main state machine */ + transition_result_t res = TRANSITION_DENIED; + + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT + print_reject_mode("EASY"); + } + + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode + + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT (EASY likely will not work too) + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } + + return res; +} + +void +print_reject_mode(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] WARNING: reject %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + +void +print_reject_arm(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + +transition_result_t +check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) +{ + transition_result_t res = TRANSITION_DENIED; + + if (status->main_state == MAIN_STATE_AUTO) { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + // TODO AUTO_LAND handling + if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + return TRANSITION_NOT_CHANGED; + } + } + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + /* possibly on ground, switch to TAKEOFF if needed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + return res; + } + } + /* switch to AUTO mode */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } + } else { + /* switch to MISSION when no RC control and first time in some AUTO mode */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; + + } else { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } + } + } else { + /* disarmed, always switch to AUTO_READY */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + } + + } else { + /* manual control modes */ + if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { + /* switch to failsafe mode */ + bool manual_control_old = control_mode->flag_control_manual_enabled; + + if (!status->condition_landed) { + /* in air: try to hold position */ + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + + } else { + /* landed: don't try to hold position but land (if taking off) */ + res = TRANSITION_DENIED; + } + + if (res == TRANSITION_DENIED) { + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + } + + control_mode->flag_control_manual_enabled = false; + + if (res == TRANSITION_NOT_CHANGED && manual_control_old) { + /* mark navigation state as changed to force immediate flag publishing */ + set_navigation_state_changed(); + res = TRANSITION_CHANGED; + } + + if (res == TRANSITION_CHANGED) { + if (control_mode->flag_control_position_enabled) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); + + } else { + if (status->condition_landed) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + } + } + } + + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; + + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + break; + + default: + break; + } + } + } + + return res; +} + +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(); + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + + default: + break; + } +} + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander_low_prio", getpid()); + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = cmd_sub; + fds[0].events = POLLIN; + + while (!thread_should_exit) { + + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; + + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } + + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF) + continue; + + /* only handle low-priority commands here */ + switch (cmd.command) { + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot */ + systemreset(false); + + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot to bootloader */ + systemreset(true); + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + + break; + + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + + int calib_ret = ERROR; + + /* try to go to INIT/PREFLIGHT arming state */ + + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } + + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } + + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); + + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + + break; + } + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } + } + + break; + } + + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; + } + + /* send any requested ACKs */ + if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + /* send acknowledge command */ + // XXX TODO + } + + } + + return 0; +} diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp new file mode 100644 index 000000000..7feace2b4 --- /dev/null +++ b/src/modules/commander/commander_helper.cpp @@ -0,0 +1,259 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file commander_helper.cpp + * Commander helper functions implementations + */ + +#include <stdio.h> +#include <unistd.h> +#include <stdint.h> +#include <stdbool.h> +#include <fcntl.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> +#include <drivers/drv_hrt.h> +#include <drivers/drv_tone_alarm.h> +#include <drivers/drv_led.h> +#include <drivers/drv_rgbled.h> + + +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); +} + + + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); +} + +static int leds; +static int rgbleds; + +int led_init() +{ + /* first open normal LEDs */ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + /* the blue LED is only available on FMUv1 but not FMUv2 */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + if (ioctl(leds, LED_ON, LED_BLUE)) { + warnx("Blue LED: ioctl fail\n"); + return ERROR; + } +#endif + + if (ioctl(leds, LED_ON, LED_AMBER)) { + warnx("Amber LED: ioctl fail\n"); + return ERROR; + } + + /* then try RGB LEDs, this can fail on FMUv1*/ + rgbleds = open(RGBLED_DEVICE_PATH, 0); + if (rgbleds == -1) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + errx(1, "Unable to open " RGBLED_DEVICE_PATH); +#else + warnx("No RGB LED found"); +#endif + } + + return 0; +} + +void led_deinit() +{ + close(leds); + + if (rgbleds != -1) { + close(rgbleds); + } +} + +int led_toggle(int led) +{ + return ioctl(leds, LED_TOGGLE, led); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} + +void rgbled_set_color(rgbled_color_t color) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); +} + +void rgbled_set_mode(rgbled_mode_t mode) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); +} + +void rgbled_set_pattern(rgbled_pattern_t *pattern) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); +} + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0.0f) ? 0.0f : ret; + ret = (ret > 1.0f) ? 1.0f : ret; + return ret; +} diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/commander/commander_helper.h index 048a55d33..027d0535f 100644 --- a/src/modules/mathlib/math/Quaternion.hpp +++ b/src/modules/commander/commander_helper.h @@ -1,6 +1,8 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,84 +34,54 @@ ****************************************************************************/ /** - * @file Quaternion.hpp - * - * math quaternion lib + * @file commander_helper.h + * Commander helper functions definitions */ -#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ - -class Dcm; -class EulerAngles; - -class __EXPORT Quaternion : public Vector -{ -public: +#ifndef COMMANDER_HELPER_H_ +#define COMMANDER_HELPER_H_ - /** - * default ctor - */ - Quaternion(); +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <drivers/drv_rgbled.h> - /** - * ctor from floats - */ - Quaternion(float a, float b, float c, float d); - /** - * ctor from data - */ - Quaternion(const float *data); +bool is_multirotor(const struct vehicle_status_s *current_status); +bool is_rotary_wing(const struct vehicle_status_s *current_status); - /** - * ctor from Vector - */ - Quaternion(const Vector &v); +int buzzer_init(void); +void buzzer_deinit(void); - /** - * ctor from EulerAngles - */ - Quaternion(const EulerAngles &euler); +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +int tune_arm(void); +int tune_low_bat(void); +int tune_critical_bat(void); +void tune_stop(void); - /** - * ctor from Dcm - */ - Quaternion(const Dcm &dcm); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); - /** - * deep copy ctor - */ - Quaternion(const Quaternion &right); +void rgbled_set_color(rgbled_color_t color); - /** - * dtor - */ - virtual ~Quaternion(); +void rgbled_set_mode(rgbled_mode_t mode); - /** - * derivative - */ - Vector derivative(const Vector &w); +void rgbled_set_pattern(rgbled_pattern_t *pattern); - /** - * accessors - */ - void setA(float a) { (*this)(0) = a; } - void setB(float b) { (*this)(1) = b; } - void setC(float c) { (*this)(2) = c; } - void setD(float d) { (*this)(3) = d; } - const float &getA() const { return (*this)(0); } - const float &getB() const { return (*this)(1); } - const float &getC() const { return (*this)(2); } - const float &getD() const { return (*this)(3); } -}; - -int __EXPORT quaternionTest(); -} // math +/** + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); +#endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c index 04b4e72ab..f22dac0c1 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander_params.c @@ -1,10 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,23 +33,22 @@ ****************************************************************************/ /** - * @file commander.h - * Main system state machine definition. + * @file commander_params.c + * + * Parameters defined by the sensors task. * - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch> * @author Julian Oes <joes@student.ethz.ch> - * */ -#ifndef COMMANDER_H_ -#define COMMANDER_H_ - -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f - -void tune_confirm(void); -void tune_error(void); +#include <nuttx/config.h> +#include <systemlib/param/param.h> -#endif /* COMMANDER_H_ */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); diff --git a/src/modules/mathlib/math/Vector.hpp b/src/modules/commander/commander_tests/commander_tests.cpp index 73de793d5..6e72cf0d9 100644 --- a/src/modules/mathlib/math/Vector.hpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -1,6 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,26 +33,23 @@ ****************************************************************************/ /** - * @file Vector.h + * @file commander_tests.cpp + * Commander unit tests. Run the tests as follows: + * nsh> commander_tests * - * math vector */ -#pragma once +#include <systemlib/err.h> -#include <nuttx/config.h> +#include "state_machine_helper_test.h" -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) -#include "arm/Vector.hpp" -#else -#include "generic/Vector.hpp" -#endif +extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); -namespace math + +int commander_tests_main(int argc, char *argv[]) { -class Vector; -int __EXPORT vectorTest(); -int __EXPORT vectorAddTest(); -int __EXPORT vectorSubTest(); -bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f); -} // math + state_machine_helper_test(); + //state_machine_test(); + + return 0; +} diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/commander/commander_tests/module.mk index fe92c8c70..4d10275d1 100644 --- a/src/modules/mathlib/math/filter/module.mk +++ b/src/modules/commander/commander_tests/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,13 +32,10 @@ ############################################################################ # -# filter library +# System state machine tests. # -SRCS = LowPassFilter2p.cpp -# -# In order to include .config we first have to save off the -# current makefile name, since app.mk needs it. -# -APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config +MODULE_COMMAND = commander_tests +SRCS = commander_tests.cpp \ + state_machine_helper_test.cpp \ + ../state_machine_helper.cpp diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp new file mode 100644 index 000000000..40bedd9f3 --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file state_machine_helper_test.cpp + * System state machine unit test. + * + */ + +#include "state_machine_helper_test.h" + +#include "../state_machine_helper.h" +#include <unit_test/unit_test.h> + +class StateMachineHelperTest : public UnitTest +{ +public: + StateMachineHelperTest(); + virtual ~StateMachineHelperTest(); + + virtual const char* run_tests(); + +private: + const char* arming_state_transition_test(); + const char* arming_state_transition_arm_disarm_test(); + const char* main_state_transition_test(); + const char* is_safe_test(); +}; + +StateMachineHelperTest::StateMachineHelperTest() { +} + +StateMachineHelperTest::~StateMachineHelperTest() { +} + +const char* +StateMachineHelperTest::arming_state_transition_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // Identical states. + status.arming_state = ARMING_STATE_INIT; + new_arming_state = ARMING_STATE_INIT; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + + // INIT to STANDBY. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = true; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("transition: init to standby", + TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("ready to arm", armed.ready_to_arm); + + // INIT to STANDBY, sensors not initialized. + armed.armed = false; + armed.ready_to_arm = false; + status.arming_state = ARMING_STATE_INIT; + status.condition_system_sensors_initialized = false; + new_arming_state = ARMING_STATE_STANDBY; + mu_assert("no transition: sensors not initialized", + TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); + mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); + mu_assert("not armed", !armed.armed); + mu_assert("not ready to arm", !armed.ready_to_arm); + + return 0; +} + +const char* +StateMachineHelperTest::arming_state_transition_arm_disarm_test() +{ + struct vehicle_status_s status; + struct safety_s safety; + arming_state_t new_arming_state; + struct actuator_armed_s armed; + + // TODO(sjwilks): ARM then DISARM. + return 0; +} + +const char* +StateMachineHelperTest::main_state_transition_test() +{ + struct vehicle_status_s current_state; + main_state_t new_main_state; + + // Identical states. + current_state.main_state = MAIN_STATE_MANUAL; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("no transition: identical states", + TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // AUTO to MANUAL. + current_state.main_state = MAIN_STATE_AUTO; + new_main_state = MAIN_STATE_MANUAL; + mu_assert("transition changed: auto to manual", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to SEATBELT. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = true; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("tranisition: manual to seatbelt", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + + // MANUAL to SEATBELT, invalid local altitude. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_altitude_valid = false; + new_main_state = MAIN_STATE_SEATBELT; + mu_assert("no transition: invalid local altitude", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to EASY. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = true; + new_main_state = MAIN_STATE_EASY; + mu_assert("transition: manual to easy", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + + // MANUAL to EASY, invalid local position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_local_position_valid = false; + new_main_state = MAIN_STATE_EASY; + mu_assert("no transition: invalid position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + // MANUAL to AUTO. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = true; + new_main_state = MAIN_STATE_AUTO; + mu_assert("transition: manual to auto", + TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + + // MANUAL to AUTO, invalid global position. + current_state.main_state = MAIN_STATE_MANUAL; + current_state.condition_global_position_valid = false; + new_main_state = MAIN_STATE_AUTO; + mu_assert("no transition: invalid global position", + TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); + mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + + return 0; +} + +const char* +StateMachineHelperTest::is_safe_test() +{ + struct vehicle_status_s current_state; + struct safety_s safety; + struct actuator_armed_s armed; + + armed.armed = false; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + + armed.armed = false; + armed.lockdown = true; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = true; + mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = true; + safety.safety_off = false; + mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + + armed.armed = true; + armed.lockdown = false; + safety.safety_switch_available = false; + safety.safety_off = false; + mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + + return 0; +} + +const char* +StateMachineHelperTest::run_tests() +{ + mu_run_test(arming_state_transition_test); + mu_run_test(arming_state_transition_arm_disarm_test); + mu_run_test(main_state_transition_test); + mu_run_test(is_safe_test); + + return 0; +} + +void +state_machine_helper_test() +{ + StateMachineHelperTest* test = new StateMachineHelperTest(); + test->UnitTest::print_results(test->run_tests()); +} diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h new file mode 100644 index 000000000..10a68e602 --- /dev/null +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file state_machine_helper_test.h + */ + +#ifndef STATE_MACHINE_HELPER_TEST_H_ +#define STATE_MACHINE_HELPER_TEST_ + +void state_machine_helper_test(); + +#endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp new file mode 100644 index 000000000..33566d4e5 --- /dev/null +++ b/src/modules/commander/gyro_calibration.cpp @@ -0,0 +1,289 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file gyro_calibration.cpp + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "commander_helper.h" + +#include <stdio.h> +#include <fcntl.h> +#include <poll.h> +#include <math.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_gyro.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + unsigned poll_errcount = 0; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); + return ERROR; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + return ERROR; + } + + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_neutral(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + return ERROR; + } + + + /*** --- SCALING --- ***/ + + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); + mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + + unsigned rotations_count = 30; + float gyro_integral = 0.0f; + float baseline_integral = 0.0f; + + // XXX change to mag topic + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; + if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + + + uint64_t last_time = hrt_absolute_time(); + uint64_t start_time = hrt_absolute_time(); + + while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { + + /* abort this loop if not rotated more than 180 degrees within 5 seconds */ + if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) + && (hrt_absolute_time() - start_time > 5 * 1e6)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + return OK; + } + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + + float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; + last_time = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + // XXX this is just a proof of concept and needs world / body + // transformation and more + + //math::Vector2f magNav(raw.magnetometer_ga); + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + //float mag = -atan2f(magNav(1),magNav(0)); + float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag > M_PI_F) mag -= 2*M_PI_F; + if (mag < -M_PI_F) mag += 2*M_PI_F; + + float diff = mag - mag_last; + + if (diff > M_PI_F) diff -= 2*M_PI_F; + if (diff < -M_PI_F) diff += 2*M_PI_F; + + baseline_integral += diff; + mag_last = mag; + // Jump through some timing scale hoops to avoid + // operating near the 1e6/1e8 max sane resolution of float. + gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; + +// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); + + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; + } + } + + float gyro_scale = baseline_integral / gyro_integral; + float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); + + + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { + + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + gyro_scales[0], + gyro_offset[1], + gyro_scales[1], + gyro_offset[2], + gyro_scales[2], + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + /* third beep by cal end routine */ + return OK; + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + return ERROR; + } +} diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/modules/commander/gyro_calibration.h index 21661622a..59c32a15e 100644 --- a/src/modules/mathlib/math/arm/Matrix.cpp +++ b/src/modules/commander/gyro_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,15 @@ ****************************************************************************/ /** - * @file Matrix.cpp - * - * matrix code + * @file gyro_calibration.h + * Gyroscope calibration routine */ -#include "Matrix.hpp" +#ifndef GYRO_CALIBRATION_H_ +#define GYRO_CALIBRATION_H_ + +#include <stdint.h> + +int do_gyro_calibration(int mavlink_fd); + +#endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp new file mode 100644 index 000000000..e38616027 --- /dev/null +++ b/src/modules/commander/mag_calibration.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file mag_calibration.cpp + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" + +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <poll.h> +#include <math.h> +#include <fcntl.h> +#include <drivers/drv_hrt.h> +#include <uORB/topics/sensor_combined.h> +#include <drivers/drv_mag.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait."); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return ERROR; + } + + mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting.."); + + unsigned poll_errcount = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); + tune_neutral(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor"); + return ERROR; + } + + + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "magnetometer calibration completed"); + mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); + + return OK; + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + return ERROR; + } +} diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/modules/commander/mag_calibration.h index 7ea6496bb..a101cd7b1 100644 --- a/src/modules/mathlib/math/generic/Vector.cpp +++ b/src/modules/commander/mag_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,15 @@ ****************************************************************************/ /** - * @file Vector.cpp - * - * math vector + * @file mag_calibration.h + * Magnetometer calibration routine */ -#include "Vector.hpp" +#ifndef MAG_CALIBRATION_H_ +#define MAG_CALIBRATION_H_ + +#include <stdint.h> + +int do_mag_calibration(int mavlink_fd); + +#endif /* MAG_CALIBRATION_H_ */ diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fe44e955a..91d75121e 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -36,8 +36,15 @@ # MODULE_COMMAND = commander -SRCS = commander.c \ - state_machine_helper.c \ - calibration_routines.c \ - accelerometer_calibration.c +SRCS = commander.cpp \ + commander_params.c \ + state_machine_helper.cpp \ + commander_helper.cpp \ + calibration_routines.cpp \ + accelerometer_calibration.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp \ + baro_calibration.cpp \ + rc_calibration.cpp \ + airspeed_calibration.cpp diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h new file mode 100644 index 000000000..b60a7e0b9 --- /dev/null +++ b/src/modules/commander/px4_custom_mode.h @@ -0,0 +1,37 @@ +/* + * px4_custom_mode.h + * + * Created on: 09.08.2013 + * Author: ton + */ + +#ifndef PX4_CUSTOM_MODE_H_ +#define PX4_CUSTOM_MODE_H_ + +enum PX4_CUSTOM_MAIN_MODE { + PX4_CUSTOM_MAIN_MODE_MANUAL = 1, + PX4_CUSTOM_MAIN_MODE_SEATBELT, + PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_AUTO, +}; + +enum PX4_CUSTOM_SUB_MODE_AUTO { + PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION, + PX4_CUSTOM_SUB_MODE_AUTO_RTL, + PX4_CUSTOM_SUB_MODE_AUTO_LAND, +}; + +union px4_custom_mode { + struct { + uint16_t reserved; + uint8_t main_mode; + uint8_t sub_mode; + }; + uint32_t data; + float data_float; +}; + +#endif /* PX4_CUSTOM_MODE_H_ */ diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/commander/rc_calibration.cpp index dcb85600e..fe87a3323 100644 --- a/src/modules/mathlib/math/Vector3.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,68 +32,57 @@ ****************************************************************************/ /** - * @file Vector3.cpp - * - * math vector + * @file rc_calibration.cpp + * Remote Control calibration routine */ -#include "test/test.hpp" +#include "rc_calibration.h" +#include "commander_helper.h" -#include "Vector3.hpp" +#include <poll.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/manual_control_setpoint.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> -namespace math -{ +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -Vector3::Vector3() : - Vector(3) +int do_rc_calibration(int mavlink_fd) { -} + mavlink_log_info(mavlink_fd, "trim calibration starting"); -Vector3::Vector3(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 3); -#endif -} + /* XXX fix this */ + // if (current_status.rc_signal) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } -Vector3::Vector3(float x, float y, float z) : - Vector(3) -{ - setX(x); - setY(y); - setZ(z); -} + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); -Vector3::Vector3(const float *data) : - Vector(3, data) -{ -} + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); -Vector3::~Vector3() -{ -} + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); -Vector3 Vector3::cross(const Vector3 &b) const -{ - const Vector3 &a = *this; - Vector3 result; - result(0) = a(1) * b(2) - a(2) * b(1); - result(1) = a(2) * b(0) - a(0) * b(2); - result(2) = a(0) * b(1) - a(1) * b(0); - return result; -} + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + return ERROR; + } -int __EXPORT vector3Test() -{ - printf("Test Vector3\t\t: "); - // test float ctor - Vector3 v(1, 2, 3); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - ASSERT(equal(v(2), 3)); - printf("PASS\n"); - return 0; + mavlink_log_info(mavlink_fd, "trim calibration done"); + return OK; } - -} // namespace math diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/modules/commander/rc_calibration.h index 7ea6496bb..9aa6faafa 100644 --- a/src/modules/mathlib/math/arm/Vector.cpp +++ b/src/modules/commander/rc_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,9 +32,15 @@ ****************************************************************************/ /** - * @file Vector.cpp - * - * math vector + * @file rc_calibration.h + * Remote Control calibration routine */ -#include "Vector.hpp" +#ifndef RC_CALIBRATION_H_ +#define RC_CALIBRATION_H_ + +#include <stdint.h> + +int do_rc_calibration(int mavlink_fd); + +#endif /* RC_CALIBRATION_H_ */ diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c deleted file mode 100644 index ab728c7bb..000000000 --- a/src/modules/commander/state_machine_helper.c +++ /dev/null @@ -1,757 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file state_machine_helper.c - * State machine helper functions implementations - */ - -#include <stdio.h> -#include <unistd.h> - -#include <uORB/uORB.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/actuator_controls.h> -#include <systemlib/systemlib.h> -#include <drivers/drv_hrt.h> -#include <mavlink/mavlink_log.h> - -#include "state_machine_helper.h" - -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; - -/** - * Transition from one state to another - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) -{ - int invalid_state = false; - int ret = ERROR; - - commander_state_machine_t old_state = current_status->state_machine; - - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); - - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } - } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; - - case SYSTEM_STATE_GROUND_ERROR: - - /* set system flags according to state */ - - /* prevent actuators from arming */ - current_status->flag_system_armed = false; - - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; - - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } - - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: - - /* set system flags according to state */ - - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; - - case SYSTEM_STATE_AUTO: - - /* set system flags according to state */ - - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; - - case SYSTEM_STATE_STABILIZED: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; - - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; - } - - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; - } - - return ret; -} - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; - - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); -} - -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - - /* XXX allow arming by external components on multicopters only if not yet armed by RC */ - /* XXX allow arming only if core sensors are ok */ - armed.ready_to_arm = true; - - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} - - -/* - * Private functions, update the state machine - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ - - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} - - - -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - - -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ - - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp new file mode 100644 index 000000000..3cef10185 --- /dev/null +++ b/src/modules/commander/state_machine_helper.cpp @@ -0,0 +1,698 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler <thomasgubler@student.ethz.ch> + * Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file state_machine_helper.cpp + * State machine helper functions implementations + */ + +#include <stdio.h> +#include <unistd.h> +#include <stdint.h> +#include <stdbool.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_controls.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <systemlib/systemlib.h> +#include <systemlib/param/param.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <mavlink/mavlink_log.h> + +#include "state_machine_helper.h" +#include "commander_helper.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +static bool arming_state_changed = true; +static bool main_state_changed = true; +static bool navigation_state_changed = true; + +transition_result_t +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) +{ + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state == status->arming_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (status->arming_state == ARMING_STATE_STANDBY) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (status->condition_system_sensors_initialized) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = true; + } + } + + break; + + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if ((status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = true; + } + + break; + + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_ARMED) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_STANDBY_ERROR: + + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + + /* end of atomic state update */ + irqrestore(flags); + + if (ret == TRANSITION_DENIED) + warnx("arming transition rejected"); + + return ret; +} + +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + + } else { + return false; + } +} + +bool +check_arming_state_changed() +{ + if (arming_state_changed) { + arming_state_changed = false; + return true; + + } else { + return false; + } +} + +transition_result_t +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_main_state == current_state->main_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; + + case MAIN_STATE_SEATBELT: + + /* need altitude estimate */ + if (current_state->condition_local_altitude_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_EASY: + + /* need local position estimate */ + if (current_state->condition_local_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case MAIN_STATE_AUTO: + + /* need global position estimate */ + if (current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + } + + if (ret == TRANSITION_CHANGED) { + current_state->main_state = new_main_state; + main_state_changed = true; + } + } + + return ret; +} + +bool +check_main_state_changed() +{ + if (main_state_changed) { + main_state_changed = false; + return true; + + } else { + return false; + } +} + +transition_result_t +navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_DIRECT: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_STABILIZE: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_ALTHOLD: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_VECTOR: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_READY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_LOITER: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_MISSION: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_RTL: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* deny transitions from landed state */ + if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; + } + + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + status->navigation_state = new_navigation_state; + control_mode->auto_state = status->navigation_state; + navigation_state_changed = true; + } + } + + return ret; +} + +bool +check_navigation_state_changed() +{ + if (navigation_state_changed) { + navigation_state_changed = false; + return true; + + } else { + return false; + } +} + +void +set_navigation_state_changed() +{ + navigation_state_changed = true; +} + +/** +* Transition from one hil state to another +*/ +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + + ret = OK; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + + + +// /* +// * Wrapper functions (to be used in the commander), all functions assume lock on current_status +// */ + +// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR +// * +// * START SUBSYSTEM/EMERGENCY FUNCTIONS +// * */ + +// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was removed something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + +// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); +// } + +// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) +// { +// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); +// current_status->counter++; +// current_status->timestamp = hrt_absolute_time(); +// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + +// /* if a subsystem was disabled something went completely wrong */ + +// switch (*subsystem_type) { +// case SUBSYSTEM_TYPE_GYRO: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_ACC: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_MAG: +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency_always_critical(status_pub, current_status); +// break; + +// case SUBSYSTEM_TYPE_GPS: +// { +// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); + +// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { +// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); +// state_machine_emergency(status_pub, current_status); +// } +// } +// break; + +// default: +// break; +// } + +// } + + +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_fmu_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2f2ccc729..1641b6f60 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler <thomasgubler@student.ethz.ch> * Julian Oes <joes@student.ethz.ch> * @@ -46,164 +46,34 @@ #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/safety.h> +#include <uORB/topics/vehicle_control_mode.h> -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +} transition_result_t; -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed); -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); +bool check_arming_state_changed(); -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +bool check_main_state_changed(); -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); +bool check_navigation_state_changed(); +void set_navigation_state_changed(); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 9c0720aa5..46dc1bec2 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -58,7 +58,7 @@ #include <poll.h> extern "C" { -#include <systemlib/geo/geo.h> +#include <geo/geo.h> } #include "../blocks.hpp" diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c index 6c9c137bb..b6b4546c2 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c @@ -53,6 +53,7 @@ #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_status.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> @@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) memset(&global_pos, 0, sizeof(global_pos)); struct manual_control_setpoint_s manual_sp; memset(&manual_sp, 0, sizeof(manual_sp)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_status_s vstatus; memset(&vstatus, 0, sizeof(vstatus)); @@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* Setup of loop */ float gyro[3] = {0.0f, 0.0f, 0.0f}; @@ -178,30 +182,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp); - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); gyro[0] = att.rollspeed; gyro[1] = att.pitchspeed; gyro[2] = att.yawspeed; - /* control */ - - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + /* set manual setpoints if required */ + if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (vstatus.rc_signal_lost) { @@ -234,15 +224,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - /* pass through flaps */ if (isfinite(manual_sp.flaps)) { actuators.control[4] = manual_sp.flaps; @@ -251,7 +232,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) actuators.control[4] = 0.0f; } - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else { /* directly pass through values */ actuators.control[0] = manual_sp.roll; /* positive pitch means negative actuator -> pull up */ @@ -267,6 +248,22 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) } } } + + /* execute attitude control if requested */ + if (control_mode.flag_control_attitude_enabled) { + /* attitude control */ + fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); + + /* angular rate control */ + fixedwing_att_control_rates(&rates_sp, gyro, &actuators); + + /* pass through throttle */ + actuators.control[3] = att_sp.thrust; + + /* set flaps to zero */ + actuators.control[4] = 0.0f; + + } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index f655a13bd..d65045d68 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -156,7 +156,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); } @@ -166,8 +166,8 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || + _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); @@ -219,89 +219,83 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { + if (_status.hil_state != HIL_STATE_ON) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; - - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between - // the min/max velocity - float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + - _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); - - // pitch channel -> rate of climb - // TODO, might want to put a gain on this, otherwise commanding - // from +1 -> -1 m/s for rate of climb - //float dThrottle = _cr2Thr.update( - //_crMax.get()*_manual.pitch - _pos.vz); - - // roll channel -> bank angle - float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); - float pCmd = _phi2P.update(phiCmd - _att.roll); - - // throttle channel -> velocity - // negative sign because nose over to increase speed - float vCmd = _vLimit.update(_manual.throttle * - (_vLimit.getMax() - _vLimit.getMin()) + - _vLimit.getMin()); - float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); - float qCmd = _theta2Q.update(thetaCmd - _att.pitch); - - // yaw rate cmd - float rCmd = 0; - - // stabilization - _stabilization.update(pCmd, qCmd, rCmd, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - // output - _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); - _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); - _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); - - // currently using manual throttle - // XXX if you enable this watch out, vz might be very noisy - //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); - _actuators.control[CH_THR] = _manual.throttle; - - // XXX limit throttle to manual setting (safety) for now. - // If it turns out to be confusing, it can be removed later once - // a first binary release can be targeted. - // This is not a hack, but a design choice. - - /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } - } + } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here? + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + // calculate velocity, XXX should be airspeed, but using ground speed for now + // for the purpose of control we will limit the velocity feedback between + // the min/max velocity + float v = _vLimit.update(sqrtf( + _pos.vx * _pos.vx + + _pos.vy * _pos.vy + + _pos.vz * _pos.vz)); + + // pitch channel -> rate of climb + // TODO, might want to put a gain on this, otherwise commanding + // from +1 -> -1 m/s for rate of climb + //float dThrottle = _cr2Thr.update( + //_crMax.get()*_manual.pitch - _pos.vz); - // body rates controller, disabled for now - else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { + // roll channel -> bank angle + float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax()); + float pCmd = _phi2P.update(phiCmd - _att.roll); - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + // throttle channel -> velocity + // negative sign because nose over to increase speed + float vCmd = _vLimit.update(_manual.throttle * + (_vLimit.getMax() - _vLimit.getMin()) + + _vLimit.getMin()); + float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v)); + float qCmd = _theta2Q.update(thetaCmd - _att.pitch); - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; + // yaw rate cmd + float rCmd = 0; + + // stabilization + _stabilization.update(pCmd, qCmd, rCmd, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + // output + _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get(); + _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get(); + _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get(); + + // currently using manual throttle + // XXX if you enable this watch out, vz might be very noisy + //_actuators.control[CH_THR] = dThrottle + _trimThr.get(); + _actuators.control[CH_THR] = _manual.throttle; + + // XXX limit throttle to manual setting (safety) for now. + // If it turns out to be confusing, it can be removed later once + // a first binary release can be targeted. + // This is not a hack, but a design choice. + + /* do not limit in HIL */ + if (_status.hil_state != HIL_STATE_ON) { + /* limit to value of manual throttle */ + _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + _actuators.control[CH_THR] : _manual.throttle; } + // body rates controller, disabled for now + // TODO + } else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) { // TODO use vehicle_control_mode here? + + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); + + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; } // update all publications diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 1aef739c7..d383146f9 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,6 +51,7 @@ #include <systemlib/err.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/actuator_armed.h> #include <poll.h> #include <drivers/drv_gpio.h> #include <modules/px4iofirmware/protocol.h> @@ -62,6 +63,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; + struct actuator_armed_s armed; + int actuator_armed_sub; bool led_state; int counter; }; @@ -109,19 +112,19 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[3], "a1")) { use_io = true; - pin = PX4IO_ACC1; + pin = PX4IO_P_SETUP_RELAYS_ACC1; } else if (!strcmp(argv[3], "a2")) { use_io = true; - pin = PX4IO_ACC2; + pin = PX4IO_P_SETUP_RELAYS_ACC2; } else if (!strcmp(argv[3], "r1")) { use_io = true; - pin = PX4IO_RELAY1; + pin = PX4IO_P_SETUP_RELAYS_POWER1; } else if (!strcmp(argv[3], "r2")) { use_io = true; - pin = PX4IO_RELAY2; + pin = PX4IO_P_SETUP_RELAYS_POWER2; } else { errx(1, "unsupported pin: %s", argv[3]); @@ -142,7 +145,7 @@ int gpio_led_main(int argc, char *argv[]) char pin_name[24]; if (use_io) { - if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) { + if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) { sprintf(pin_name, "PX4IO ACC%i", (pin >> 3)); } else { @@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg) if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); + /* select pattern for current status */ int pattern = 0; - if (priv->status.flag_system_armed) { + if (priv->armed.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (priv->armed.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h deleted file mode 100644 index 8f39acd9d..000000000 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h +++ /dev/null @@ -1,264 +0,0 @@ -/**************************************************************************//**
- * @file ARMCM3.h
- * @brief CMSIS Core Peripheral Access Layer Header File for
- * ARMCM3 Device Series
- * @version V1.07
- * @date 30. January 2012
- *
- * @note
- * Copyright (C) 2012 ARM Limited. All rights reserved.
- *
- * @par
- * ARM Limited (ARM) is supplying this software for use with Cortex-M
- * processor based microcontrollers. This file can be freely distributed
- * within development tools that are supporting such ARM based processors.
- *
- * @par
- * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
- * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
- * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
- * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
- *
- ******************************************************************************/
-
-#ifndef ARMCM3_H
-#define ARMCM3_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-/* ------------------------- Interrupt Number Definition ------------------------ */
-
-typedef enum IRQn
-{
-/* ------------------- Cortex-M3 Processor Exceptions Numbers ------------------- */
- NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */
- HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */
- MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */
- BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */
- UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */
- SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */
- DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */
- PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */
- SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */
-
-/* ---------------------- ARMCM3 Specific Interrupt Numbers --------------------- */
- WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */
- RTC_IRQn = 1, /*!< Real Time Clock Interrupt */
- TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */
- TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */
- MCIA_IRQn = 4, /*!< MCIa Interrupt */
- MCIB_IRQn = 5, /*!< MCIb Interrupt */
- UART0_IRQn = 6, /*!< UART0 Interrupt */
- UART1_IRQn = 7, /*!< UART1 Interrupt */
- UART2_IRQn = 8, /*!< UART2 Interrupt */
- UART4_IRQn = 9, /*!< UART4 Interrupt */
- AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */
- CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */
- ENET_IRQn = 12, /*!< Ethernet Interrupt */
- USBDC_IRQn = 13, /*!< USB Device Interrupt */
- USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */
- CHLCD_IRQn = 15, /*!< Character LCD Interrupt */
- FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */
- CAN_IRQn = 17, /*!< CAN Interrupt */
- LIN_IRQn = 18, /*!< LIN Interrupt */
- I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */
- CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */
- UART3_IRQn = 30, /*!< UART3 Interrupt */
- SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */
-} IRQn_Type;
-
-
-/* ================================================================================ */
-/* ================ Processor and Core Peripheral Section ================ */
-/* ================================================================================ */
-
-/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */
-#define __CM3_REV 0x0201 /*!< Core revision r2p1 */
-#define __MPU_PRESENT 1 /*!< MPU present or not */
-#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */
-#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */
-
-#include <core_cm3.h> /* Processor and core peripherals */
-/* NuttX */
-//#include "system_ARMCM3.h" /* System Header */
-
-
-/* ================================================================================ */
-/* ================ Device Specific Peripheral Section ================ */
-/* ================================================================================ */
-
-/* ------------------- Start of section using anonymous unions ------------------ */
-#if defined(__CC_ARM)
- #pragma push
- #pragma anon_unions
-#elif defined(__ICCARM__)
- #pragma language=extended
-#elif defined(__GNUC__)
- /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
-/* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
- #pragma warning 586
-#else
- #warning Not supported compiler type
-#endif
-
-
-
-/* ================================================================================ */
-/* ================ CPU FPGA System (CPU_SYS) ================ */
-/* ================================================================================ */
-typedef struct
-{
- __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
- __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */
- __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
- __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
- __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */
- __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */
- uint32_t RESERVED0[2];
- __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */
- __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */
- __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */
- uint32_t RESERVED1[3];
- __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */
- __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */
-} ARM_CPU_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================ DUT FPGA System (DUT_SYS) ================ */
-/* ================================================================================ */
-typedef struct
-{
- __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
- __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */
- __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
- __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
- __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */
- __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */
- __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */
-} ARM_DUT_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================ Timer (TIM) ================ */
-/* ================================================================================ */
-typedef struct
-{
- __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */
- __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */
- __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */
- __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */
- __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */
- __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */
- __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */
- uint32_t RESERVED0[1];
- __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */
- __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */
- __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */
- __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */
- __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */
- __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */
- __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */
-} ARM_TIM_TypeDef;
-
-
-/* ================================================================================ */
-/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
-/* ================================================================================ */
-typedef struct
-{
- __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */
- union {
- __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */
- __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */
- };
- uint32_t RESERVED0[4];
- __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */
- uint32_t RESERVED1[1];
- __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */
- __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */
- __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */
- __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */
- __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */
- __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */
- __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */
- __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */
- __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */
- __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */
- __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */
-} ARM_UART_TypeDef;
-
-
-/* -------------------- End of section using anonymous unions ------------------- */
-#if defined(__CC_ARM)
- #pragma pop
-#elif defined(__ICCARM__)
- /* leave anonymous unions enabled */
-#elif defined(__GNUC__)
- /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
- /* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
- #pragma warning restore
-#else
- #warning Not supported compiler type
-#endif
-
-
-
-
-/* ================================================================================ */
-/* ================ Peripheral memory map ================ */
-/* ================================================================================ */
-/* -------------------------- CPU FPGA memory map ------------------------------- */
-#define ARM_FLASH_BASE (0x00000000UL)
-#define ARM_RAM_BASE (0x20000000UL)
-#define ARM_RAM_FPGA_BASE (0x1EFF0000UL)
-#define ARM_CPU_CFG_BASE (0xDFFF0000UL)
-
-#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000)
-#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000)
-
-/* -------------------------- DUT FPGA memory map ------------------------------- */
-#define ARM_APB_BASE (0x40000000UL)
-#define ARM_AHB_BASE (0x4FF00000UL)
-#define ARM_DMC_BASE (0x60000000UL)
-#define ARM_SMC_BASE (0xA0000000UL)
-
-#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000)
-#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000)
-#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000)
-#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000)
-#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000)
-#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000)
-#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000)
-
-
-/* ================================================================================ */
-/* ================ Peripheral declaration ================ */
-/* ================================================================================ */
-/* -------------------------- CPU FPGA Peripherals ------------------------------ */
-#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE)
-#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE)
-
-/* -------------------------- DUT FPGA Peripherals ------------------------------ */
-#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE)
-#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE)
-#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE)
-#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE)
-#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE)
-#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE)
-#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE)
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* ARMCM3_H */
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h deleted file mode 100644 index 181b7e433..000000000 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h +++ /dev/null @@ -1,265 +0,0 @@ -/**************************************************************************//**
- * @file ARMCM4.h
- * @brief CMSIS Core Peripheral Access Layer Header File for
- * ARMCM4 Device Series
- * @version V1.07
- * @date 30. January 2012
- *
- * @note
- * Copyright (C) 2012 ARM Limited. All rights reserved.
- *
- * @par
- * ARM Limited (ARM) is supplying this software for use with Cortex-M
- * processor based microcontrollers. This file can be freely distributed
- * within development tools that are supporting such ARM based processors.
- *
- * @par
- * THIS SOFTWARE IS PROVIDED "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED
- * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
- * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
- * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
- *
- ******************************************************************************/
-
-#ifndef ARMCM4_H
-#define ARMCM4_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-
-/* ------------------------- Interrupt Number Definition ------------------------ */
-
-typedef enum IRQn
-{
-/* ------------------- Cortex-M4 Processor Exceptions Numbers ------------------- */
- NonMaskableInt_IRQn = -14, /*!< 2 Non Maskable Interrupt */
- HardFault_IRQn = -13, /*!< 3 HardFault Interrupt */
- MemoryManagement_IRQn = -12, /*!< 4 Memory Management Interrupt */
- BusFault_IRQn = -11, /*!< 5 Bus Fault Interrupt */
- UsageFault_IRQn = -10, /*!< 6 Usage Fault Interrupt */
- SVCall_IRQn = -5, /*!< 11 SV Call Interrupt */
- DebugMonitor_IRQn = -4, /*!< 12 Debug Monitor Interrupt */
- PendSV_IRQn = -2, /*!< 14 Pend SV Interrupt */
- SysTick_IRQn = -1, /*!< 15 System Tick Interrupt */
-
-/* ---------------------- ARMCM4 Specific Interrupt Numbers --------------------- */
- WDT_IRQn = 0, /*!< Watchdog Timer Interrupt */
- RTC_IRQn = 1, /*!< Real Time Clock Interrupt */
- TIM0_IRQn = 2, /*!< Timer0 / Timer1 Interrupt */
- TIM2_IRQn = 3, /*!< Timer2 / Timer3 Interrupt */
- MCIA_IRQn = 4, /*!< MCIa Interrupt */
- MCIB_IRQn = 5, /*!< MCIb Interrupt */
- UART0_IRQn = 6, /*!< UART0 Interrupt */
- UART1_IRQn = 7, /*!< UART1 Interrupt */
- UART2_IRQn = 8, /*!< UART2 Interrupt */
- UART4_IRQn = 9, /*!< UART4 Interrupt */
- AACI_IRQn = 10, /*!< AACI / AC97 Interrupt */
- CLCD_IRQn = 11, /*!< CLCD Combined Interrupt */
- ENET_IRQn = 12, /*!< Ethernet Interrupt */
- USBDC_IRQn = 13, /*!< USB Device Interrupt */
- USBHC_IRQn = 14, /*!< USB Host Controller Interrupt */
- CHLCD_IRQn = 15, /*!< Character LCD Interrupt */
- FLEXRAY_IRQn = 16, /*!< Flexray Interrupt */
- CAN_IRQn = 17, /*!< CAN Interrupt */
- LIN_IRQn = 18, /*!< LIN Interrupt */
- I2C_IRQn = 19, /*!< I2C ADC/DAC Interrupt */
- CPU_CLCD_IRQn = 28, /*!< CPU CLCD Combined Interrupt */
- UART3_IRQn = 30, /*!< UART3 Interrupt */
- SPI_IRQn = 31, /*!< SPI Touchscreen Interrupt */
-} IRQn_Type;
-
-
-/* ================================================================================ */
-/* ================ Processor and Core Peripheral Section ================ */
-/* ================================================================================ */
-
-/* -------- Configuration of the Cortex-M4 Processor and Core Peripherals ------- */
-#define __CM4_REV 0x0001 /*!< Core revision r0p1 */
-#define __MPU_PRESENT 1 /*!< MPU present or not */
-#define __NVIC_PRIO_BITS 3 /*!< Number of Bits used for Priority Levels */
-#define __Vendor_SysTickConfig 0 /*!< Set to 1 if different SysTick Config is used */
-#define __FPU_PRESENT 1 /*!< FPU present or not */
-
-#include <core_cm4.h> /* Processor and core peripherals */
-/* NuttX */
-//#include "system_ARMCM4.h" /* System Header */
-
-
-/* ================================================================================ */
-/* ================ Device Specific Peripheral Section ================ */
-/* ================================================================================ */
-
-/* ------------------- Start of section using anonymous unions ------------------ */
-#if defined(__CC_ARM)
- #pragma push
- #pragma anon_unions
-#elif defined(__ICCARM__)
- #pragma language=extended
-#elif defined(__GNUC__)
- /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
-/* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
- #pragma warning 586
-#else
- #warning Not supported compiler type
-#endif
-
-
-
-/* ================================================================================ */
-/* ================ CPU FPGA System (CPU_SYS) ================ */
-/* ================================================================================ */
-typedef struct
-{
- __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
- __IO uint32_t MEMCFG; /* Offset: 0x004 (R/W) Remap and Alias Memory Control */
- __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
- __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
- __I uint32_t TS; /* Offset: 0x010 (R/ ) Touchscreen Register */
- __IO uint32_t CTRL1; /* Offset: 0x014 (R/W) Misc Control Functions */
- uint32_t RESERVED0[2];
- __IO uint32_t CLKCFG; /* Offset: 0x020 (R/W) System Clock Configuration */
- __IO uint32_t WSCFG; /* Offset: 0x024 (R/W) Flash Waitstate Configuration */
- __IO uint32_t CPUCFG; /* Offset: 0x028 (R/W) Processor Configuration */
- uint32_t RESERVED1[3];
- __IO uint32_t BASE; /* Offset: 0x038 (R/W) ROM Table base Address */
- __IO uint32_t ID2; /* Offset: 0x03C (R/W) Secondary Identification Register */
-} ARM_CPU_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================ DUT FPGA System (DUT_SYS) ================ */
-/* ================================================================================ */
-typedef struct
-{
- __I uint32_t ID; /* Offset: 0x000 (R/ ) Board and FPGA Identifier */
- __IO uint32_t PERCFG; /* Offset: 0x004 (R/W) Peripheral Control Signals */
- __I uint32_t SW; /* Offset: 0x008 (R/ ) Switch States */
- __IO uint32_t LED; /* Offset: 0x00C (R/W) LED Output States */
- __IO uint32_t SEG7; /* Offset: 0x010 (R/W) 7-segment LED Output States */
- __I uint32_t CNT25MHz; /* Offset: 0x014 (R/ ) Freerunning counter incrementing at 25MHz */
- __I uint32_t CNT100Hz; /* Offset: 0x018 (R/ ) Freerunning counter incrementing at 100Hz */
-} ARM_DUT_SYS_TypeDef;
-
-
-/* ================================================================================ */
-/* ================ Timer (TIM) ================ */
-/* ================================================================================ */
-typedef struct
-{
- __IO uint32_t Timer1Load; /* Offset: 0x000 (R/W) Timer 1 Load */
- __I uint32_t Timer1Value; /* Offset: 0x004 (R/ ) Timer 1 Counter Current Value */
- __IO uint32_t Timer1Control; /* Offset: 0x008 (R/W) Timer 1 Control */
- __O uint32_t Timer1IntClr; /* Offset: 0x00C ( /W) Timer 1 Interrupt Clear */
- __I uint32_t Timer1RIS; /* Offset: 0x010 (R/ ) Timer 1 Raw Interrupt Status */
- __I uint32_t Timer1MIS; /* Offset: 0x014 (R/ ) Timer 1 Masked Interrupt Status */
- __IO uint32_t Timer1BGLoad; /* Offset: 0x018 (R/W) Background Load Register */
- uint32_t RESERVED0[1];
- __IO uint32_t Timer2Load; /* Offset: 0x020 (R/W) Timer 2 Load */
- __I uint32_t Timer2Value; /* Offset: 0x024 (R/ ) Timer 2 Counter Current Value */
- __IO uint32_t Timer2Control; /* Offset: 0x028 (R/W) Timer 2 Control */
- __O uint32_t Timer2IntClr; /* Offset: 0x02C ( /W) Timer 2 Interrupt Clear */
- __I uint32_t Timer2RIS; /* Offset: 0x030 (R/ ) Timer 2 Raw Interrupt Status */
- __I uint32_t Timer2MIS; /* Offset: 0x034 (R/ ) Timer 2 Masked Interrupt Status */
- __IO uint32_t Timer2BGLoad; /* Offset: 0x038 (R/W) Background Load Register */
-} ARM_TIM_TypeDef;
-
-
-/* ================================================================================ */
-/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
-/* ================================================================================ */
-typedef struct
-{
- __IO uint32_t DR; /* Offset: 0x000 (R/W) Data */
- union {
- __I uint32_t RSR; /* Offset: 0x000 (R/ ) Receive Status */
- __O uint32_t ECR; /* Offset: 0x000 ( /W) Error Clear */
- };
- uint32_t RESERVED0[4];
- __IO uint32_t FR; /* Offset: 0x018 (R/W) Flags */
- uint32_t RESERVED1[1];
- __IO uint32_t ILPR; /* Offset: 0x020 (R/W) IrDA Low-power Counter */
- __IO uint32_t IBRD; /* Offset: 0x024 (R/W) Interger Baud Rate */
- __IO uint32_t FBRD; /* Offset: 0x028 (R/W) Fractional Baud Rate */
- __IO uint32_t LCR_H; /* Offset: 0x02C (R/W) Line Control */
- __IO uint32_t CR; /* Offset: 0x030 (R/W) Control */
- __IO uint32_t IFLS; /* Offset: 0x034 (R/W) Interrupt FIFO Level Select */
- __IO uint32_t IMSC; /* Offset: 0x038 (R/W) Interrupt Mask Set / Clear */
- __IO uint32_t RIS; /* Offset: 0x03C (R/W) Raw Interrupt Status */
- __IO uint32_t MIS; /* Offset: 0x040 (R/W) Masked Interrupt Status */
- __O uint32_t ICR; /* Offset: 0x044 ( /W) Interrupt Clear */
- __IO uint32_t DMACR; /* Offset: 0x048 (R/W) DMA Control */
-} ARM_UART_TypeDef;
-
-
-/* -------------------- End of section using anonymous unions ------------------- */
-#if defined(__CC_ARM)
- #pragma pop
-#elif defined(__ICCARM__)
- /* leave anonymous unions enabled */
-#elif defined(__GNUC__)
- /* anonymous unions are enabled by default */
-#elif defined(__TMS470__)
- /* anonymous unions are enabled by default */
-#elif defined(__TASKING__)
- #pragma warning restore
-#else
- #warning Not supported compiler type
-#endif
-
-
-
-
-/* ================================================================================ */
-/* ================ Peripheral memory map ================ */
-/* ================================================================================ */
-/* -------------------------- CPU FPGA memory map ------------------------------- */
-#define ARM_FLASH_BASE (0x00000000UL)
-#define ARM_RAM_BASE (0x20000000UL)
-#define ARM_RAM_FPGA_BASE (0x1EFF0000UL)
-#define ARM_CPU_CFG_BASE (0xDFFF0000UL)
-
-#define ARM_CPU_SYS_BASE (ARM_CPU_CFG_BASE + 0x00000)
-#define ARM_UART3_BASE (ARM_CPU_CFG_BASE + 0x05000)
-
-/* -------------------------- DUT FPGA memory map ------------------------------- */
-#define ARM_APB_BASE (0x40000000UL)
-#define ARM_AHB_BASE (0x4FF00000UL)
-#define ARM_DMC_BASE (0x60000000UL)
-#define ARM_SMC_BASE (0xA0000000UL)
-
-#define ARM_TIM0_BASE (ARM_APB_BASE + 0x02000)
-#define ARM_TIM2_BASE (ARM_APB_BASE + 0x03000)
-#define ARM_DUT_SYS_BASE (ARM_APB_BASE + 0x04000)
-#define ARM_UART0_BASE (ARM_APB_BASE + 0x06000)
-#define ARM_UART1_BASE (ARM_APB_BASE + 0x07000)
-#define ARM_UART2_BASE (ARM_APB_BASE + 0x08000)
-#define ARM_UART4_BASE (ARM_APB_BASE + 0x09000)
-
-
-/* ================================================================================ */
-/* ================ Peripheral declaration ================ */
-/* ================================================================================ */
-/* -------------------------- CPU FPGA Peripherals ------------------------------ */
-#define ARM_CPU_SYS ((ARM_CPU_SYS_TypeDef *) ARM_CPU_SYS_BASE)
-#define ARM_UART3 (( ARM_UART_TypeDef *) ARM_UART3_BASE)
-
-/* -------------------------- DUT FPGA Peripherals ------------------------------ */
-#define ARM_DUT_SYS ((ARM_DUT_SYS_TypeDef *) ARM_DUT_SYS_BASE)
-#define ARM_TIM0 (( ARM_TIM_TypeDef *) ARM_TIM0_BASE)
-#define ARM_TIM2 (( ARM_TIM_TypeDef *) ARM_TIM2_BASE)
-#define ARM_UART0 (( ARM_UART_TypeDef *) ARM_UART0_BASE)
-#define ARM_UART1 (( ARM_UART_TypeDef *) ARM_UART1_BASE)
-#define ARM_UART2 (( ARM_UART_TypeDef *) ARM_UART2_BASE)
-#define ARM_UART4 (( ARM_UART_TypeDef *) ARM_UART4_BASE)
-
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* ARMCM4_H */
diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h deleted file mode 100644 index 9c37ab4e5..000000000 --- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h +++ /dev/null @@ -1,93 +0,0 @@ -/* ----------------------------------------------------------------------
-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
-*
-* $Date: 17. January 2013
-* $Revision: V1.4.1
-*
-* Project: CMSIS DSP Library
-* Title: arm_common_tables.h
-*
-* Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions
-*
-* Target Processor: Cortex-M4/Cortex-M3
-*
-* 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 ARM LIMITED 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 OWNER 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 _ARM_COMMON_TABLES_H
-#define _ARM_COMMON_TABLES_H
-
-#include "arm_math.h"
-
-extern const uint16_t armBitRevTable[1024];
-extern const q15_t armRecipTableQ15[64];
-extern const q31_t armRecipTableQ31[64];
-extern const q31_t realCoefAQ31[1024];
-extern const q31_t realCoefBQ31[1024];
-extern const float32_t twiddleCoef_16[32];
-extern const float32_t twiddleCoef_32[64];
-extern const float32_t twiddleCoef_64[128];
-extern const float32_t twiddleCoef_128[256];
-extern const float32_t twiddleCoef_256[512];
-extern const float32_t twiddleCoef_512[1024];
-extern const float32_t twiddleCoef_1024[2048];
-extern const float32_t twiddleCoef_2048[4096];
-extern const float32_t twiddleCoef_4096[8192];
-#define twiddleCoef twiddleCoef_4096
-extern const q31_t twiddleCoefQ31[6144];
-extern const q15_t twiddleCoefQ15[6144];
-extern const float32_t twiddleCoef_rfft_32[32];
-extern const float32_t twiddleCoef_rfft_64[64];
-extern const float32_t twiddleCoef_rfft_128[128];
-extern const float32_t twiddleCoef_rfft_256[256];
-extern const float32_t twiddleCoef_rfft_512[512];
-extern const float32_t twiddleCoef_rfft_1024[1024];
-extern const float32_t twiddleCoef_rfft_2048[2048];
-extern const float32_t twiddleCoef_rfft_4096[4096];
-
-
-#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20 )
-#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48 )
-#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56 )
-#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 )
-#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 )
-#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 )
-#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800)
-#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808)
-#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032)
-
-extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH];
-extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH];
-
-#endif /* ARM_COMMON_TABLES_H */
diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/modules/mathlib/CMSIS/Include/arm_const_structs.h deleted file mode 100644 index 406f737dc..000000000 --- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h +++ /dev/null @@ -1,85 +0,0 @@ -/* ----------------------------------------------------------------------
-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
-*
-* $Date: 17. January 2013
-* $Revision: V1.4.1
-*
-* Project: CMSIS DSP Library
-* Title: arm_const_structs.h
-*
-* Description: This file has constant structs that are initialized for
-* user convenience. For example, some can be given as
-* arguments to the arm_cfft_f32() function.
-*
-* Target Processor: Cortex-M4/Cortex-M3
-*
-* 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 ARM LIMITED 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 OWNER 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 _ARM_CONST_STRUCTS_H
-#define _ARM_CONST_STRUCTS_H
-
-#include "arm_math.h"
-#include "arm_common_tables.h"
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = {
- 16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = {
- 32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = {
- 64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = {
- 128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = {
- 256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = {
- 512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = {
- 1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = {
- 2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH
- };
-
- const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = {
- 4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH
- };
-
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/modules/mathlib/CMSIS/Include/arm_math.h deleted file mode 100644 index 6f66f9ee3..000000000 --- a/src/modules/mathlib/CMSIS/Include/arm_math.h +++ /dev/null @@ -1,7318 +0,0 @@ -/* ----------------------------------------------------------------------
-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
-*
-* $Date: 17. January 2013
-* $Revision: V1.4.1
-*
-* Project: CMSIS DSP Library
-* Title: arm_math.h
-*
-* Description: Public header file for CMSIS DSP Library
-*
-* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
-*
-* 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 ARM LIMITED 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 OWNER 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.
- * -------------------------------------------------------------------- */
-
-/**
- \mainpage CMSIS DSP Software Library
- *
- * <b>Introduction</b>
- *
- * This user manual describes the CMSIS DSP software library,
- * a suite of common signal processing functions for use on Cortex-M processor based devices.
- *
- * The library is divided into a number of functions each covering a specific category:
- * - Basic math functions
- * - Fast math functions
- * - Complex math functions
- * - Filters
- * - Matrix functions
- * - Transforms
- * - Motor control functions
- * - Statistical functions
- * - Support functions
- * - Interpolation functions
- *
- * The library has separate functions for operating on 8-bit integers, 16-bit integers,
- * 32-bit integer and 32-bit floating-point values.
- *
- * <b>Using the Library</b>
- *
- * The library installer contains prebuilt versions of the libraries in the <code>Lib</code> folder.
- * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4)
- * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4)
- * - arm_cortexM4l_math.lib (Little endian on Cortex-M4)
- * - arm_cortexM4b_math.lib (Big endian on Cortex-M4)
- * - arm_cortexM3l_math.lib (Little endian on Cortex-M3)
- * - arm_cortexM3b_math.lib (Big endian on Cortex-M3)
- * - arm_cortexM0l_math.lib (Little endian on Cortex-M0)
- * - arm_cortexM0b_math.lib (Big endian on Cortex-M3)
- *
- * The library functions are declared in the public file <code>arm_math.h</code> which is placed in the <code>Include</code> folder.
- * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single
- * public header file <code> arm_math.h</code> for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants.
- * Define the appropriate pre processor MACRO ARM_MATH_CM4 or ARM_MATH_CM3 or
- * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application.
- *
- * <b>Examples</b>
- *
- * The library ships with a number of examples which demonstrate how to use the library functions.
- *
- * <b>Toolchain Support</b>
- *
- * The library has been developed and tested with MDK-ARM version 4.60.
- * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly.
- *
- * <b>Building the Library</b>
- *
- * The library installer contains project files to re build libraries on MDK Tool chain in the <code>CMSIS\\DSP_Lib\\Source\\ARM</code> folder.
- * - arm_cortexM0b_math.uvproj
- * - arm_cortexM0l_math.uvproj
- * - arm_cortexM3b_math.uvproj
- * - arm_cortexM3l_math.uvproj
- * - arm_cortexM4b_math.uvproj
- * - arm_cortexM4l_math.uvproj
- * - arm_cortexM4bf_math.uvproj
- * - arm_cortexM4lf_math.uvproj
- *
- *
- * The project can be built by opening the appropriate project in MDK-ARM 4.60 chain and defining the optional pre processor MACROs detailed above.
- *
- * <b>Pre-processor Macros</b>
- *
- * Each library project have differant pre-processor macros.
- *
- * - UNALIGNED_SUPPORT_DISABLE:
- *
- * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access
- *
- * - ARM_MATH_BIG_ENDIAN:
- *
- * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets.
- *
- * - ARM_MATH_MATRIX_CHECK:
- *
- * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices
- *
- * - ARM_MATH_ROUNDING:
- *
- * Define macro ARM_MATH_ROUNDING for rounding on support functions
- *
- * - ARM_MATH_CMx:
- *
- * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target
- * and ARM_MATH_CM0 for building library on cortex-M0 target, ARM_MATH_CM0PLUS for building library on cortex-M0+ target.
- *
- * - __FPU_PRESENT:
- *
- * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries
- *
- * <b>Copyright Notice</b>
- *
- * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
- */
-
-
-/**
- * @defgroup groupMath Basic Math Functions
- */
-
-/**
- * @defgroup groupFastMath Fast Math Functions
- * This set of functions provides a fast approximation to sine, cosine, and square root.
- * As compared to most of the other functions in the CMSIS math library, the fast math functions
- * operate on individual values and not arrays.
- * There are separate functions for Q15, Q31, and floating-point data.
- *
- */
-
-/**
- * @defgroup groupCmplxMath Complex Math Functions
- * This set of functions operates on complex data vectors.
- * The data in the complex arrays is stored in an interleaved fashion
- * (real, imag, real, imag, ...).
- * In the API functions, the number of samples in a complex array refers
- * to the number of complex values; the array contains twice this number of
- * real values.
- */
-
-/**
- * @defgroup groupFilters Filtering Functions
- */
-
-/**
- * @defgroup groupMatrix Matrix Functions
- *
- * This set of functions provides basic matrix math operations.
- * The functions operate on matrix data structures. For example,
- * the type
- * definition for the floating-point matrix structure is shown
- * below:
- * <pre>
- * typedef struct
- * {
- * uint16_t numRows; // number of rows of the matrix.
- * uint16_t numCols; // number of columns of the matrix.
- * float32_t *pData; // points to the data of the matrix.
- * } arm_matrix_instance_f32;
- * </pre>
- * There are similar definitions for Q15 and Q31 data types.
- *
- * The structure specifies the size of the matrix and then points to
- * an array of data. The array is of size <code>numRows X numCols</code>
- * and the values are arranged in row order. That is, the
- * matrix element (i, j) is stored at:
- * <pre>
- * pData[i*numCols + j]
- * </pre>
- *
- * \par Init Functions
- * There is an associated initialization function for each type of matrix
- * data structure.
- * The initialization function sets the values of the internal structure fields.
- * Refer to the function <code>arm_mat_init_f32()</code>, <code>arm_mat_init_q31()</code>
- * and <code>arm_mat_init_q15()</code> for floating-point, Q31 and Q15 types, respectively.
- *
- * \par
- * Use of the initialization function is optional. However, if initialization function is used
- * then the instance structure cannot be placed into a const data section.
- * To place the instance structure in a const data
- * section, manually initialize the data structure. For example:
- * <pre>
- * <code>arm_matrix_instance_f32 S = {nRows, nColumns, pData};</code>
- * <code>arm_matrix_instance_q31 S = {nRows, nColumns, pData};</code>
- * <code>arm_matrix_instance_q15 S = {nRows, nColumns, pData};</code>
- * </pre>
- * where <code>nRows</code> specifies the number of rows, <code>nColumns</code>
- * specifies the number of columns, and <code>pData</code> points to the
- * data array.
- *
- * \par Size Checking
- * By default all of the matrix functions perform size checking on the input and
- * output matrices. For example, the matrix addition function verifies that the
- * two input matrices and the output matrix all have the same number of rows and
- * columns. If the size check fails the functions return:
- * <pre>
- * ARM_MATH_SIZE_MISMATCH
- * </pre>
- * Otherwise the functions return
- * <pre>
- * ARM_MATH_SUCCESS
- * </pre>
- * There is some overhead associated with this matrix size checking.
- * The matrix size checking is enabled via the \#define
- * <pre>
- * ARM_MATH_MATRIX_CHECK
- * </pre>
- * within the library project settings. By default this macro is defined
- * and size checking is enabled. By changing the project settings and
- * undefining this macro size checking is eliminated and the functions
- * run a bit faster. With size checking disabled the functions always
- * return <code>ARM_MATH_SUCCESS</code>.
- */
-
-/**
- * @defgroup groupTransforms Transform Functions
- */
-
-/**
- * @defgroup groupController Controller Functions
- */
-
-/**
- * @defgroup groupStats Statistics Functions
- */
-/**
- * @defgroup groupSupport Support Functions
- */
-
-/**
- * @defgroup groupInterpolation Interpolation Functions
- * These functions perform 1- and 2-dimensional interpolation of data.
- * Linear interpolation is used for 1-dimensional data and
- * bilinear interpolation is used for 2-dimensional data.
- */
-
-/**
- * @defgroup groupExamples Examples
- */
-#ifndef _ARM_MATH_H
-#define _ARM_MATH_H
-
-#define __CMSIS_GENERIC /* disable NVIC and Systick functions */
-
-/* PX4 */
-#include <nuttx/config.h>
-#ifdef CONFIG_ARCH_CORTEXM4
-# define ARM_MATH_CM4 1
-#endif
-#ifdef CONFIG_ARCH_CORTEXM3
-# define ARM_MATH_CM3 1
-#endif
-#ifdef CONFIG_ARCH_FPU
-# define __FPU_PRESENT 1
-#endif
-
-#if defined (ARM_MATH_CM4)
-#include "core_cm4.h"
-#elif defined (ARM_MATH_CM3)
-#include "core_cm3.h"
-#elif defined (ARM_MATH_CM0)
-#include "core_cm0.h"
-#define ARM_MATH_CM0_FAMILY
-#elif defined (ARM_MATH_CM0PLUS)
-#include "core_cm0plus.h"
-#define ARM_MATH_CM0_FAMILY
-#else
-#include "ARMCM4.h"
-#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....."
-#endif
-
-#undef __CMSIS_GENERIC /* enable NVIC and Systick functions */
-#include "string.h"
-#include "math.h"
-#ifdef __cplusplus
-extern "C"
-{
-#endif
-
-
- /**
- * @brief Macros required for reciprocal calculation in Normalized LMS
- */
-
-#define DELTA_Q31 (0x100)
-#define DELTA_Q15 0x5
-#define INDEX_MASK 0x0000003F
-#ifndef PI
-#define PI 3.14159265358979f
-#endif
-
- /**
- * @brief Macros required for SINE and COSINE Fast math approximations
- */
-
-#define TABLE_SIZE 256
-#define TABLE_SPACING_Q31 0x800000
-#define TABLE_SPACING_Q15 0x80
-
- /**
- * @brief Macros required for SINE and COSINE Controller functions
- */
- /* 1.31(q31) Fixed value of 2/360 */
- /* -1 to +1 is divided into 360 values so total spacing is (2/360) */
-#define INPUT_SPACING 0xB60B61
-
- /**
- * @brief Macro for Unaligned Support
- */
-#ifndef UNALIGNED_SUPPORT_DISABLE
- #define ALIGN4
-#else
- #if defined (__GNUC__)
- #define ALIGN4 __attribute__((aligned(4)))
- #else
- #define ALIGN4 __align(4)
- #endif
-#endif /* #ifndef UNALIGNED_SUPPORT_DISABLE */
-
- /**
- * @brief Error status returned by some functions in the library.
- */
-
- typedef enum
- {
- ARM_MATH_SUCCESS = 0, /**< No error */
- ARM_MATH_ARGUMENT_ERROR = -1, /**< One or more arguments are incorrect */
- ARM_MATH_LENGTH_ERROR = -2, /**< Length of data buffer is incorrect */
- ARM_MATH_SIZE_MISMATCH = -3, /**< Size of matrices is not compatible with the operation. */
- ARM_MATH_NANINF = -4, /**< Not-a-number (NaN) or infinity is generated */
- ARM_MATH_SINGULAR = -5, /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */
- ARM_MATH_TEST_FAILURE = -6 /**< Test Failed */
- } arm_status;
-
- /**
- * @brief 8-bit fractional data type in 1.7 format.
- */
- typedef int8_t q7_t;
-
- /**
- * @brief 16-bit fractional data type in 1.15 format.
- */
- typedef int16_t q15_t;
-
- /**
- * @brief 32-bit fractional data type in 1.31 format.
- */
- typedef int32_t q31_t;
-
- /**
- * @brief 64-bit fractional data type in 1.63 format.
- */
- typedef int64_t q63_t;
-
- /**
- * @brief 32-bit floating-point type definition.
- */
- typedef float float32_t;
-
- /**
- * @brief 64-bit floating-point type definition.
- */
- typedef double float64_t;
-
- /**
- * @brief definition to read/write two 16 bit values.
- */
-#if defined __CC_ARM
-#define __SIMD32_TYPE int32_t __packed
-#define CMSIS_UNUSED __attribute__((unused))
-#elif defined __ICCARM__
-#define CMSIS_UNUSED
-#define __SIMD32_TYPE int32_t __packed
-#elif defined __GNUC__
-#define __SIMD32_TYPE int32_t
-#define CMSIS_UNUSED __attribute__((unused))
-#else
-#error Unknown compiler
-#endif
-
-#define __SIMD32(addr) (*(__SIMD32_TYPE **) & (addr))
-#define __SIMD32_CONST(addr) ((__SIMD32_TYPE *)(addr))
-
-#define _SIMD32_OFFSET(addr) (*(__SIMD32_TYPE *) (addr))
-
-#define __SIMD64(addr) (*(int64_t **) & (addr))
-
-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
- /**
- * @brief definition to pack two 16 bit values.
- */
-#define __PKHBT(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0x0000FFFF) | \
- (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000) )
-#define __PKHTB(ARG1, ARG2, ARG3) ( (((int32_t)(ARG1) << 0) & (int32_t)0xFFFF0000) | \
- (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF) )
-
-#endif
-
-
- /**
- * @brief definition to pack four 8 bit values.
- */
-#ifndef ARM_MATH_BIG_ENDIAN
-
-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) << 0) & (int32_t)0x000000FF) | \
- (((int32_t)(v1) << 8) & (int32_t)0x0000FF00) | \
- (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) | \
- (((int32_t)(v3) << 24) & (int32_t)0xFF000000) )
-#else
-
-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) << 0) & (int32_t)0x000000FF) | \
- (((int32_t)(v2) << 8) & (int32_t)0x0000FF00) | \
- (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) | \
- (((int32_t)(v0) << 24) & (int32_t)0xFF000000) )
-
-#endif
-
-
- /**
- * @brief Clips Q63 to Q31 values.
- */
- static __INLINE q31_t clip_q63_to_q31(
- q63_t x)
- {
- return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
- ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x;
- }
-
- /**
- * @brief Clips Q63 to Q15 values.
- */
- static __INLINE q15_t clip_q63_to_q15(
- q63_t x)
- {
- return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?
- ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);
- }
-
- /**
- * @brief Clips Q31 to Q7 values.
- */
- static __INLINE q7_t clip_q31_to_q7(
- q31_t x)
- {
- return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
- ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
- }
-
- /**
- * @brief Clips Q31 to Q15 values.
- */
- static __INLINE q15_t clip_q31_to_q15(
- q31_t x)
- {
- return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
- ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
- }
-
- /**
- * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
- */
-
- static __INLINE q63_t mult32x64(
- q63_t x,
- q31_t y)
- {
- return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
- (((q63_t) (x >> 32) * y)));
- }
-
-
-#if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM )
-#define __CLZ __clz
-#endif
-
-#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )
-
- static __INLINE uint32_t __CLZ(
- q31_t data);
-
-
- static __INLINE uint32_t __CLZ(
- q31_t data)
- {
- uint32_t count = 0;
- uint32_t mask = 0x80000000;
-
- while((data & mask) == 0)
- {
- count += 1u;
- mask = mask >> 1u;
- }
-
- return (count);
-
- }
-
-#endif
-
- /**
- * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type.
- */
-
- static __INLINE uint32_t arm_recip_q31(
- q31_t in,
- q31_t * dst,
- q31_t * pRecipTable)
- {
-
- uint32_t out, tempVal;
- uint32_t index, i;
- uint32_t signBits;
-
- if(in > 0)
- {
- signBits = __CLZ(in) - 1;
- }
- else
- {
- signBits = __CLZ(-in) - 1;
- }
-
- /* Convert input sample to 1.31 format */
- in = in << signBits;
-
- /* calculation of index for initial approximated Val */
- index = (uint32_t) (in >> 24u);
- index = (index & INDEX_MASK);
-
- /* 1.31 with exp 1 */
- out = pRecipTable[index];
-
- /* calculation of reciprocal value */
- /* running approximation for two iterations */
- for (i = 0u; i < 2u; i++)
- {
- tempVal = (q31_t) (((q63_t) in * out) >> 31u);
- tempVal = 0x7FFFFFFF - tempVal;
- /* 1.31 with exp 1 */
- //out = (q31_t) (((q63_t) out * tempVal) >> 30u);
- out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);
- }
-
- /* write output */
- *dst = out;
-
- /* return num of signbits of out = 1/in value */
- return (signBits + 1u);
-
- }
-
- /**
- * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type.
- */
- static __INLINE uint32_t arm_recip_q15(
- q15_t in,
- q15_t * dst,
- q15_t * pRecipTable)
- {
-
- uint32_t out = 0, tempVal = 0;
- uint32_t index = 0, i = 0;
- uint32_t signBits = 0;
-
- if(in > 0)
- {
- signBits = __CLZ(in) - 17;
- }
- else
- {
- signBits = __CLZ(-in) - 17;
- }
-
- /* Convert input sample to 1.15 format */
- in = in << signBits;
-
- /* calculation of index for initial approximated Val */
- index = in >> 8;
- index = (index & INDEX_MASK);
-
- /* 1.15 with exp 1 */
- out = pRecipTable[index];
-
- /* calculation of reciprocal value */
- /* running approximation for two iterations */
- for (i = 0; i < 2; i++)
- {
- tempVal = (q15_t) (((q31_t) in * out) >> 15);
- tempVal = 0x7FFF - tempVal;
- /* 1.15 with exp 1 */
- out = (q15_t) (((q31_t) out * tempVal) >> 14);
- }
-
- /* write output */
- *dst = out;
-
- /* return num of signbits of out = 1/in value */
- return (signBits + 1);
-
- }
-
-
- /*
- * @brief C custom defined intrinisic function for only M0 processors
- */
-#if defined(ARM_MATH_CM0_FAMILY)
-
- static __INLINE q31_t __SSAT(
- q31_t x,
- uint32_t y)
- {
- int32_t posMax, negMin;
- uint32_t i;
-
- posMax = 1;
- for (i = 0; i < (y - 1); i++)
- {
- posMax = posMax * 2;
- }
-
- if(x > 0)
- {
- posMax = (posMax - 1);
-
- if(x > posMax)
- {
- x = posMax;
- }
- }
- else
- {
- negMin = -posMax;
-
- if(x < negMin)
- {
- x = negMin;
- }
- }
- return (x);
-
-
- }
-
-#endif /* end of ARM_MATH_CM0_FAMILY */
-
-
-
- /*
- * @brief C custom defined intrinsic function for M3 and M0 processors
- */
-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)
-
- /*
- * @brief C custom defined QADD8 for M3 and M0 processors
- */
- static __INLINE q31_t __QADD8(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q7_t r, s, t, u;
-
- r = (q7_t) x;
- s = (q7_t) y;
-
- r = __SSAT((q31_t) (r + s), 8);
- s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);
- t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);
- u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);
-
- sum =
- (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |
- (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);
-
- return sum;
-
- }
-
- /*
- * @brief C custom defined QSUB8 for M3 and M0 processors
- */
- static __INLINE q31_t __QSUB8(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s, t, u;
-
- r = (q7_t) x;
- s = (q7_t) y;
-
- r = __SSAT((r - s), 8);
- s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;
- t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;
- u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;
-
- sum =
- (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r &
- 0x000000FF);
-
- return sum;
- }
-
- /*
- * @brief C custom defined QADD16 for M3 and M0 processors
- */
-
- /*
- * @brief C custom defined QADD16 for M3 and M0 processors
- */
- static __INLINE q31_t __QADD16(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = __SSAT(r + s, 16);
- s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
-
- }
-
- /*
- * @brief C custom defined SHADD16 for M3 and M0 processors
- */
- static __INLINE q31_t __SHADD16(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) + (s >> 1));
- s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
-
- }
-
- /*
- * @brief C custom defined QSUB16 for M3 and M0 processors
- */
- static __INLINE q31_t __QSUB16(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = __SSAT(r - s, 16);
- s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
- }
-
- /*
- * @brief C custom defined SHSUB16 for M3 and M0 processors
- */
- static __INLINE q31_t __SHSUB16(
- q31_t x,
- q31_t y)
- {
-
- q31_t diff;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) - (s >> 1));
- s = (((x >> 17) - (y >> 17)) << 16);
-
- diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return diff;
- }
-
- /*
- * @brief C custom defined QASX for M3 and M0 processors
- */
- static __INLINE q31_t __QASX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum = 0;
-
- sum =
- ((sum +
- clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +
- clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));
-
- return sum;
- }
-
- /*
- * @brief C custom defined SHASX for M3 and M0 processors
- */
- static __INLINE q31_t __SHASX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) - (y >> 17));
- s = (((x >> 17) + (s >> 1)) << 16);
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
- }
-
-
- /*
- * @brief C custom defined QSAX for M3 and M0 processors
- */
- static __INLINE q31_t __QSAX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum = 0;
-
- sum =
- ((sum +
- clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) +
- clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16)));
-
- return sum;
- }
-
- /*
- * @brief C custom defined SHSAX for M3 and M0 processors
- */
- static __INLINE q31_t __SHSAX(
- q31_t x,
- q31_t y)
- {
-
- q31_t sum;
- q31_t r, s;
-
- r = (short) x;
- s = (short) y;
-
- r = ((r >> 1) + (y >> 17));
- s = (((x >> 17) - (s >> 1)) << 16);
-
- sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);
-
- return sum;
- }
-
- /*
- * @brief C custom defined SMUSDX for M3 and M0 processors
- */
- static __INLINE q31_t __SMUSDX(
- q31_t x,
- q31_t y)
- {
-
- return ((q31_t) (((short) x * (short) (y >> 16)) -
- ((short) (x >> 16) * (short) y)));
- }
-
- /*
- * @brief C custom defined SMUADX for M3 and M0 processors
- */
- static __INLINE q31_t __SMUADX(
- q31_t x,
- q31_t y)
- {
-
- return ((q31_t) (((short) x * (short) (y >> 16)) +
- ((short) (x >> 16) * (short) y)));
- }
-
- /*
- * @brief C custom defined QADD for M3 and M0 processors
- */
- static __INLINE q31_t __QADD(
- q31_t x,
- q31_t y)
- {
- return clip_q63_to_q31((q63_t) x + y);
- }
-
- /*
- * @brief C custom defined QSUB for M3 and M0 processors
- */
- static __INLINE q31_t __QSUB(
- q31_t x,
- q31_t y)
- {
- return clip_q63_to_q31((q63_t) x - y);
- }
-
- /*
- * @brief C custom defined SMLAD for M3 and M0 processors
- */
- static __INLINE q31_t __SMLAD(
- q31_t x,
- q31_t y,
- q31_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
- ((short) x * (short) y));
- }
-
- /*
- * @brief C custom defined SMLADX for M3 and M0 processors
- */
- static __INLINE q31_t __SMLADX(
- q31_t x,
- q31_t y,
- q31_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) (y)) +
- ((short) x * (short) (y >> 16)));
- }
-
- /*
- * @brief C custom defined SMLSDX for M3 and M0 processors
- */
- static __INLINE q31_t __SMLSDX(
- q31_t x,
- q31_t y,
- q31_t sum)
- {
-
- return (sum - ((short) (x >> 16) * (short) (y)) +
- ((short) x * (short) (y >> 16)));
- }
-
- /*
- * @brief C custom defined SMLALD for M3 and M0 processors
- */
- static __INLINE q63_t __SMLALD(
- q31_t x,
- q31_t y,
- q63_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) (y >> 16)) +
- ((short) x * (short) y));
- }
-
- /*
- * @brief C custom defined SMLALDX for M3 and M0 processors
- */
- static __INLINE q63_t __SMLALDX(
- q31_t x,
- q31_t y,
- q63_t sum)
- {
-
- return (sum + ((short) (x >> 16) * (short) y)) +
- ((short) x * (short) (y >> 16));
- }
-
- /*
- * @brief C custom defined SMUAD for M3 and M0 processors
- */
- static __INLINE q31_t __SMUAD(
- q31_t x,
- q31_t y)
- {
-
- return (((x >> 16) * (y >> 16)) +
- (((x << 16) >> 16) * ((y << 16) >> 16)));
- }
-
- /*
- * @brief C custom defined SMUSD for M3 and M0 processors
- */
- static __INLINE q31_t __SMUSD(
- q31_t x,
- q31_t y)
- {
-
- return (-((x >> 16) * (y >> 16)) +
- (((x << 16) >> 16) * ((y << 16) >> 16)));
- }
-
-
- /*
- * @brief C custom defined SXTB16 for M3 and M0 processors
- */
- static __INLINE q31_t __SXTB16(
- q31_t x)
- {
-
- return ((((x << 24) >> 24) & 0x0000FFFF) |
- (((x << 8) >> 8) & 0xFFFF0000));
- }
-
-
-#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */
-
-
- /**
- * @brief Instance structure for the Q7 FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- q7_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- } arm_fir_instance_q7;
-
- /**
- * @brief Instance structure for the Q15 FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- } arm_fir_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- } arm_fir_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of filter coefficients in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- } arm_fir_instance_f32;
-
-
- /**
- * @brief Processing function for the Q7 FIR filter.
- * @param[in] *S points to an instance of the Q7 FIR filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_q7(
- const arm_fir_instance_q7 * S,
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q7 FIR filter.
- * @param[in,out] *S points to an instance of the Q7 FIR structure.
- * @param[in] numTaps Number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed.
- * @return none
- */
- void arm_fir_init_q7(
- arm_fir_instance_q7 * S,
- uint16_t numTaps,
- q7_t * pCoeffs,
- q7_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q15 FIR filter.
- * @param[in] *S points to an instance of the Q15 FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_q15(
- const arm_fir_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q15 FIR filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_fast_q15(
- const arm_fir_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q15 FIR filter.
- * @param[in,out] *S points to an instance of the Q15 FIR filter structure.
- * @param[in] numTaps Number of filter coefficients in the filter. Must be even and greater than or equal to 4.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed at a time.
- * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if
- * <code>numTaps</code> is not a supported value.
- */
-
- arm_status arm_fir_init_q15(
- arm_fir_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR filter.
- * @param[in] *S points to an instance of the Q31 FIR filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_q31(
- const arm_fir_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q31 FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_fast_q31(
- const arm_fir_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 FIR filter.
- * @param[in,out] *S points to an instance of the Q31 FIR structure.
- * @param[in] numTaps Number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed at a time.
- * @return none.
- */
- void arm_fir_init_q31(
- arm_fir_instance_q31 * S,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the floating-point FIR filter.
- * @param[in] *S points to an instance of the floating-point FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_f32(
- const arm_fir_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point FIR filter.
- * @param[in,out] *S points to an instance of the floating-point FIR filter structure.
- * @param[in] numTaps Number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of samples that are processed at a time.
- * @return none.
- */
- void arm_fir_init_f32(
- arm_fir_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Instance structure for the Q15 Biquad cascade filter.
- */
- typedef struct
- {
- int8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- q15_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
- q15_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
- int8_t postShift; /**< Additional shift, in bits, applied to each output sample. */
-
- } arm_biquad_casd_df1_inst_q15;
-
-
- /**
- * @brief Instance structure for the Q31 Biquad cascade filter.
- */
- typedef struct
- {
- uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- q31_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
- q31_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
- uint8_t postShift; /**< Additional shift, in bits, applied to each output sample. */
-
- } arm_biquad_casd_df1_inst_q31;
-
- /**
- * @brief Instance structure for the floating-point Biquad cascade filter.
- */
- typedef struct
- {
- uint32_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- float32_t *pState; /**< Points to the array of state coefficients. The array is of length 4*numStages. */
- float32_t *pCoeffs; /**< Points to the array of coefficients. The array is of length 5*numStages. */
-
-
- } arm_biquad_casd_df1_inst_f32;
-
-
-
- /**
- * @brief Processing function for the Q15 Biquad cascade filter.
- * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_q15(
- const arm_biquad_casd_df1_inst_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q15 Biquad cascade filter.
- * @param[in,out] *S points to an instance of the Q15 Biquad cascade structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format
- * @return none
- */
-
- void arm_biquad_cascade_df1_init_q15(
- arm_biquad_casd_df1_inst_q15 * S,
- uint8_t numStages,
- q15_t * pCoeffs,
- q15_t * pState,
- int8_t postShift);
-
-
- /**
- * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q15 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_fast_q15(
- const arm_biquad_casd_df1_inst_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q31 Biquad cascade filter
- * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_q31(
- const arm_biquad_casd_df1_inst_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_fast_q31(
- const arm_biquad_casd_df1_inst_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 Biquad cascade filter.
- * @param[in,out] *S points to an instance of the Q31 Biquad cascade structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] postShift Shift to be applied to the output. Varies according to the coefficients format
- * @return none
- */
-
- void arm_biquad_cascade_df1_init_q31(
- arm_biquad_casd_df1_inst_q31 * S,
- uint8_t numStages,
- q31_t * pCoeffs,
- q31_t * pState,
- int8_t postShift);
-
- /**
- * @brief Processing function for the floating-point Biquad cascade filter.
- * @param[in] *S points to an instance of the floating-point Biquad cascade structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df1_f32(
- const arm_biquad_casd_df1_inst_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point Biquad cascade filter.
- * @param[in,out] *S points to an instance of the floating-point Biquad cascade structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @return none
- */
-
- void arm_biquad_cascade_df1_init_f32(
- arm_biquad_casd_df1_inst_f32 * S,
- uint8_t numStages,
- float32_t * pCoeffs,
- float32_t * pState);
-
-
- /**
- * @brief Instance structure for the floating-point matrix structure.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows of the matrix. */
- uint16_t numCols; /**< number of columns of the matrix. */
- float32_t *pData; /**< points to the data of the matrix. */
- } arm_matrix_instance_f32;
-
- /**
- * @brief Instance structure for the Q15 matrix structure.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows of the matrix. */
- uint16_t numCols; /**< number of columns of the matrix. */
- q15_t *pData; /**< points to the data of the matrix. */
-
- } arm_matrix_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 matrix structure.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows of the matrix. */
- uint16_t numCols; /**< number of columns of the matrix. */
- q31_t *pData; /**< points to the data of the matrix. */
-
- } arm_matrix_instance_q31;
-
-
-
- /**
- * @brief Floating-point matrix addition.
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_add_f32(
- const arm_matrix_instance_f32 * pSrcA,
- const arm_matrix_instance_f32 * pSrcB,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix addition.
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_add_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix addition.
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_add_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Floating-point matrix transpose.
- * @param[in] *pSrc points to the input matrix
- * @param[out] *pDst points to the output matrix
- * @return The function returns either <code>ARM_MATH_SIZE_MISMATCH</code>
- * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_trans_f32(
- const arm_matrix_instance_f32 * pSrc,
- arm_matrix_instance_f32 * pDst);
-
-
- /**
- * @brief Q15 matrix transpose.
- * @param[in] *pSrc points to the input matrix
- * @param[out] *pDst points to the output matrix
- * @return The function returns either <code>ARM_MATH_SIZE_MISMATCH</code>
- * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_trans_q15(
- const arm_matrix_instance_q15 * pSrc,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix transpose.
- * @param[in] *pSrc points to the input matrix
- * @param[out] *pDst points to the output matrix
- * @return The function returns either <code>ARM_MATH_SIZE_MISMATCH</code>
- * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_trans_q31(
- const arm_matrix_instance_q31 * pSrc,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Floating-point matrix multiplication
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_f32(
- const arm_matrix_instance_f32 * pSrcA,
- const arm_matrix_instance_f32 * pSrcB,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix multiplication
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @param[in] *pState points to the array for storing intermediate results
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst,
- q15_t * pState);
-
- /**
- * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @param[in] *pState points to the array for storing intermediate results
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_fast_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst,
- q15_t * pState);
-
- /**
- * @brief Q31 matrix multiplication
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
- /**
- * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_mult_fast_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Floating-point matrix subtraction
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_sub_f32(
- const arm_matrix_instance_f32 * pSrcA,
- const arm_matrix_instance_f32 * pSrcB,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix subtraction
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_sub_q15(
- const arm_matrix_instance_q15 * pSrcA,
- const arm_matrix_instance_q15 * pSrcB,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix subtraction
- * @param[in] *pSrcA points to the first input matrix structure
- * @param[in] *pSrcB points to the second input matrix structure
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_sub_q31(
- const arm_matrix_instance_q31 * pSrcA,
- const arm_matrix_instance_q31 * pSrcB,
- arm_matrix_instance_q31 * pDst);
-
- /**
- * @brief Floating-point matrix scaling.
- * @param[in] *pSrc points to the input matrix
- * @param[in] scale scale factor
- * @param[out] *pDst points to the output matrix
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_scale_f32(
- const arm_matrix_instance_f32 * pSrc,
- float32_t scale,
- arm_matrix_instance_f32 * pDst);
-
- /**
- * @brief Q15 matrix scaling.
- * @param[in] *pSrc points to input matrix
- * @param[in] scaleFract fractional portion of the scale factor
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to output matrix
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_scale_q15(
- const arm_matrix_instance_q15 * pSrc,
- q15_t scaleFract,
- int32_t shift,
- arm_matrix_instance_q15 * pDst);
-
- /**
- * @brief Q31 matrix scaling.
- * @param[in] *pSrc points to input matrix
- * @param[in] scaleFract fractional portion of the scale factor
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to output matrix structure
- * @return The function returns either
- * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.
- */
-
- arm_status arm_mat_scale_q31(
- const arm_matrix_instance_q31 * pSrc,
- q31_t scaleFract,
- int32_t shift,
- arm_matrix_instance_q31 * pDst);
-
-
- /**
- * @brief Q31 matrix initialization.
- * @param[in,out] *S points to an instance of the floating-point matrix structure.
- * @param[in] nRows number of rows in the matrix.
- * @param[in] nColumns number of columns in the matrix.
- * @param[in] *pData points to the matrix data array.
- * @return none
- */
-
- void arm_mat_init_q31(
- arm_matrix_instance_q31 * S,
- uint16_t nRows,
- uint16_t nColumns,
- q31_t * pData);
-
- /**
- * @brief Q15 matrix initialization.
- * @param[in,out] *S points to an instance of the floating-point matrix structure.
- * @param[in] nRows number of rows in the matrix.
- * @param[in] nColumns number of columns in the matrix.
- * @param[in] *pData points to the matrix data array.
- * @return none
- */
-
- void arm_mat_init_q15(
- arm_matrix_instance_q15 * S,
- uint16_t nRows,
- uint16_t nColumns,
- q15_t * pData);
-
- /**
- * @brief Floating-point matrix initialization.
- * @param[in,out] *S points to an instance of the floating-point matrix structure.
- * @param[in] nRows number of rows in the matrix.
- * @param[in] nColumns number of columns in the matrix.
- * @param[in] *pData points to the matrix data array.
- * @return none
- */
-
- void arm_mat_init_f32(
- arm_matrix_instance_f32 * S,
- uint16_t nRows,
- uint16_t nColumns,
- float32_t * pData);
-
-
-
- /**
- * @brief Instance structure for the Q15 PID Control.
- */
- typedef struct
- {
- q15_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
-#ifdef ARM_MATH_CM0_FAMILY
- q15_t A1;
- q15_t A2;
-#else
- q31_t A1; /**< The derived gain A1 = -Kp - 2Kd | Kd.*/
-#endif
- q15_t state[3]; /**< The state array of length 3. */
- q15_t Kp; /**< The proportional gain. */
- q15_t Ki; /**< The integral gain. */
- q15_t Kd; /**< The derivative gain. */
- } arm_pid_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 PID Control.
- */
- typedef struct
- {
- q31_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
- q31_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
- q31_t A2; /**< The derived gain, A2 = Kd . */
- q31_t state[3]; /**< The state array of length 3. */
- q31_t Kp; /**< The proportional gain. */
- q31_t Ki; /**< The integral gain. */
- q31_t Kd; /**< The derivative gain. */
-
- } arm_pid_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point PID Control.
- */
- typedef struct
- {
- float32_t A0; /**< The derived gain, A0 = Kp + Ki + Kd . */
- float32_t A1; /**< The derived gain, A1 = -Kp - 2Kd. */
- float32_t A2; /**< The derived gain, A2 = Kd . */
- float32_t state[3]; /**< The state array of length 3. */
- float32_t Kp; /**< The proportional gain. */
- float32_t Ki; /**< The integral gain. */
- float32_t Kd; /**< The derivative gain. */
- } arm_pid_instance_f32;
-
-
-
- /**
- * @brief Initialization function for the floating-point PID Control.
- * @param[in,out] *S points to an instance of the PID structure.
- * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
- * @return none.
- */
- void arm_pid_init_f32(
- arm_pid_instance_f32 * S,
- int32_t resetStateFlag);
-
- /**
- * @brief Reset function for the floating-point PID Control.
- * @param[in,out] *S is an instance of the floating-point PID Control structure
- * @return none
- */
- void arm_pid_reset_f32(
- arm_pid_instance_f32 * S);
-
-
- /**
- * @brief Initialization function for the Q31 PID Control.
- * @param[in,out] *S points to an instance of the Q15 PID structure.
- * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
- * @return none.
- */
- void arm_pid_init_q31(
- arm_pid_instance_q31 * S,
- int32_t resetStateFlag);
-
-
- /**
- * @brief Reset function for the Q31 PID Control.
- * @param[in,out] *S points to an instance of the Q31 PID Control structure
- * @return none
- */
-
- void arm_pid_reset_q31(
- arm_pid_instance_q31 * S);
-
- /**
- * @brief Initialization function for the Q15 PID Control.
- * @param[in,out] *S points to an instance of the Q15 PID structure.
- * @param[in] resetStateFlag flag to reset the state. 0 = no change in state 1 = reset the state.
- * @return none.
- */
- void arm_pid_init_q15(
- arm_pid_instance_q15 * S,
- int32_t resetStateFlag);
-
- /**
- * @brief Reset function for the Q15 PID Control.
- * @param[in,out] *S points to an instance of the q15 PID Control structure
- * @return none
- */
- void arm_pid_reset_q15(
- arm_pid_instance_q15 * S);
-
-
- /**
- * @brief Instance structure for the floating-point Linear Interpolate function.
- */
- typedef struct
- {
- uint32_t nValues; /**< nValues */
- float32_t x1; /**< x1 */
- float32_t xSpacing; /**< xSpacing */
- float32_t *pYData; /**< pointer to the table of Y values */
- } arm_linear_interp_instance_f32;
-
- /**
- * @brief Instance structure for the floating-point bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- float32_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_f32;
-
- /**
- * @brief Instance structure for the Q31 bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- q31_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_q31;
-
- /**
- * @brief Instance structure for the Q15 bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- q15_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_q15;
-
- /**
- * @brief Instance structure for the Q15 bilinear interpolation function.
- */
-
- typedef struct
- {
- uint16_t numRows; /**< number of rows in the data table. */
- uint16_t numCols; /**< number of columns in the data table. */
- q7_t *pData; /**< points to the data table. */
- } arm_bilinear_interp_instance_q7;
-
-
- /**
- * @brief Q7 vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Floating-point vector multiplication.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_mult_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- float32_t * pDst,
- uint32_t blockSize);
-
-
-
-
-
-
- /**
- * @brief Instance structure for the Q15 CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- q15_t *pTwiddle; /**< points to the Sin twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- } arm_cfft_radix2_instance_q15;
-
- arm_status arm_cfft_radix2_init_q15(
- arm_cfft_radix2_instance_q15 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
- void arm_cfft_radix2_q15(
- const arm_cfft_radix2_instance_q15 * S,
- q15_t * pSrc);
-
-
-
- /**
- * @brief Instance structure for the Q15 CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- q15_t *pTwiddle; /**< points to the twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- } arm_cfft_radix4_instance_q15;
-
- arm_status arm_cfft_radix4_init_q15(
- arm_cfft_radix4_instance_q15 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
- void arm_cfft_radix4_q15(
- const arm_cfft_radix4_instance_q15 * S,
- q15_t * pSrc);
-
- /**
- * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- q31_t *pTwiddle; /**< points to the Twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- } arm_cfft_radix2_instance_q31;
-
- arm_status arm_cfft_radix2_init_q31(
- arm_cfft_radix2_instance_q31 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
- void arm_cfft_radix2_q31(
- const arm_cfft_radix2_instance_q31 * S,
- q31_t * pSrc);
-
- /**
- * @brief Instance structure for the Q31 CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- q31_t *pTwiddle; /**< points to the twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- } arm_cfft_radix4_instance_q31;
-
-
- void arm_cfft_radix4_q31(
- const arm_cfft_radix4_instance_q31 * S,
- q31_t * pSrc);
-
- arm_status arm_cfft_radix4_init_q31(
- arm_cfft_radix4_instance_q31 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
- /**
- * @brief Instance structure for the floating-point CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- float32_t *pTwiddle; /**< points to the Twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- float32_t onebyfftLen; /**< value of 1/fftLen. */
- } arm_cfft_radix2_instance_f32;
-
-/* Deprecated */
- arm_status arm_cfft_radix2_init_f32(
- arm_cfft_radix2_instance_f32 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
-/* Deprecated */
- void arm_cfft_radix2_f32(
- const arm_cfft_radix2_instance_f32 * S,
- float32_t * pSrc);
-
- /**
- * @brief Instance structure for the floating-point CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- uint8_t ifftFlag; /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */
- uint8_t bitReverseFlag; /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */
- float32_t *pTwiddle; /**< points to the Twiddle factor table. */
- uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t twidCoefModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- uint16_t bitRevFactor; /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */
- float32_t onebyfftLen; /**< value of 1/fftLen. */
- } arm_cfft_radix4_instance_f32;
-
-/* Deprecated */
- arm_status arm_cfft_radix4_init_f32(
- arm_cfft_radix4_instance_f32 * S,
- uint16_t fftLen,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
-/* Deprecated */
- void arm_cfft_radix4_f32(
- const arm_cfft_radix4_instance_f32 * S,
- float32_t * pSrc);
-
- /**
- * @brief Instance structure for the floating-point CFFT/CIFFT function.
- */
-
- typedef struct
- {
- uint16_t fftLen; /**< length of the FFT. */
- const float32_t *pTwiddle; /**< points to the Twiddle factor table. */
- const uint16_t *pBitRevTable; /**< points to the bit reversal table. */
- uint16_t bitRevLength; /**< bit reversal table length. */
- } arm_cfft_instance_f32;
-
- void arm_cfft_f32(
- const arm_cfft_instance_f32 * S,
- float32_t * p1,
- uint8_t ifftFlag,
- uint8_t bitReverseFlag);
-
- /**
- * @brief Instance structure for the Q15 RFFT/RIFFT function.
- */
-
- typedef struct
- {
- uint32_t fftLenReal; /**< length of the real FFT. */
- uint32_t fftLenBy2; /**< length of the complex FFT. */
- uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
- uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
- uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- q15_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
- q15_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
- arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
- } arm_rfft_instance_q15;
-
- arm_status arm_rfft_init_q15(
- arm_rfft_instance_q15 * S,
- arm_cfft_radix4_instance_q15 * S_CFFT,
- uint32_t fftLenReal,
- uint32_t ifftFlagR,
- uint32_t bitReverseFlag);
-
- void arm_rfft_q15(
- const arm_rfft_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst);
-
- /**
- * @brief Instance structure for the Q31 RFFT/RIFFT function.
- */
-
- typedef struct
- {
- uint32_t fftLenReal; /**< length of the real FFT. */
- uint32_t fftLenBy2; /**< length of the complex FFT. */
- uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
- uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
- uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- q31_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
- q31_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
- arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
- } arm_rfft_instance_q31;
-
- arm_status arm_rfft_init_q31(
- arm_rfft_instance_q31 * S,
- arm_cfft_radix4_instance_q31 * S_CFFT,
- uint32_t fftLenReal,
- uint32_t ifftFlagR,
- uint32_t bitReverseFlag);
-
- void arm_rfft_q31(
- const arm_rfft_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst);
-
- /**
- * @brief Instance structure for the floating-point RFFT/RIFFT function.
- */
-
- typedef struct
- {
- uint32_t fftLenReal; /**< length of the real FFT. */
- uint16_t fftLenBy2; /**< length of the complex FFT. */
- uint8_t ifftFlagR; /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */
- uint8_t bitReverseFlagR; /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */
- uint32_t twidCoefRModifier; /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */
- float32_t *pTwiddleAReal; /**< points to the real twiddle factor table. */
- float32_t *pTwiddleBReal; /**< points to the imag twiddle factor table. */
- arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
- } arm_rfft_instance_f32;
-
- arm_status arm_rfft_init_f32(
- arm_rfft_instance_f32 * S,
- arm_cfft_radix4_instance_f32 * S_CFFT,
- uint32_t fftLenReal,
- uint32_t ifftFlagR,
- uint32_t bitReverseFlag);
-
- void arm_rfft_f32(
- const arm_rfft_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst);
-
- /**
- * @brief Instance structure for the floating-point RFFT/RIFFT function.
- */
-
-typedef struct
- {
- arm_cfft_instance_f32 Sint; /**< Internal CFFT structure. */
- uint16_t fftLenRFFT; /**< length of the real sequence */
- float32_t * pTwiddleRFFT; /**< Twiddle factors real stage */
- } arm_rfft_fast_instance_f32 ;
-
-arm_status arm_rfft_fast_init_f32 (
- arm_rfft_fast_instance_f32 * S,
- uint16_t fftLen);
-
-void arm_rfft_fast_f32(
- arm_rfft_fast_instance_f32 * S,
- float32_t * p, float32_t * pOut,
- uint8_t ifftFlag);
-
- /**
- * @brief Instance structure for the floating-point DCT4/IDCT4 function.
- */
-
- typedef struct
- {
- uint16_t N; /**< length of the DCT4. */
- uint16_t Nby2; /**< half of the length of the DCT4. */
- float32_t normalize; /**< normalizing factor. */
- float32_t *pTwiddle; /**< points to the twiddle factor table. */
- float32_t *pCosFactor; /**< points to the cosFactor table. */
- arm_rfft_instance_f32 *pRfft; /**< points to the real FFT instance. */
- arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */
- } arm_dct4_instance_f32;
-
- /**
- * @brief Initialization function for the floating-point DCT4/IDCT4.
- * @param[in,out] *S points to an instance of floating-point DCT4/IDCT4 structure.
- * @param[in] *S_RFFT points to an instance of floating-point RFFT/RIFFT structure.
- * @param[in] *S_CFFT points to an instance of floating-point CFFT/CIFFT structure.
- * @param[in] N length of the DCT4.
- * @param[in] Nby2 half of the length of the DCT4.
- * @param[in] normalize normalizing factor.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>fftLenReal</code> is not a supported transform length.
- */
-
- arm_status arm_dct4_init_f32(
- arm_dct4_instance_f32 * S,
- arm_rfft_instance_f32 * S_RFFT,
- arm_cfft_radix4_instance_f32 * S_CFFT,
- uint16_t N,
- uint16_t Nby2,
- float32_t normalize);
-
- /**
- * @brief Processing function for the floating-point DCT4/IDCT4.
- * @param[in] *S points to an instance of the floating-point DCT4/IDCT4 structure.
- * @param[in] *pState points to state buffer.
- * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
- * @return none.
- */
-
- void arm_dct4_f32(
- const arm_dct4_instance_f32 * S,
- float32_t * pState,
- float32_t * pInlineBuffer);
-
- /**
- * @brief Instance structure for the Q31 DCT4/IDCT4 function.
- */
-
- typedef struct
- {
- uint16_t N; /**< length of the DCT4. */
- uint16_t Nby2; /**< half of the length of the DCT4. */
- q31_t normalize; /**< normalizing factor. */
- q31_t *pTwiddle; /**< points to the twiddle factor table. */
- q31_t *pCosFactor; /**< points to the cosFactor table. */
- arm_rfft_instance_q31 *pRfft; /**< points to the real FFT instance. */
- arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */
- } arm_dct4_instance_q31;
-
- /**
- * @brief Initialization function for the Q31 DCT4/IDCT4.
- * @param[in,out] *S points to an instance of Q31 DCT4/IDCT4 structure.
- * @param[in] *S_RFFT points to an instance of Q31 RFFT/RIFFT structure
- * @param[in] *S_CFFT points to an instance of Q31 CFFT/CIFFT structure
- * @param[in] N length of the DCT4.
- * @param[in] Nby2 half of the length of the DCT4.
- * @param[in] normalize normalizing factor.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
- */
-
- arm_status arm_dct4_init_q31(
- arm_dct4_instance_q31 * S,
- arm_rfft_instance_q31 * S_RFFT,
- arm_cfft_radix4_instance_q31 * S_CFFT,
- uint16_t N,
- uint16_t Nby2,
- q31_t normalize);
-
- /**
- * @brief Processing function for the Q31 DCT4/IDCT4.
- * @param[in] *S points to an instance of the Q31 DCT4 structure.
- * @param[in] *pState points to state buffer.
- * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
- * @return none.
- */
-
- void arm_dct4_q31(
- const arm_dct4_instance_q31 * S,
- q31_t * pState,
- q31_t * pInlineBuffer);
-
- /**
- * @brief Instance structure for the Q15 DCT4/IDCT4 function.
- */
-
- typedef struct
- {
- uint16_t N; /**< length of the DCT4. */
- uint16_t Nby2; /**< half of the length of the DCT4. */
- q15_t normalize; /**< normalizing factor. */
- q15_t *pTwiddle; /**< points to the twiddle factor table. */
- q15_t *pCosFactor; /**< points to the cosFactor table. */
- arm_rfft_instance_q15 *pRfft; /**< points to the real FFT instance. */
- arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */
- } arm_dct4_instance_q15;
-
- /**
- * @brief Initialization function for the Q15 DCT4/IDCT4.
- * @param[in,out] *S points to an instance of Q15 DCT4/IDCT4 structure.
- * @param[in] *S_RFFT points to an instance of Q15 RFFT/RIFFT structure.
- * @param[in] *S_CFFT points to an instance of Q15 CFFT/CIFFT structure.
- * @param[in] N length of the DCT4.
- * @param[in] Nby2 half of the length of the DCT4.
- * @param[in] normalize normalizing factor.
- * @return arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.
- */
-
- arm_status arm_dct4_init_q15(
- arm_dct4_instance_q15 * S,
- arm_rfft_instance_q15 * S_RFFT,
- arm_cfft_radix4_instance_q15 * S_CFFT,
- uint16_t N,
- uint16_t Nby2,
- q15_t normalize);
-
- /**
- * @brief Processing function for the Q15 DCT4/IDCT4.
- * @param[in] *S points to an instance of the Q15 DCT4 structure.
- * @param[in] *pState points to state buffer.
- * @param[in,out] *pInlineBuffer points to the in-place input and output buffer.
- * @return none.
- */
-
- void arm_dct4_q15(
- const arm_dct4_instance_q15 * S,
- q15_t * pState,
- q15_t * pInlineBuffer);
-
- /**
- * @brief Floating-point vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q7 vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector addition.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_add_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Floating-point vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q7 vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector subtraction.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_sub_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a floating-point vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scale scale factor to be applied
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_f32(
- float32_t * pSrc,
- float32_t scale,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a Q7 vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scaleFract fractional portion of the scale value
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_q7(
- q7_t * pSrc,
- q7_t scaleFract,
- int8_t shift,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a Q15 vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scaleFract fractional portion of the scale value
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_q15(
- q15_t * pSrc,
- q15_t scaleFract,
- int8_t shift,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Multiplies a Q31 vector by a scalar.
- * @param[in] *pSrc points to the input vector
- * @param[in] scaleFract fractional portion of the scale value
- * @param[in] shift number of bits to shift the result by
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_scale_q31(
- q31_t * pSrc,
- q31_t scaleFract,
- int8_t shift,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q7 vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_q7(
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Floating-point vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q15 vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Q31 vector absolute value.
- * @param[in] *pSrc points to the input buffer
- * @param[out] *pDst points to the output buffer
- * @param[in] blockSize number of samples in each vector
- * @return none.
- */
-
- void arm_abs_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Dot product of floating-point vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- uint32_t blockSize,
- float32_t * result);
-
- /**
- * @brief Dot product of Q7 vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_q7(
- q7_t * pSrcA,
- q7_t * pSrcB,
- uint32_t blockSize,
- q31_t * result);
-
- /**
- * @brief Dot product of Q15 vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- uint32_t blockSize,
- q63_t * result);
-
- /**
- * @brief Dot product of Q31 vectors.
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] blockSize number of samples in each vector
- * @param[out] *result output result returned here
- * @return none.
- */
-
- void arm_dot_prod_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- uint32_t blockSize,
- q63_t * result);
-
- /**
- * @brief Shifts the elements of a Q7 vector a specified number of bits.
- * @param[in] *pSrc points to the input vector
- * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_shift_q7(
- q7_t * pSrc,
- int8_t shiftBits,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Shifts the elements of a Q15 vector a specified number of bits.
- * @param[in] *pSrc points to the input vector
- * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_shift_q15(
- q15_t * pSrc,
- int8_t shiftBits,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Shifts the elements of a Q31 vector a specified number of bits.
- * @param[in] *pSrc points to the input vector
- * @param[in] shiftBits number of bits to shift. A positive value shifts left; a negative value shifts right.
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_shift_q31(
- q31_t * pSrc,
- int8_t shiftBits,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a floating-point vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_f32(
- float32_t * pSrc,
- float32_t offset,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a Q7 vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_q7(
- q7_t * pSrc,
- q7_t offset,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a Q15 vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_q15(
- q15_t * pSrc,
- q15_t offset,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Adds a constant offset to a Q31 vector.
- * @param[in] *pSrc points to the input vector
- * @param[in] offset is the offset to be added
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_offset_q31(
- q31_t * pSrc,
- q31_t offset,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a floating-point vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a Q7 vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_q7(
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a Q15 vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Negates the elements of a Q31 vector.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] blockSize number of samples in the vector
- * @return none.
- */
-
- void arm_negate_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
- /**
- * @brief Copies the elements of a floating-point vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Copies the elements of a Q7 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_q7(
- q7_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Copies the elements of a Q15 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Copies the elements of a Q31 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_copy_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
- /**
- * @brief Fills a constant value into a floating-point vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_f32(
- float32_t value,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fills a constant value into a Q7 vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_q7(
- q7_t value,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fills a constant value into a Q15 vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_q15(
- q15_t value,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Fills a constant value into a Q31 vector.
- * @param[in] value input value to be filled
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_fill_q31(
- q31_t value,
- q31_t * pDst,
- uint32_t blockSize);
-
-/**
- * @brief Convolution of floating-point sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst);
-
-
- /**
- * @brief Convolution of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
- * @return none.
- */
-
-
- void arm_conv_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- q15_t * pScratch1,
- q15_t * pScratch2);
-
-
-/**
- * @brief Convolution of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the location where the output result is written. Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
- /**
- * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
- /**
- * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @param[in] *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @param[in] *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
- * @return none.
- */
-
- void arm_conv_fast_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- q15_t * pScratch1,
- q15_t * pScratch2);
-
-
-
- /**
- * @brief Convolution of Q31 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
- /**
- * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
-
- /**
- * @brief Convolution of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
- * @return none.
- */
-
- void arm_conv_opt_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst,
- q15_t * pScratch1,
- q15_t * pScratch2);
-
-
-
- /**
- * @brief Convolution of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length srcALen+srcBLen-1.
- * @return none.
- */
-
- void arm_conv_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst);
-
-
- /**
- * @brief Partial convolution of floating-point sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
- /**
- * @brief Partial convolution of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints,
- q15_t * pScratch1,
- q15_t * pScratch2);
-
-
-/**
- * @brief Partial convolution of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
- /**
- * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
-
- /**
- * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @param[in] * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @param[in] * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_fast_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints,
- q15_t * pScratch1,
- q15_t * pScratch2);
-
-
- /**
- * @brief Partial convolution of Q31 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
-
- /**
- * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
-
- /**
- * @brief Partial convolution of Q7 sequences
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_opt_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints,
- q15_t * pScratch1,
- q15_t * pScratch2);
-
-
-/**
- * @brief Partial convolution of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data
- * @param[in] firstIndex is the first output sample to start with.
- * @param[in] numPoints is the number of output points to be computed.
- * @return Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].
- */
-
- arm_status arm_conv_partial_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst,
- uint32_t firstIndex,
- uint32_t numPoints);
-
-
-
- /**
- * @brief Instance structure for the Q15 FIR decimator.
- */
-
- typedef struct
- {
- uint8_t M; /**< decimation factor. */
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- } arm_fir_decimate_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR decimator.
- */
-
- typedef struct
- {
- uint8_t M; /**< decimation factor. */
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-
- } arm_fir_decimate_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR decimator.
- */
-
- typedef struct
- {
- uint8_t M; /**< decimation factor. */
- uint16_t numTaps; /**< number of coefficients in the filter. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
-
- } arm_fir_decimate_instance_f32;
-
-
-
- /**
- * @brief Processing function for the floating-point FIR decimator.
- * @param[in] *S points to an instance of the floating-point FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_f32(
- const arm_fir_decimate_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the floating-point FIR decimator.
- * @param[in,out] *S points to an instance of the floating-point FIR decimator structure.
- * @param[in] numTaps number of coefficients in the filter.
- * @param[in] M decimation factor.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * <code>blockSize</code> is not a multiple of <code>M</code>.
- */
-
- arm_status arm_fir_decimate_init_f32(
- arm_fir_decimate_instance_f32 * S,
- uint16_t numTaps,
- uint8_t M,
- float32_t * pCoeffs,
- float32_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q15 FIR decimator.
- * @param[in] *S points to an instance of the Q15 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_q15(
- const arm_fir_decimate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q15 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_fast_q15(
- const arm_fir_decimate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
-
- /**
- * @brief Initialization function for the Q15 FIR decimator.
- * @param[in,out] *S points to an instance of the Q15 FIR decimator structure.
- * @param[in] numTaps number of coefficients in the filter.
- * @param[in] M decimation factor.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * <code>blockSize</code> is not a multiple of <code>M</code>.
- */
-
- arm_status arm_fir_decimate_init_q15(
- arm_fir_decimate_instance_q15 * S,
- uint16_t numTaps,
- uint8_t M,
- q15_t * pCoeffs,
- q15_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR decimator.
- * @param[in] *S points to an instance of the Q31 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_q31(
- const arm_fir_decimate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.
- * @param[in] *S points to an instance of the Q31 FIR decimator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of input samples to process per call.
- * @return none
- */
-
- void arm_fir_decimate_fast_q31(
- arm_fir_decimate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q31 FIR decimator.
- * @param[in,out] *S points to an instance of the Q31 FIR decimator structure.
- * @param[in] numTaps number of coefficients in the filter.
- * @param[in] M decimation factor.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * <code>blockSize</code> is not a multiple of <code>M</code>.
- */
-
- arm_status arm_fir_decimate_init_q31(
- arm_fir_decimate_instance_q31 * S,
- uint16_t numTaps,
- uint8_t M,
- q31_t * pCoeffs,
- q31_t * pState,
- uint32_t blockSize);
-
-
-
- /**
- * @brief Instance structure for the Q15 FIR interpolator.
- */
-
- typedef struct
- {
- uint8_t L; /**< upsample factor. */
- uint16_t phaseLength; /**< length of each polyphase filter component. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
- q15_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
- } arm_fir_interpolate_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR interpolator.
- */
-
- typedef struct
- {
- uint8_t L; /**< upsample factor. */
- uint16_t phaseLength; /**< length of each polyphase filter component. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
- q31_t *pState; /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */
- } arm_fir_interpolate_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR interpolator.
- */
-
- typedef struct
- {
- uint8_t L; /**< upsample factor. */
- uint16_t phaseLength; /**< length of each polyphase filter component. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length L*phaseLength. */
- float32_t *pState; /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */
- } arm_fir_interpolate_instance_f32;
-
-
- /**
- * @brief Processing function for the Q15 FIR interpolator.
- * @param[in] *S points to an instance of the Q15 FIR interpolator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_interpolate_q15(
- const arm_fir_interpolate_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q15 FIR interpolator.
- * @param[in,out] *S points to an instance of the Q15 FIR interpolator structure.
- * @param[in] L upsample factor.
- * @param[in] numTaps number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
- */
-
- arm_status arm_fir_interpolate_init_q15(
- arm_fir_interpolate_instance_q15 * S,
- uint8_t L,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 FIR interpolator.
- * @param[in] *S points to an instance of the Q15 FIR interpolator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_interpolate_q31(
- const arm_fir_interpolate_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 FIR interpolator.
- * @param[in,out] *S points to an instance of the Q31 FIR interpolator structure.
- * @param[in] L upsample factor.
- * @param[in] numTaps number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
- */
-
- arm_status arm_fir_interpolate_init_q31(
- arm_fir_interpolate_instance_q31 * S,
- uint8_t L,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the floating-point FIR interpolator.
- * @param[in] *S points to an instance of the floating-point FIR interpolator structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_interpolate_f32(
- const arm_fir_interpolate_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point FIR interpolator.
- * @param[in,out] *S points to an instance of the floating-point FIR interpolator structure.
- * @param[in] L upsample factor.
- * @param[in] numTaps number of filter coefficients in the filter.
- * @param[in] *pCoeffs points to the filter coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] blockSize number of input samples to process per call.
- * @return The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if
- * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.
- */
-
- arm_status arm_fir_interpolate_init_f32(
- arm_fir_interpolate_instance_f32 * S,
- uint8_t L,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the high precision Q31 Biquad cascade filter.
- */
-
- typedef struct
- {
- uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- q63_t *pState; /**< points to the array of state coefficients. The array is of length 4*numStages. */
- q31_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */
- uint8_t postShift; /**< additional shift, in bits, applied to each output sample. */
-
- } arm_biquad_cas_df1_32x64_ins_q31;
-
-
- /**
- * @param[in] *S points to an instance of the high precision Q31 Biquad cascade filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cas_df1_32x64_q31(
- const arm_biquad_cas_df1_32x64_ins_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @param[in,out] *S points to an instance of the high precision Q31 Biquad cascade filter structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] postShift shift to be applied to the output. Varies according to the coefficients format
- * @return none
- */
-
- void arm_biquad_cas_df1_32x64_init_q31(
- arm_biquad_cas_df1_32x64_ins_q31 * S,
- uint8_t numStages,
- q31_t * pCoeffs,
- q63_t * pState,
- uint8_t postShift);
-
-
-
- /**
- * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.
- */
-
- typedef struct
- {
- uint8_t numStages; /**< number of 2nd order stages in the filter. Overall order is 2*numStages. */
- float32_t *pState; /**< points to the array of state coefficients. The array is of length 2*numStages. */
- float32_t *pCoeffs; /**< points to the array of coefficients. The array is of length 5*numStages. */
- } arm_biquad_cascade_df2T_instance_f32;
-
-
- /**
- * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
- * @param[in] *S points to an instance of the filter data structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_biquad_cascade_df2T_f32(
- const arm_biquad_cascade_df2T_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the floating-point transposed direct form II Biquad cascade filter.
- * @param[in,out] *S points to an instance of the filter data structure.
- * @param[in] numStages number of 2nd order stages in the filter.
- * @param[in] *pCoeffs points to the filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @return none
- */
-
- void arm_biquad_cascade_df2T_init_f32(
- arm_biquad_cascade_df2T_instance_f32 * S,
- uint8_t numStages,
- float32_t * pCoeffs,
- float32_t * pState);
-
-
-
- /**
- * @brief Instance structure for the Q15 FIR lattice filter.
- */
-
- typedef struct
- {
- uint16_t numStages; /**< number of filter stages. */
- q15_t *pState; /**< points to the state variable array. The array is of length numStages. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
- } arm_fir_lattice_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 FIR lattice filter.
- */
-
- typedef struct
- {
- uint16_t numStages; /**< number of filter stages. */
- q31_t *pState; /**< points to the state variable array. The array is of length numStages. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
- } arm_fir_lattice_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point FIR lattice filter.
- */
-
- typedef struct
- {
- uint16_t numStages; /**< number of filter stages. */
- float32_t *pState; /**< points to the state variable array. The array is of length numStages. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numStages. */
- } arm_fir_lattice_instance_f32;
-
- /**
- * @brief Initialization function for the Q15 FIR lattice filter.
- * @param[in] *S points to an instance of the Q15 FIR lattice structure.
- * @param[in] numStages number of filter stages.
- * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
- * @param[in] *pState points to the state buffer. The array is of length numStages.
- * @return none.
- */
-
- void arm_fir_lattice_init_q15(
- arm_fir_lattice_instance_q15 * S,
- uint16_t numStages,
- q15_t * pCoeffs,
- q15_t * pState);
-
-
- /**
- * @brief Processing function for the Q15 FIR lattice filter.
- * @param[in] *S points to an instance of the Q15 FIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
- void arm_fir_lattice_q15(
- const arm_fir_lattice_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 FIR lattice filter.
- * @param[in] *S points to an instance of the Q31 FIR lattice structure.
- * @param[in] numStages number of filter stages.
- * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
- * @param[in] *pState points to the state buffer. The array is of length numStages.
- * @return none.
- */
-
- void arm_fir_lattice_init_q31(
- arm_fir_lattice_instance_q31 * S,
- uint16_t numStages,
- q31_t * pCoeffs,
- q31_t * pState);
-
-
- /**
- * @brief Processing function for the Q31 FIR lattice filter.
- * @param[in] *S points to an instance of the Q31 FIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_fir_lattice_q31(
- const arm_fir_lattice_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-/**
- * @brief Initialization function for the floating-point FIR lattice filter.
- * @param[in] *S points to an instance of the floating-point FIR lattice structure.
- * @param[in] numStages number of filter stages.
- * @param[in] *pCoeffs points to the coefficient buffer. The array is of length numStages.
- * @param[in] *pState points to the state buffer. The array is of length numStages.
- * @return none.
- */
-
- void arm_fir_lattice_init_f32(
- arm_fir_lattice_instance_f32 * S,
- uint16_t numStages,
- float32_t * pCoeffs,
- float32_t * pState);
-
- /**
- * @brief Processing function for the floating-point FIR lattice filter.
- * @param[in] *S points to an instance of the floating-point FIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_fir_lattice_f32(
- const arm_fir_lattice_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the Q15 IIR lattice filter.
- */
- typedef struct
- {
- uint16_t numStages; /**< number of stages in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
- q15_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
- q15_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
- } arm_iir_lattice_instance_q15;
-
- /**
- * @brief Instance structure for the Q31 IIR lattice filter.
- */
- typedef struct
- {
- uint16_t numStages; /**< number of stages in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
- q31_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
- q31_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
- } arm_iir_lattice_instance_q31;
-
- /**
- * @brief Instance structure for the floating-point IIR lattice filter.
- */
- typedef struct
- {
- uint16_t numStages; /**< number of stages in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numStages+blockSize. */
- float32_t *pkCoeffs; /**< points to the reflection coefficient array. The array is of length numStages. */
- float32_t *pvCoeffs; /**< points to the ladder coefficient array. The array is of length numStages+1. */
- } arm_iir_lattice_instance_f32;
-
- /**
- * @brief Processing function for the floating-point IIR lattice filter.
- * @param[in] *S points to an instance of the floating-point IIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_f32(
- const arm_iir_lattice_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point IIR lattice filter.
- * @param[in] *S points to an instance of the floating-point IIR lattice structure.
- * @param[in] numStages number of stages in the filter.
- * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages.
- * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1.
- * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize-1.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_init_f32(
- arm_iir_lattice_instance_f32 * S,
- uint16_t numStages,
- float32_t * pkCoeffs,
- float32_t * pvCoeffs,
- float32_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q31 IIR lattice filter.
- * @param[in] *S points to an instance of the Q31 IIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_q31(
- const arm_iir_lattice_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q31 IIR lattice filter.
- * @param[in] *S points to an instance of the Q31 IIR lattice structure.
- * @param[in] numStages number of stages in the filter.
- * @param[in] *pkCoeffs points to the reflection coefficient buffer. The array is of length numStages.
- * @param[in] *pvCoeffs points to the ladder coefficient buffer. The array is of length numStages+1.
- * @param[in] *pState points to the state buffer. The array is of length numStages+blockSize.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_init_q31(
- arm_iir_lattice_instance_q31 * S,
- uint16_t numStages,
- q31_t * pkCoeffs,
- q31_t * pvCoeffs,
- q31_t * pState,
- uint32_t blockSize);
-
-
- /**
- * @brief Processing function for the Q15 IIR lattice filter.
- * @param[in] *S points to an instance of the Q15 IIR lattice structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_iir_lattice_q15(
- const arm_iir_lattice_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
-/**
- * @brief Initialization function for the Q15 IIR lattice filter.
- * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure.
- * @param[in] numStages number of stages in the filter.
- * @param[in] *pkCoeffs points to reflection coefficient buffer. The array is of length numStages.
- * @param[in] *pvCoeffs points to ladder coefficient buffer. The array is of length numStages+1.
- * @param[in] *pState points to state buffer. The array is of length numStages+blockSize.
- * @param[in] blockSize number of samples to process per call.
- * @return none.
- */
-
- void arm_iir_lattice_init_q15(
- arm_iir_lattice_instance_q15 * S,
- uint16_t numStages,
- q15_t * pkCoeffs,
- q15_t * pvCoeffs,
- q15_t * pState,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the floating-point LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- float32_t mu; /**< step size that controls filter coefficient updates. */
- } arm_lms_instance_f32;
-
- /**
- * @brief Processing function for floating-point LMS filter.
- * @param[in] *S points to an instance of the floating-point LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_f32(
- const arm_lms_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pRef,
- float32_t * pOut,
- float32_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for floating-point LMS filter.
- * @param[in] *S points to an instance of the floating-point LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to the coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_init_f32(
- arm_lms_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- float32_t mu,
- uint32_t blockSize);
-
- /**
- * @brief Instance structure for the Q15 LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q15_t mu; /**< step size that controls filter coefficient updates. */
- uint32_t postShift; /**< bit shift applied to coefficients. */
- } arm_lms_instance_q15;
-
-
- /**
- * @brief Initialization function for the Q15 LMS filter.
- * @param[in] *S points to an instance of the Q15 LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to the coefficient buffer.
- * @param[in] *pState points to the state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_init_q15(
- arm_lms_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- q15_t mu,
- uint32_t blockSize,
- uint32_t postShift);
-
- /**
- * @brief Processing function for Q15 LMS filter.
- * @param[in] *S points to an instance of the Q15 LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_q15(
- const arm_lms_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pRef,
- q15_t * pOut,
- q15_t * pErr,
- uint32_t blockSize);
-
-
- /**
- * @brief Instance structure for the Q31 LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q31_t mu; /**< step size that controls filter coefficient updates. */
- uint32_t postShift; /**< bit shift applied to coefficients. */
-
- } arm_lms_instance_q31;
-
- /**
- * @brief Processing function for Q31 LMS filter.
- * @param[in] *S points to an instance of the Q15 LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_q31(
- const arm_lms_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pRef,
- q31_t * pOut,
- q31_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for Q31 LMS filter.
- * @param[in] *S points to an instance of the Q31 LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_init_q31(
- arm_lms_instance_q31 * S,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- q31_t mu,
- uint32_t blockSize,
- uint32_t postShift);
-
- /**
- * @brief Instance structure for the floating-point normalized LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- float32_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- float32_t mu; /**< step size that control filter coefficient updates. */
- float32_t energy; /**< saves previous frame energy. */
- float32_t x0; /**< saves previous input sample. */
- } arm_lms_norm_instance_f32;
-
- /**
- * @brief Processing function for floating-point normalized LMS filter.
- * @param[in] *S points to an instance of the floating-point normalized LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_f32(
- arm_lms_norm_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pRef,
- float32_t * pOut,
- float32_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for floating-point normalized LMS filter.
- * @param[in] *S points to an instance of the floating-point LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_init_f32(
- arm_lms_norm_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- float32_t mu,
- uint32_t blockSize);
-
-
- /**
- * @brief Instance structure for the Q31 normalized LMS filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- q31_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q31_t mu; /**< step size that controls filter coefficient updates. */
- uint8_t postShift; /**< bit shift applied to coefficients. */
- q31_t *recipTable; /**< points to the reciprocal initial value table. */
- q31_t energy; /**< saves previous frame energy. */
- q31_t x0; /**< saves previous input sample. */
- } arm_lms_norm_instance_q31;
-
- /**
- * @brief Processing function for Q31 normalized LMS filter.
- * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_q31(
- arm_lms_norm_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pRef,
- q31_t * pOut,
- q31_t * pErr,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for Q31 normalized LMS filter.
- * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_norm_init_q31(
- arm_lms_norm_instance_q31 * S,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- q31_t mu,
- uint32_t blockSize,
- uint8_t postShift);
-
- /**
- * @brief Instance structure for the Q15 normalized LMS filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< Number of coefficients in the filter. */
- q15_t *pState; /**< points to the state variable array. The array is of length numTaps+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps. */
- q15_t mu; /**< step size that controls filter coefficient updates. */
- uint8_t postShift; /**< bit shift applied to coefficients. */
- q15_t *recipTable; /**< Points to the reciprocal initial value table. */
- q15_t energy; /**< saves previous frame energy. */
- q15_t x0; /**< saves previous input sample. */
- } arm_lms_norm_instance_q15;
-
- /**
- * @brief Processing function for Q15 normalized LMS filter.
- * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[in] *pRef points to the block of reference data.
- * @param[out] *pOut points to the block of output data.
- * @param[out] *pErr points to the block of error data.
- * @param[in] blockSize number of samples to process.
- * @return none.
- */
-
- void arm_lms_norm_q15(
- arm_lms_norm_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pRef,
- q15_t * pOut,
- q15_t * pErr,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for Q15 normalized LMS filter.
- * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.
- * @param[in] numTaps number of filter coefficients.
- * @param[in] *pCoeffs points to coefficient buffer.
- * @param[in] *pState points to state buffer.
- * @param[in] mu step size that controls filter coefficient updates.
- * @param[in] blockSize number of samples to process.
- * @param[in] postShift bit shift applied to coefficients.
- * @return none.
- */
-
- void arm_lms_norm_init_q15(
- arm_lms_norm_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- q15_t mu,
- uint32_t blockSize,
- uint8_t postShift);
-
- /**
- * @brief Correlation of floating-point sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_f32(
- float32_t * pSrcA,
- uint32_t srcALen,
- float32_t * pSrcB,
- uint32_t srcBLen,
- float32_t * pDst);
-
-
- /**
- * @brief Correlation of Q15 sequences
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @return none.
- */
- void arm_correlate_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- q15_t * pScratch);
-
-
- /**
- * @brief Correlation of Q15 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
- /**
- * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_fast_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst);
-
-
-
- /**
- * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @param[in] *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @return none.
- */
-
- void arm_correlate_fast_opt_q15(
- q15_t * pSrcA,
- uint32_t srcALen,
- q15_t * pSrcB,
- uint32_t srcBLen,
- q15_t * pDst,
- q15_t * pScratch);
-
- /**
- * @brief Correlation of Q31 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
- /**
- * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_fast_q31(
- q31_t * pSrcA,
- uint32_t srcALen,
- q31_t * pSrcB,
- uint32_t srcBLen,
- q31_t * pDst);
-
-
-
- /**
- * @brief Correlation of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @param[in] *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.
- * @param[in] *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).
- * @return none.
- */
-
- void arm_correlate_opt_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst,
- q15_t * pScratch1,
- q15_t * pScratch2);
-
-
- /**
- * @brief Correlation of Q7 sequences.
- * @param[in] *pSrcA points to the first input sequence.
- * @param[in] srcALen length of the first input sequence.
- * @param[in] *pSrcB points to the second input sequence.
- * @param[in] srcBLen length of the second input sequence.
- * @param[out] *pDst points to the block of output data Length 2 * max(srcALen, srcBLen) - 1.
- * @return none.
- */
-
- void arm_correlate_q7(
- q7_t * pSrcA,
- uint32_t srcALen,
- q7_t * pSrcB,
- uint32_t srcBLen,
- q7_t * pDst);
-
-
- /**
- * @brief Instance structure for the floating-point sparse FIR filter.
- */
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- float32_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- float32_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_f32;
-
- /**
- * @brief Instance structure for the Q31 sparse FIR filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- q31_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- q31_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_q31;
-
- /**
- * @brief Instance structure for the Q15 sparse FIR filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- q15_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- q15_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_q15;
-
- /**
- * @brief Instance structure for the Q7 sparse FIR filter.
- */
-
- typedef struct
- {
- uint16_t numTaps; /**< number of coefficients in the filter. */
- uint16_t stateIndex; /**< state buffer index. Points to the oldest sample in the state buffer. */
- q7_t *pState; /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */
- q7_t *pCoeffs; /**< points to the coefficient array. The array is of length numTaps.*/
- uint16_t maxDelay; /**< maximum offset specified by the pTapDelay array. */
- int32_t *pTapDelay; /**< points to the array of delay values. The array is of length numTaps. */
- } arm_fir_sparse_instance_q7;
-
- /**
- * @brief Processing function for the floating-point sparse FIR filter.
- * @param[in] *S points to an instance of the floating-point sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_f32(
- arm_fir_sparse_instance_f32 * S,
- float32_t * pSrc,
- float32_t * pDst,
- float32_t * pScratchIn,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the floating-point sparse FIR filter.
- * @param[in,out] *S points to an instance of the floating-point sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_f32(
- arm_fir_sparse_instance_f32 * S,
- uint16_t numTaps,
- float32_t * pCoeffs,
- float32_t * pState,
- int32_t * pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q31 sparse FIR filter.
- * @param[in] *S points to an instance of the Q31 sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_q31(
- arm_fir_sparse_instance_q31 * S,
- q31_t * pSrc,
- q31_t * pDst,
- q31_t * pScratchIn,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q31 sparse FIR filter.
- * @param[in,out] *S points to an instance of the Q31 sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_q31(
- arm_fir_sparse_instance_q31 * S,
- uint16_t numTaps,
- q31_t * pCoeffs,
- q31_t * pState,
- int32_t * pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q15 sparse FIR filter.
- * @param[in] *S points to an instance of the Q15 sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] *pScratchOut points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_q15(
- arm_fir_sparse_instance_q15 * S,
- q15_t * pSrc,
- q15_t * pDst,
- q15_t * pScratchIn,
- q31_t * pScratchOut,
- uint32_t blockSize);
-
-
- /**
- * @brief Initialization function for the Q15 sparse FIR filter.
- * @param[in,out] *S points to an instance of the Q15 sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_q15(
- arm_fir_sparse_instance_q15 * S,
- uint16_t numTaps,
- q15_t * pCoeffs,
- q15_t * pState,
- int32_t * pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
- /**
- * @brief Processing function for the Q7 sparse FIR filter.
- * @param[in] *S points to an instance of the Q7 sparse FIR structure.
- * @param[in] *pSrc points to the block of input data.
- * @param[out] *pDst points to the block of output data
- * @param[in] *pScratchIn points to a temporary buffer of size blockSize.
- * @param[in] *pScratchOut points to a temporary buffer of size blockSize.
- * @param[in] blockSize number of input samples to process per call.
- * @return none.
- */
-
- void arm_fir_sparse_q7(
- arm_fir_sparse_instance_q7 * S,
- q7_t * pSrc,
- q7_t * pDst,
- q7_t * pScratchIn,
- q31_t * pScratchOut,
- uint32_t blockSize);
-
- /**
- * @brief Initialization function for the Q7 sparse FIR filter.
- * @param[in,out] *S points to an instance of the Q7 sparse FIR structure.
- * @param[in] numTaps number of nonzero coefficients in the filter.
- * @param[in] *pCoeffs points to the array of filter coefficients.
- * @param[in] *pState points to the state buffer.
- * @param[in] *pTapDelay points to the array of offset times.
- * @param[in] maxDelay maximum offset time supported.
- * @param[in] blockSize number of samples that will be processed per block.
- * @return none
- */
-
- void arm_fir_sparse_init_q7(
- arm_fir_sparse_instance_q7 * S,
- uint16_t numTaps,
- q7_t * pCoeffs,
- q7_t * pState,
- int32_t * pTapDelay,
- uint16_t maxDelay,
- uint32_t blockSize);
-
-
- /*
- * @brief Floating-point sin_cos function.
- * @param[in] theta input value in degrees
- * @param[out] *pSinVal points to the processed sine output.
- * @param[out] *pCosVal points to the processed cos output.
- * @return none.
- */
-
- void arm_sin_cos_f32(
- float32_t theta,
- float32_t * pSinVal,
- float32_t * pCcosVal);
-
- /*
- * @brief Q31 sin_cos function.
- * @param[in] theta scaled input value in degrees
- * @param[out] *pSinVal points to the processed sine output.
- * @param[out] *pCosVal points to the processed cosine output.
- * @return none.
- */
-
- void arm_sin_cos_q31(
- q31_t theta,
- q31_t * pSinVal,
- q31_t * pCosVal);
-
-
- /**
- * @brief Floating-point complex conjugate.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_conj_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q31 complex conjugate.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_conj_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q15 complex conjugate.
- * @param[in] *pSrc points to the input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_conj_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t numSamples);
-
-
-
- /**
- * @brief Floating-point complex magnitude squared
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_squared_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q31 complex magnitude squared
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_squared_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q15 complex magnitude squared
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_squared_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t numSamples);
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup PID PID Motor Control
- *
- * A Proportional Integral Derivative (PID) controller is a generic feedback control
- * loop mechanism widely used in industrial control systems.
- * A PID controller is the most commonly used type of feedback controller.
- *
- * This set of functions implements (PID) controllers
- * for Q15, Q31, and floating-point data types. The functions operate on a single sample
- * of data and each call to the function returns a single processed value.
- * <code>S</code> points to an instance of the PID control data structure. <code>in</code>
- * is the input sample value. The functions return the output value.
- *
- * \par Algorithm:
- * <pre>
- * y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]
- * A0 = Kp + Ki + Kd
- * A1 = (-Kp ) - (2 * Kd )
- * A2 = Kd </pre>
- *
- * \par
- * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant
- *
- * \par
- * \image html PID.gif "Proportional Integral Derivative Controller"
- *
- * \par
- * The PID controller calculates an "error" value as the difference between
- * the measured output and the reference input.
- * The controller attempts to minimize the error by adjusting the process control inputs.
- * The proportional value determines the reaction to the current error,
- * the integral value determines the reaction based on the sum of recent errors,
- * and the derivative value determines the reaction based on the rate at which the error has been changing.
- *
- * \par Instance Structure
- * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.
- * A separate instance structure must be defined for each PID Controller.
- * There are separate instance structure declarations for each of the 3 supported data types.
- *
- * \par Reset Functions
- * There is also an associated reset function for each data type which clears the state array.
- *
- * \par Initialization Functions
- * There is also an associated initialization function for each data type.
- * The initialization function performs the following operations:
- * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.
- * - Zeros out the values in the state buffer.
- *
- * \par
- * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.
- *
- * \par Fixed-Point Behavior
- * Care must be taken when using the fixed-point versions of the PID Controller functions.
- * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup PID
- * @{
- */
-
- /**
- * @brief Process function for the floating-point PID Control.
- * @param[in,out] *S is an instance of the floating-point PID Control structure
- * @param[in] in input sample to process
- * @return out processed output sample.
- */
-
-
- static __INLINE float32_t arm_pid_f32(
- arm_pid_instance_f32 * S,
- float32_t in)
- {
- float32_t out;
-
- /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */
- out = (S->A0 * in) +
- (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);
-
- /* Update state */
- S->state[1] = S->state[0];
- S->state[0] = in;
- S->state[2] = out;
-
- /* return to application */
- return (out);
-
- }
-
- /**
- * @brief Process function for the Q31 PID Control.
- * @param[in,out] *S points to an instance of the Q31 PID Control structure
- * @param[in] in input sample to process
- * @return out processed output sample.
- *
- * <b>Scaling and Overflow Behavior:</b>
- * \par
- * The function is implemented using an internal 64-bit accumulator.
- * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
- * Thus, if the accumulator result overflows it wraps around rather than clip.
- * In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions.
- * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.
- */
-
- static __INLINE q31_t arm_pid_q31(
- arm_pid_instance_q31 * S,
- q31_t in)
- {
- q63_t acc;
- q31_t out;
-
- /* acc = A0 * x[n] */
- acc = (q63_t) S->A0 * in;
-
- /* acc += A1 * x[n-1] */
- acc += (q63_t) S->A1 * S->state[0];
-
- /* acc += A2 * x[n-2] */
- acc += (q63_t) S->A2 * S->state[1];
-
- /* convert output to 1.31 format to add y[n-1] */
- out = (q31_t) (acc >> 31u);
-
- /* out += y[n-1] */
- out += S->state[2];
-
- /* Update state */
- S->state[1] = S->state[0];
- S->state[0] = in;
- S->state[2] = out;
-
- /* return to application */
- return (out);
-
- }
-
- /**
- * @brief Process function for the Q15 PID Control.
- * @param[in,out] *S points to an instance of the Q15 PID Control structure
- * @param[in] in input sample to process
- * @return out processed output sample.
- *
- * <b>Scaling and Overflow Behavior:</b>
- * \par
- * The function is implemented using a 64-bit internal accumulator.
- * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.
- * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.
- * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.
- * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.
- * Lastly, the accumulator is saturated to yield a result in 1.15 format.
- */
-
- static __INLINE q15_t arm_pid_q15(
- arm_pid_instance_q15 * S,
- q15_t in)
- {
- q63_t acc;
- q15_t out;
-
-#ifndef ARM_MATH_CM0_FAMILY
- __SIMD32_TYPE *vstate;
-
- /* Implementation of PID controller */
-
- /* acc = A0 * x[n] */
- acc = (q31_t) __SMUAD(S->A0, in);
-
- /* acc += A1 * x[n-1] + A2 * x[n-2] */
- vstate = __SIMD32_CONST(S->state);
- acc = __SMLALD(S->A1, (q31_t) *vstate, acc);
-
-#else
- /* acc = A0 * x[n] */
- acc = ((q31_t) S->A0) * in;
-
- /* acc += A1 * x[n-1] + A2 * x[n-2] */
- acc += (q31_t) S->A1 * S->state[0];
- acc += (q31_t) S->A2 * S->state[1];
-
-#endif
-
- /* acc += y[n-1] */
- acc += (q31_t) S->state[2] << 15;
-
- /* saturate the output */
- out = (q15_t) (__SSAT((acc >> 15), 16));
-
- /* Update state */
- S->state[1] = S->state[0];
- S->state[0] = in;
- S->state[2] = out;
-
- /* return to application */
- return (out);
-
- }
-
- /**
- * @} end of PID group
- */
-
-
- /**
- * @brief Floating-point matrix inverse.
- * @param[in] *src points to the instance of the input floating-point matrix structure.
- * @param[out] *dst points to the instance of the output floating-point matrix structure.
- * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.
- * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.
- */
-
- arm_status arm_mat_inverse_f32(
- const arm_matrix_instance_f32 * src,
- arm_matrix_instance_f32 * dst);
-
-
-
- /**
- * @ingroup groupController
- */
-
-
- /**
- * @defgroup clarke Vector Clarke Transform
- * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.
- * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents
- * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.
- * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below
- * \image html clarke.gif Stator current space vector and its components in (a,b).
- * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>
- * can be calculated using only <code>Ia</code> and <code>Ib</code>.
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html clarkeFormula.gif
- * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and
- * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Clarke transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup clarke
- * @{
- */
-
- /**
- *
- * @brief Floating-point Clarke transform
- * @param[in] Ia input three-phase coordinate <code>a</code>
- * @param[in] Ib input three-phase coordinate <code>b</code>
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @return none.
- */
-
- static __INLINE void arm_clarke_f32(
- float32_t Ia,
- float32_t Ib,
- float32_t * pIalpha,
- float32_t * pIbeta)
- {
- /* Calculate pIalpha using the equation, pIalpha = Ia */
- *pIalpha = Ia;
-
- /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */
- *pIbeta =
- ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib);
-
- }
-
- /**
- * @brief Clarke transform for Q31 version
- * @param[in] Ia input three-phase coordinate <code>a</code>
- * @param[in] Ib input three-phase coordinate <code>b</code>
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @return none.
- *
- * <b>Scaling and Overflow Behavior:</b>
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the addition, hence there is no risk of overflow.
- */
-
- static __INLINE void arm_clarke_q31(
- q31_t Ia,
- q31_t Ib,
- q31_t * pIalpha,
- q31_t * pIbeta)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
-
- /* Calculating pIalpha from Ia by equation pIalpha = Ia */
- *pIalpha = Ia;
-
- /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */
- product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);
-
- /* Intermediate product is calculated by (2/sqrt(3) * Ib) */
- product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);
-
- /* pIbeta is calculated by adding the intermediate products */
- *pIbeta = __QADD(product1, product2);
- }
-
- /**
- * @} end of clarke group
- */
-
- /**
- * @brief Converts the elements of the Q7 vector to Q31 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_q7_to_q31(
- q7_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup inv_clarke Vector Inverse Clarke Transform
- * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html clarkeInvFormula.gif
- * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and
- * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Clarke transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup inv_clarke
- * @{
- */
-
- /**
- * @brief Floating-point Inverse Clarke transform
- * @param[in] Ialpha input two-phase orthogonal vector axis alpha
- * @param[in] Ibeta input two-phase orthogonal vector axis beta
- * @param[out] *pIa points to output three-phase coordinate <code>a</code>
- * @param[out] *pIb points to output three-phase coordinate <code>b</code>
- * @return none.
- */
-
-
- static __INLINE void arm_inv_clarke_f32(
- float32_t Ialpha,
- float32_t Ibeta,
- float32_t * pIa,
- float32_t * pIb)
- {
- /* Calculating pIa from Ialpha by equation pIa = Ialpha */
- *pIa = Ialpha;
-
- /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */
- *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;
-
- }
-
- /**
- * @brief Inverse Clarke transform for Q31 version
- * @param[in] Ialpha input two-phase orthogonal vector axis alpha
- * @param[in] Ibeta input two-phase orthogonal vector axis beta
- * @param[out] *pIa points to output three-phase coordinate <code>a</code>
- * @param[out] *pIb points to output three-phase coordinate <code>b</code>
- * @return none.
- *
- * <b>Scaling and Overflow Behavior:</b>
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the subtraction, hence there is no risk of overflow.
- */
-
- static __INLINE void arm_inv_clarke_q31(
- q31_t Ialpha,
- q31_t Ibeta,
- q31_t * pIa,
- q31_t * pIb)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
-
- /* Calculating pIa from Ialpha by equation pIa = Ialpha */
- *pIa = Ialpha;
-
- /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */
- product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);
-
- /* Intermediate product is calculated by (1/sqrt(3) * pIb) */
- product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);
-
- /* pIb is calculated by subtracting the products */
- *pIb = __QSUB(product2, product1);
-
- }
-
- /**
- * @} end of inv_clarke group
- */
-
- /**
- * @brief Converts the elements of the Q7 vector to Q15 vector.
- * @param[in] *pSrc input pointer
- * @param[out] *pDst output pointer
- * @param[in] blockSize number of samples to process
- * @return none.
- */
- void arm_q7_to_q15(
- q7_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup park Vector Park Transform
- *
- * Forward Park transform converts the input two-coordinate vector to flux and torque components.
- * The Park transform can be used to realize the transformation of the <code>Ialpha</code> and the <code>Ibeta</code> currents
- * from the stationary to the moving reference frame and control the spatial relationship between
- * the stator vector current and rotor flux vector.
- * If we consider the d axis aligned with the rotor flux, the diagram below shows the
- * current vector and the relationship from the two reference frames:
- * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html parkFormula.gif
- * where <code>Ialpha</code> and <code>Ibeta</code> are the stator vector components,
- * <code>pId</code> and <code>pIq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
- * cosine and sine values of theta (rotor flux position).
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Park transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup park
- * @{
- */
-
- /**
- * @brief Floating-point Park transform
- * @param[in] Ialpha input two-phase vector coordinate alpha
- * @param[in] Ibeta input two-phase vector coordinate beta
- * @param[out] *pId points to output rotor reference frame d
- * @param[out] *pIq points to output rotor reference frame q
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- *
- * The function implements the forward Park transform.
- *
- */
-
- static __INLINE void arm_park_f32(
- float32_t Ialpha,
- float32_t Ibeta,
- float32_t * pId,
- float32_t * pIq,
- float32_t sinVal,
- float32_t cosVal)
- {
- /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */
- *pId = Ialpha * cosVal + Ibeta * sinVal;
-
- /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */
- *pIq = -Ialpha * sinVal + Ibeta * cosVal;
-
- }
-
- /**
- * @brief Park transform for Q31 version
- * @param[in] Ialpha input two-phase vector coordinate alpha
- * @param[in] Ibeta input two-phase vector coordinate beta
- * @param[out] *pId points to output rotor reference frame d
- * @param[out] *pIq points to output rotor reference frame q
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- *
- * <b>Scaling and Overflow Behavior:</b>
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the addition and subtraction, hence there is no risk of overflow.
- */
-
-
- static __INLINE void arm_park_q31(
- q31_t Ialpha,
- q31_t Ibeta,
- q31_t * pId,
- q31_t * pIq,
- q31_t sinVal,
- q31_t cosVal)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
- q31_t product3, product4; /* Temporary variables used to store intermediate results */
-
- /* Intermediate product is calculated by (Ialpha * cosVal) */
- product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);
-
- /* Intermediate product is calculated by (Ibeta * sinVal) */
- product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);
-
-
- /* Intermediate product is calculated by (Ialpha * sinVal) */
- product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);
-
- /* Intermediate product is calculated by (Ibeta * cosVal) */
- product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);
-
- /* Calculate pId by adding the two intermediate products 1 and 2 */
- *pId = __QADD(product1, product2);
-
- /* Calculate pIq by subtracting the two intermediate products 3 from 4 */
- *pIq = __QSUB(product4, product3);
- }
-
- /**
- * @} end of park group
- */
-
- /**
- * @brief Converts the elements of the Q7 vector to floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q7_to_float(
- q7_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @ingroup groupController
- */
-
- /**
- * @defgroup inv_park Vector Inverse Park transform
- * Inverse Park transform converts the input flux and torque components to two-coordinate vector.
- *
- * The function operates on a single sample of data and each call to the function returns the processed output.
- * The library provides separate functions for Q31 and floating-point data types.
- * \par Algorithm
- * \image html parkInvFormula.gif
- * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,
- * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the
- * cosine and sine values of theta (rotor flux position).
- * \par Fixed-Point Behavior
- * Care must be taken when using the Q31 version of the Park transform.
- * In particular, the overflow and saturation behavior of the accumulator used must be considered.
- * Refer to the function specific documentation below for usage guidelines.
- */
-
- /**
- * @addtogroup inv_park
- * @{
- */
-
- /**
- * @brief Floating-point Inverse Park transform
- * @param[in] Id input coordinate of rotor reference frame d
- * @param[in] Iq input coordinate of rotor reference frame q
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- */
-
- static __INLINE void arm_inv_park_f32(
- float32_t Id,
- float32_t Iq,
- float32_t * pIalpha,
- float32_t * pIbeta,
- float32_t sinVal,
- float32_t cosVal)
- {
- /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */
- *pIalpha = Id * cosVal - Iq * sinVal;
-
- /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */
- *pIbeta = Id * sinVal + Iq * cosVal;
-
- }
-
-
- /**
- * @brief Inverse Park transform for Q31 version
- * @param[in] Id input coordinate of rotor reference frame d
- * @param[in] Iq input coordinate of rotor reference frame q
- * @param[out] *pIalpha points to output two-phase orthogonal vector axis alpha
- * @param[out] *pIbeta points to output two-phase orthogonal vector axis beta
- * @param[in] sinVal sine value of rotation angle theta
- * @param[in] cosVal cosine value of rotation angle theta
- * @return none.
- *
- * <b>Scaling and Overflow Behavior:</b>
- * \par
- * The function is implemented using an internal 32-bit accumulator.
- * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.
- * There is saturation on the addition, hence there is no risk of overflow.
- */
-
-
- static __INLINE void arm_inv_park_q31(
- q31_t Id,
- q31_t Iq,
- q31_t * pIalpha,
- q31_t * pIbeta,
- q31_t sinVal,
- q31_t cosVal)
- {
- q31_t product1, product2; /* Temporary variables used to store intermediate results */
- q31_t product3, product4; /* Temporary variables used to store intermediate results */
-
- /* Intermediate product is calculated by (Id * cosVal) */
- product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);
-
- /* Intermediate product is calculated by (Iq * sinVal) */
- product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);
-
-
- /* Intermediate product is calculated by (Id * sinVal) */
- product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);
-
- /* Intermediate product is calculated by (Iq * cosVal) */
- product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);
-
- /* Calculate pIalpha by using the two intermediate products 1 and 2 */
- *pIalpha = __QSUB(product1, product2);
-
- /* Calculate pIbeta by using the two intermediate products 3 and 4 */
- *pIbeta = __QADD(product4, product3);
-
- }
-
- /**
- * @} end of Inverse park group
- */
-
-
- /**
- * @brief Converts the elements of the Q31 vector to floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q31_to_float(
- q31_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
- /**
- * @ingroup groupInterpolation
- */
-
- /**
- * @defgroup LinearInterpolate Linear Interpolation
- *
- * Linear interpolation is a method of curve fitting using linear polynomials.
- * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line
- *
- * \par
- * \image html LinearInterp.gif "Linear interpolation"
- *
- * \par
- * A Linear Interpolate function calculates an output value(y), for the input(x)
- * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)
- *
- * \par Algorithm:
- * <pre>
- * y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))
- * where x0, x1 are nearest values of input x
- * y0, y1 are nearest values to output y
- * </pre>
- *
- * \par
- * This set of functions implements Linear interpolation process
- * for Q7, Q15, Q31, and floating-point data types. The functions operate on a single
- * sample of data and each call to the function returns a single processed value.
- * <code>S</code> points to an instance of the Linear Interpolate function data structure.
- * <code>x</code> is the input sample value. The functions returns the output value.
- *
- * \par
- * if x is outside of the table boundary, Linear interpolation returns first value of the table
- * if x is below input range and returns last value of table if x is above range.
- */
-
- /**
- * @addtogroup LinearInterpolate
- * @{
- */
-
- /**
- * @brief Process function for the floating-point Linear Interpolation Function.
- * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure
- * @param[in] x input sample to process
- * @return y processed output sample.
- *
- */
-
- static __INLINE float32_t arm_linear_interp_f32(
- arm_linear_interp_instance_f32 * S,
- float32_t x)
- {
-
- float32_t y;
- float32_t x0, x1; /* Nearest input values */
- float32_t y0, y1; /* Nearest output values */
- float32_t xSpacing = S->xSpacing; /* spacing between input values */
- int32_t i; /* Index variable */
- float32_t *pYData = S->pYData; /* pointer to output table */
-
- /* Calculation of index */
- i = (int32_t) ((x - S->x1) / xSpacing);
-
- if(i < 0)
- {
- /* Iniatilize output for below specified range as least output value of table */
- y = pYData[0];
- }
- else if((uint32_t)i >= S->nValues)
- {
- /* Iniatilize output for above specified range as last output value of table */
- y = pYData[S->nValues - 1];
- }
- else
- {
- /* Calculation of nearest input values */
- x0 = S->x1 + i * xSpacing;
- x1 = S->x1 + (i + 1) * xSpacing;
-
- /* Read of nearest output values */
- y0 = pYData[i];
- y1 = pYData[i + 1];
-
- /* Calculation of output */
- y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0));
-
- }
-
- /* returns output value */
- return (y);
- }
-
- /**
- *
- * @brief Process function for the Q31 Linear Interpolation Function.
- * @param[in] *pYData pointer to Q31 Linear Interpolation table
- * @param[in] x input sample to process
- * @param[in] nValues number of table values
- * @return y processed output sample.
- *
- * \par
- * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
- * This function can support maximum of table size 2^12.
- *
- */
-
-
- static __INLINE q31_t arm_linear_interp_q31(
- q31_t * pYData,
- q31_t x,
- uint32_t nValues)
- {
- q31_t y; /* output */
- q31_t y0, y1; /* Nearest output values */
- q31_t fract; /* fractional part */
- int32_t index; /* Index to read nearest output values */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- index = ((x & 0xFFF00000) >> 20);
-
- if(index >= (int32_t)(nValues - 1))
- {
- return (pYData[nValues - 1]);
- }
- else if(index < 0)
- {
- return (pYData[0]);
- }
- else
- {
-
- /* 20 bits for the fractional part */
- /* shift left by 11 to keep fract in 1.31 format */
- fract = (x & 0x000FFFFF) << 11;
-
- /* Read two nearest output values from the index in 1.31(q31) format */
- y0 = pYData[index];
- y1 = pYData[index + 1u];
-
- /* Calculation of y0 * (1-fract) and y is in 2.30 format */
- y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32));
-
- /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */
- y += ((q31_t) (((q63_t) y1 * fract) >> 32));
-
- /* Convert y to 1.31 format */
- return (y << 1u);
-
- }
-
- }
-
- /**
- *
- * @brief Process function for the Q15 Linear Interpolation Function.
- * @param[in] *pYData pointer to Q15 Linear Interpolation table
- * @param[in] x input sample to process
- * @param[in] nValues number of table values
- * @return y processed output sample.
- *
- * \par
- * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
- * This function can support maximum of table size 2^12.
- *
- */
-
-
- static __INLINE q15_t arm_linear_interp_q15(
- q15_t * pYData,
- q31_t x,
- uint32_t nValues)
- {
- q63_t y; /* output */
- q15_t y0, y1; /* Nearest output values */
- q31_t fract; /* fractional part */
- int32_t index; /* Index to read nearest output values */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- index = ((x & 0xFFF00000) >> 20u);
-
- if(index >= (int32_t)(nValues - 1))
- {
- return (pYData[nValues - 1]);
- }
- else if(index < 0)
- {
- return (pYData[0]);
- }
- else
- {
- /* 20 bits for the fractional part */
- /* fract is in 12.20 format */
- fract = (x & 0x000FFFFF);
-
- /* Read two nearest output values from the index */
- y0 = pYData[index];
- y1 = pYData[index + 1u];
-
- /* Calculation of y0 * (1-fract) and y is in 13.35 format */
- y = ((q63_t) y0 * (0xFFFFF - fract));
-
- /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */
- y += ((q63_t) y1 * (fract));
-
- /* convert y to 1.15 format */
- return (y >> 20);
- }
-
-
- }
-
- /**
- *
- * @brief Process function for the Q7 Linear Interpolation Function.
- * @param[in] *pYData pointer to Q7 Linear Interpolation table
- * @param[in] x input sample to process
- * @param[in] nValues number of table values
- * @return y processed output sample.
- *
- * \par
- * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.
- * This function can support maximum of table size 2^12.
- */
-
-
- static __INLINE q7_t arm_linear_interp_q7(
- q7_t * pYData,
- q31_t x,
- uint32_t nValues)
- {
- q31_t y; /* output */
- q7_t y0, y1; /* Nearest output values */
- q31_t fract; /* fractional part */
- uint32_t index; /* Index to read nearest output values */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- if (x < 0)
- {
- return (pYData[0]);
- }
- index = (x >> 20) & 0xfff;
-
-
- if(index >= (nValues - 1))
- {
- return (pYData[nValues - 1]);
- }
- else
- {
-
- /* 20 bits for the fractional part */
- /* fract is in 12.20 format */
- fract = (x & 0x000FFFFF);
-
- /* Read two nearest output values from the index and are in 1.7(q7) format */
- y0 = pYData[index];
- y1 = pYData[index + 1u];
-
- /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */
- y = ((y0 * (0xFFFFF - fract)));
-
- /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */
- y += (y1 * fract);
-
- /* convert y to 1.7(q7) format */
- return (y >> 20u);
-
- }
-
- }
- /**
- * @} end of LinearInterpolate group
- */
-
- /**
- * @brief Fast approximation to the trigonometric sine function for floating-point data.
- * @param[in] x input value in radians.
- * @return sin(x).
- */
-
- float32_t arm_sin_f32(
- float32_t x);
-
- /**
- * @brief Fast approximation to the trigonometric sine function for Q31 data.
- * @param[in] x Scaled input value in radians.
- * @return sin(x).
- */
-
- q31_t arm_sin_q31(
- q31_t x);
-
- /**
- * @brief Fast approximation to the trigonometric sine function for Q15 data.
- * @param[in] x Scaled input value in radians.
- * @return sin(x).
- */
-
- q15_t arm_sin_q15(
- q15_t x);
-
- /**
- * @brief Fast approximation to the trigonometric cosine function for floating-point data.
- * @param[in] x input value in radians.
- * @return cos(x).
- */
-
- float32_t arm_cos_f32(
- float32_t x);
-
- /**
- * @brief Fast approximation to the trigonometric cosine function for Q31 data.
- * @param[in] x Scaled input value in radians.
- * @return cos(x).
- */
-
- q31_t arm_cos_q31(
- q31_t x);
-
- /**
- * @brief Fast approximation to the trigonometric cosine function for Q15 data.
- * @param[in] x Scaled input value in radians.
- * @return cos(x).
- */
-
- q15_t arm_cos_q15(
- q15_t x);
-
-
- /**
- * @ingroup groupFastMath
- */
-
-
- /**
- * @defgroup SQRT Square Root
- *
- * Computes the square root of a number.
- * There are separate functions for Q15, Q31, and floating-point data types.
- * The square root function is computed using the Newton-Raphson algorithm.
- * This is an iterative algorithm of the form:
- * <pre>
- * x1 = x0 - f(x0)/f'(x0)
- * </pre>
- * where <code>x1</code> is the current estimate,
- * <code>x0</code> is the previous estimate, and
- * <code>f'(x0)</code> is the derivative of <code>f()</code> evaluated at <code>x0</code>.
- * For the square root function, the algorithm reduces to:
- * <pre>
- * x0 = in/2 [initial guess]
- * x1 = 1/2 * ( x0 + in / x0) [each iteration]
- * </pre>
- */
-
-
- /**
- * @addtogroup SQRT
- * @{
- */
-
- /**
- * @brief Floating-point square root function.
- * @param[in] in input value.
- * @param[out] *pOut square root of input value.
- * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
- * <code>in</code> is negative value and returns zero output for negative values.
- */
-
- static __INLINE arm_status arm_sqrt_f32(
- float32_t in,
- float32_t * pOut)
- {
- if(in > 0)
- {
-
-// #if __FPU_USED
-#if (__FPU_USED == 1) && defined ( __CC_ARM )
- *pOut = __sqrtf(in);
-#else
- *pOut = sqrtf(in);
-#endif
-
- return (ARM_MATH_SUCCESS);
- }
- else
- {
- *pOut = 0.0f;
- return (ARM_MATH_ARGUMENT_ERROR);
- }
-
- }
-
-
- /**
- * @brief Q31 square root function.
- * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
- * @param[out] *pOut square root of input value.
- * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
- * <code>in</code> is negative value and returns zero output for negative values.
- */
- arm_status arm_sqrt_q31(
- q31_t in,
- q31_t * pOut);
-
- /**
- * @brief Q15 square root function.
- * @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF.
- * @param[out] *pOut square root of input value.
- * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
- * <code>in</code> is negative value and returns zero output for negative values.
- */
- arm_status arm_sqrt_q15(
- q15_t in,
- q15_t * pOut);
-
- /**
- * @} end of SQRT group
- */
-
-
-
-
-
-
- /**
- * @brief floating-point Circular write function.
- */
-
- static __INLINE void arm_circularWrite_f32(
- int32_t * circBuffer,
- int32_t L,
- uint16_t * writeOffset,
- int32_t bufferInc,
- const int32_t * src,
- int32_t srcInc,
- uint32_t blockSize)
- {
- uint32_t i = 0u;
- int32_t wOffset;
-
- /* Copy the value of Index pointer that points
- * to the current location where the input samples to be copied */
- wOffset = *writeOffset;
-
- /* Loop over the blockSize */
- i = blockSize;
-
- while(i > 0u)
- {
- /* copy the input sample to the circular buffer */
- circBuffer[wOffset] = *src;
-
- /* Update the input pointer */
- src += srcInc;
-
- /* Circularly update wOffset. Watch out for positive and negative value */
- wOffset += bufferInc;
- if(wOffset >= L)
- wOffset -= L;
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Update the index pointer */
- *writeOffset = wOffset;
- }
-
-
-
- /**
- * @brief floating-point Circular Read function.
- */
- static __INLINE void arm_circularRead_f32(
- int32_t * circBuffer,
- int32_t L,
- int32_t * readOffset,
- int32_t bufferInc,
- int32_t * dst,
- int32_t * dst_base,
- int32_t dst_length,
- int32_t dstInc,
- uint32_t blockSize)
- {
- uint32_t i = 0u;
- int32_t rOffset, dst_end;
-
- /* Copy the value of Index pointer that points
- * to the current location from where the input samples to be read */
- rOffset = *readOffset;
- dst_end = (int32_t) (dst_base + dst_length);
-
- /* Loop over the blockSize */
- i = blockSize;
-
- while(i > 0u)
- {
- /* copy the sample from the circular buffer to the destination buffer */
- *dst = circBuffer[rOffset];
-
- /* Update the input pointer */
- dst += dstInc;
-
- if(dst == (int32_t *) dst_end)
- {
- dst = dst_base;
- }
-
- /* Circularly update rOffset. Watch out for positive and negative value */
- rOffset += bufferInc;
-
- if(rOffset >= L)
- {
- rOffset -= L;
- }
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Update the index pointer */
- *readOffset = rOffset;
- }
-
- /**
- * @brief Q15 Circular write function.
- */
-
- static __INLINE void arm_circularWrite_q15(
- q15_t * circBuffer,
- int32_t L,
- uint16_t * writeOffset,
- int32_t bufferInc,
- const q15_t * src,
- int32_t srcInc,
- uint32_t blockSize)
- {
- uint32_t i = 0u;
- int32_t wOffset;
-
- /* Copy the value of Index pointer that points
- * to the current location where the input samples to be copied */
- wOffset = *writeOffset;
-
- /* Loop over the blockSize */
- i = blockSize;
-
- while(i > 0u)
- {
- /* copy the input sample to the circular buffer */
- circBuffer[wOffset] = *src;
-
- /* Update the input pointer */
- src += srcInc;
-
- /* Circularly update wOffset. Watch out for positive and negative value */
- wOffset += bufferInc;
- if(wOffset >= L)
- wOffset -= L;
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Update the index pointer */
- *writeOffset = wOffset;
- }
-
-
-
- /**
- * @brief Q15 Circular Read function.
- */
- static __INLINE void arm_circularRead_q15(
- q15_t * circBuffer,
- int32_t L,
- int32_t * readOffset,
- int32_t bufferInc,
- q15_t * dst,
- q15_t * dst_base,
- int32_t dst_length,
- int32_t dstInc,
- uint32_t blockSize)
- {
- uint32_t i = 0;
- int32_t rOffset, dst_end;
-
- /* Copy the value of Index pointer that points
- * to the current location from where the input samples to be read */
- rOffset = *readOffset;
-
- dst_end = (int32_t) (dst_base + dst_length);
-
- /* Loop over the blockSize */
- i = blockSize;
-
- while(i > 0u)
- {
- /* copy the sample from the circular buffer to the destination buffer */
- *dst = circBuffer[rOffset];
-
- /* Update the input pointer */
- dst += dstInc;
-
- if(dst == (q15_t *) dst_end)
- {
- dst = dst_base;
- }
-
- /* Circularly update wOffset. Watch out for positive and negative value */
- rOffset += bufferInc;
-
- if(rOffset >= L)
- {
- rOffset -= L;
- }
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Update the index pointer */
- *readOffset = rOffset;
- }
-
-
- /**
- * @brief Q7 Circular write function.
- */
-
- static __INLINE void arm_circularWrite_q7(
- q7_t * circBuffer,
- int32_t L,
- uint16_t * writeOffset,
- int32_t bufferInc,
- const q7_t * src,
- int32_t srcInc,
- uint32_t blockSize)
- {
- uint32_t i = 0u;
- int32_t wOffset;
-
- /* Copy the value of Index pointer that points
- * to the current location where the input samples to be copied */
- wOffset = *writeOffset;
-
- /* Loop over the blockSize */
- i = blockSize;
-
- while(i > 0u)
- {
- /* copy the input sample to the circular buffer */
- circBuffer[wOffset] = *src;
-
- /* Update the input pointer */
- src += srcInc;
-
- /* Circularly update wOffset. Watch out for positive and negative value */
- wOffset += bufferInc;
- if(wOffset >= L)
- wOffset -= L;
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Update the index pointer */
- *writeOffset = wOffset;
- }
-
-
-
- /**
- * @brief Q7 Circular Read function.
- */
- static __INLINE void arm_circularRead_q7(
- q7_t * circBuffer,
- int32_t L,
- int32_t * readOffset,
- int32_t bufferInc,
- q7_t * dst,
- q7_t * dst_base,
- int32_t dst_length,
- int32_t dstInc,
- uint32_t blockSize)
- {
- uint32_t i = 0;
- int32_t rOffset, dst_end;
-
- /* Copy the value of Index pointer that points
- * to the current location from where the input samples to be read */
- rOffset = *readOffset;
-
- dst_end = (int32_t) (dst_base + dst_length);
-
- /* Loop over the blockSize */
- i = blockSize;
-
- while(i > 0u)
- {
- /* copy the sample from the circular buffer to the destination buffer */
- *dst = circBuffer[rOffset];
-
- /* Update the input pointer */
- dst += dstInc;
-
- if(dst == (q7_t *) dst_end)
- {
- dst = dst_base;
- }
-
- /* Circularly update rOffset. Watch out for positive and negative value */
- rOffset += bufferInc;
-
- if(rOffset >= L)
- {
- rOffset -= L;
- }
-
- /* Decrement the loop counter */
- i--;
- }
-
- /* Update the index pointer */
- *readOffset = rOffset;
- }
-
-
- /**
- * @brief Sum of the squares of the elements of a Q31 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_power_q31(
- q31_t * pSrc,
- uint32_t blockSize,
- q63_t * pResult);
-
- /**
- * @brief Sum of the squares of the elements of a floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_power_f32(
- float32_t * pSrc,
- uint32_t blockSize,
- float32_t * pResult);
-
- /**
- * @brief Sum of the squares of the elements of a Q15 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_power_q15(
- q15_t * pSrc,
- uint32_t blockSize,
- q63_t * pResult);
-
- /**
- * @brief Sum of the squares of the elements of a Q7 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_power_q7(
- q7_t * pSrc,
- uint32_t blockSize,
- q31_t * pResult);
-
- /**
- * @brief Mean value of a Q7 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_mean_q7(
- q7_t * pSrc,
- uint32_t blockSize,
- q7_t * pResult);
-
- /**
- * @brief Mean value of a Q15 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
- void arm_mean_q15(
- q15_t * pSrc,
- uint32_t blockSize,
- q15_t * pResult);
-
- /**
- * @brief Mean value of a Q31 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
- void arm_mean_q31(
- q31_t * pSrc,
- uint32_t blockSize,
- q31_t * pResult);
-
- /**
- * @brief Mean value of a floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
- void arm_mean_f32(
- float32_t * pSrc,
- uint32_t blockSize,
- float32_t * pResult);
-
- /**
- * @brief Variance of the elements of a floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_var_f32(
- float32_t * pSrc,
- uint32_t blockSize,
- float32_t * pResult);
-
- /**
- * @brief Variance of the elements of a Q31 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_var_q31(
- q31_t * pSrc,
- uint32_t blockSize,
- q63_t * pResult);
-
- /**
- * @brief Variance of the elements of a Q15 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_var_q15(
- q15_t * pSrc,
- uint32_t blockSize,
- q31_t * pResult);
-
- /**
- * @brief Root Mean Square of the elements of a floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_rms_f32(
- float32_t * pSrc,
- uint32_t blockSize,
- float32_t * pResult);
-
- /**
- * @brief Root Mean Square of the elements of a Q31 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_rms_q31(
- q31_t * pSrc,
- uint32_t blockSize,
- q31_t * pResult);
-
- /**
- * @brief Root Mean Square of the elements of a Q15 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_rms_q15(
- q15_t * pSrc,
- uint32_t blockSize,
- q15_t * pResult);
-
- /**
- * @brief Standard deviation of the elements of a floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_std_f32(
- float32_t * pSrc,
- uint32_t blockSize,
- float32_t * pResult);
-
- /**
- * @brief Standard deviation of the elements of a Q31 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_std_q31(
- q31_t * pSrc,
- uint32_t blockSize,
- q31_t * pResult);
-
- /**
- * @brief Standard deviation of the elements of a Q15 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output value.
- * @return none.
- */
-
- void arm_std_q15(
- q15_t * pSrc,
- uint32_t blockSize,
- q15_t * pResult);
-
- /**
- * @brief Floating-point complex magnitude
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_f32(
- float32_t * pSrc,
- float32_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q31 complex magnitude
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_q31(
- q31_t * pSrc,
- q31_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q15 complex magnitude
- * @param[in] *pSrc points to the complex input vector
- * @param[out] *pDst points to the real output vector
- * @param[in] numSamples number of complex samples in the input vector
- * @return none.
- */
-
- void arm_cmplx_mag_q15(
- q15_t * pSrc,
- q15_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q15 complex dot product
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] numSamples number of complex samples in each vector
- * @param[out] *realResult real part of the result returned here
- * @param[out] *imagResult imaginary part of the result returned here
- * @return none.
- */
-
- void arm_cmplx_dot_prod_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- uint32_t numSamples,
- q31_t * realResult,
- q31_t * imagResult);
-
- /**
- * @brief Q31 complex dot product
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] numSamples number of complex samples in each vector
- * @param[out] *realResult real part of the result returned here
- * @param[out] *imagResult imaginary part of the result returned here
- * @return none.
- */
-
- void arm_cmplx_dot_prod_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- uint32_t numSamples,
- q63_t * realResult,
- q63_t * imagResult);
-
- /**
- * @brief Floating-point complex dot product
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[in] numSamples number of complex samples in each vector
- * @param[out] *realResult real part of the result returned here
- * @param[out] *imagResult imaginary part of the result returned here
- * @return none.
- */
-
- void arm_cmplx_dot_prod_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- uint32_t numSamples,
- float32_t * realResult,
- float32_t * imagResult);
-
- /**
- * @brief Q15 complex-by-real multiplication
- * @param[in] *pSrcCmplx points to the complex input vector
- * @param[in] *pSrcReal points to the real input vector
- * @param[out] *pCmplxDst points to the complex output vector
- * @param[in] numSamples number of samples in each vector
- * @return none.
- */
-
- void arm_cmplx_mult_real_q15(
- q15_t * pSrcCmplx,
- q15_t * pSrcReal,
- q15_t * pCmplxDst,
- uint32_t numSamples);
-
- /**
- * @brief Q31 complex-by-real multiplication
- * @param[in] *pSrcCmplx points to the complex input vector
- * @param[in] *pSrcReal points to the real input vector
- * @param[out] *pCmplxDst points to the complex output vector
- * @param[in] numSamples number of samples in each vector
- * @return none.
- */
-
- void arm_cmplx_mult_real_q31(
- q31_t * pSrcCmplx,
- q31_t * pSrcReal,
- q31_t * pCmplxDst,
- uint32_t numSamples);
-
- /**
- * @brief Floating-point complex-by-real multiplication
- * @param[in] *pSrcCmplx points to the complex input vector
- * @param[in] *pSrcReal points to the real input vector
- * @param[out] *pCmplxDst points to the complex output vector
- * @param[in] numSamples number of samples in each vector
- * @return none.
- */
-
- void arm_cmplx_mult_real_f32(
- float32_t * pSrcCmplx,
- float32_t * pSrcReal,
- float32_t * pCmplxDst,
- uint32_t numSamples);
-
- /**
- * @brief Minimum value of a Q7 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *result is output pointer
- * @param[in] index is the array index of the minimum value in the input buffer.
- * @return none.
- */
-
- void arm_min_q7(
- q7_t * pSrc,
- uint32_t blockSize,
- q7_t * result,
- uint32_t * index);
-
- /**
- * @brief Minimum value of a Q15 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output pointer
- * @param[in] *pIndex is the array index of the minimum value in the input buffer.
- * @return none.
- */
-
- void arm_min_q15(
- q15_t * pSrc,
- uint32_t blockSize,
- q15_t * pResult,
- uint32_t * pIndex);
-
- /**
- * @brief Minimum value of a Q31 vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output pointer
- * @param[out] *pIndex is the array index of the minimum value in the input buffer.
- * @return none.
- */
- void arm_min_q31(
- q31_t * pSrc,
- uint32_t blockSize,
- q31_t * pResult,
- uint32_t * pIndex);
-
- /**
- * @brief Minimum value of a floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[in] blockSize is the number of samples to process
- * @param[out] *pResult is output pointer
- * @param[out] *pIndex is the array index of the minimum value in the input buffer.
- * @return none.
- */
-
- void arm_min_f32(
- float32_t * pSrc,
- uint32_t blockSize,
- float32_t * pResult,
- uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a Q7 vector.
- * @param[in] *pSrc points to the input buffer
- * @param[in] blockSize length of the input vector
- * @param[out] *pResult maximum value returned here
- * @param[out] *pIndex index of maximum value returned here
- * @return none.
- */
-
- void arm_max_q7(
- q7_t * pSrc,
- uint32_t blockSize,
- q7_t * pResult,
- uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a Q15 vector.
- * @param[in] *pSrc points to the input buffer
- * @param[in] blockSize length of the input vector
- * @param[out] *pResult maximum value returned here
- * @param[out] *pIndex index of maximum value returned here
- * @return none.
- */
-
- void arm_max_q15(
- q15_t * pSrc,
- uint32_t blockSize,
- q15_t * pResult,
- uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a Q31 vector.
- * @param[in] *pSrc points to the input buffer
- * @param[in] blockSize length of the input vector
- * @param[out] *pResult maximum value returned here
- * @param[out] *pIndex index of maximum value returned here
- * @return none.
- */
-
- void arm_max_q31(
- q31_t * pSrc,
- uint32_t blockSize,
- q31_t * pResult,
- uint32_t * pIndex);
-
-/**
- * @brief Maximum value of a floating-point vector.
- * @param[in] *pSrc points to the input buffer
- * @param[in] blockSize length of the input vector
- * @param[out] *pResult maximum value returned here
- * @param[out] *pIndex index of maximum value returned here
- * @return none.
- */
-
- void arm_max_f32(
- float32_t * pSrc,
- uint32_t blockSize,
- float32_t * pResult,
- uint32_t * pIndex);
-
- /**
- * @brief Q15 complex-by-complex multiplication
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_mult_cmplx_q15(
- q15_t * pSrcA,
- q15_t * pSrcB,
- q15_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Q31 complex-by-complex multiplication
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_mult_cmplx_q31(
- q31_t * pSrcA,
- q31_t * pSrcB,
- q31_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Floating-point complex-by-complex multiplication
- * @param[in] *pSrcA points to the first input vector
- * @param[in] *pSrcB points to the second input vector
- * @param[out] *pDst points to the output vector
- * @param[in] numSamples number of complex samples in each vector
- * @return none.
- */
-
- void arm_cmplx_mult_cmplx_f32(
- float32_t * pSrcA,
- float32_t * pSrcB,
- float32_t * pDst,
- uint32_t numSamples);
-
- /**
- * @brief Converts the elements of the floating-point vector to Q31 vector.
- * @param[in] *pSrc points to the floating-point input vector
- * @param[out] *pDst points to the Q31 output vector
- * @param[in] blockSize length of the input vector
- * @return none.
- */
- void arm_float_to_q31(
- float32_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Converts the elements of the floating-point vector to Q15 vector.
- * @param[in] *pSrc points to the floating-point input vector
- * @param[out] *pDst points to the Q15 output vector
- * @param[in] blockSize length of the input vector
- * @return none
- */
- void arm_float_to_q15(
- float32_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Converts the elements of the floating-point vector to Q7 vector.
- * @param[in] *pSrc points to the floating-point input vector
- * @param[out] *pDst points to the Q7 output vector
- * @param[in] blockSize length of the input vector
- * @return none
- */
- void arm_float_to_q7(
- float32_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Converts the elements of the Q31 vector to Q15 vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q31_to_q15(
- q31_t * pSrc,
- q15_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Converts the elements of the Q31 vector to Q7 vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q31_to_q7(
- q31_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
- /**
- * @brief Converts the elements of the Q15 vector to floating-point vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q15_to_float(
- q15_t * pSrc,
- float32_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Converts the elements of the Q15 vector to Q31 vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q15_to_q31(
- q15_t * pSrc,
- q31_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @brief Converts the elements of the Q15 vector to Q7 vector.
- * @param[in] *pSrc is input pointer
- * @param[out] *pDst is output pointer
- * @param[in] blockSize is the number of samples to process
- * @return none.
- */
- void arm_q15_to_q7(
- q15_t * pSrc,
- q7_t * pDst,
- uint32_t blockSize);
-
-
- /**
- * @ingroup groupInterpolation
- */
-
- /**
- * @defgroup BilinearInterpolate Bilinear Interpolation
- *
- * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid.
- * The underlying function <code>f(x, y)</code> is sampled on a regular grid and the interpolation process
- * determines values between the grid points.
- * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension.
- * Bilinear interpolation is often used in image processing to rescale images.
- * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types.
- *
- * <b>Algorithm</b>
- * \par
- * The instance structure used by the bilinear interpolation functions describes a two dimensional data table.
- * For floating-point, the instance structure is defined as:
- * <pre>
- * typedef struct
- * {
- * uint16_t numRows;
- * uint16_t numCols;
- * float32_t *pData;
- * } arm_bilinear_interp_instance_f32;
- * </pre>
- *
- * \par
- * where <code>numRows</code> specifies the number of rows in the table;
- * <code>numCols</code> specifies the number of columns in the table;
- * and <code>pData</code> points to an array of size <code>numRows*numCols</code> values.
- * The data table <code>pTable</code> is organized in row order and the supplied data values fall on integer indexes.
- * That is, table element (x,y) is located at <code>pTable[x + y*numCols]</code> where x and y are integers.
- *
- * \par
- * Let <code>(x, y)</code> specify the desired interpolation point. Then define:
- * <pre>
- * XF = floor(x)
- * YF = floor(y)
- * </pre>
- * \par
- * The interpolated output point is computed as:
- * <pre>
- * f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))
- * + f(XF+1, YF) * (x-XF)*(1-(y-YF))
- * + f(XF, YF+1) * (1-(x-XF))*(y-YF)
- * + f(XF+1, YF+1) * (x-XF)*(y-YF)
- * </pre>
- * Note that the coordinates (x, y) contain integer and fractional components.
- * The integer components specify which portion of the table to use while the
- * fractional components control the interpolation processor.
- *
- * \par
- * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output.
- */
-
- /**
- * @addtogroup BilinearInterpolate
- * @{
- */
-
- /**
- *
- * @brief Floating-point bilinear interpolation.
- * @param[in,out] *S points to an instance of the interpolation structure.
- * @param[in] X interpolation coordinate.
- * @param[in] Y interpolation coordinate.
- * @return out interpolated value.
- */
-
-
- static __INLINE float32_t arm_bilinear_interp_f32(
- const arm_bilinear_interp_instance_f32 * S,
- float32_t X,
- float32_t Y)
- {
- float32_t out;
- float32_t f00, f01, f10, f11;
- float32_t *pData = S->pData;
- int32_t xIndex, yIndex, index;
- float32_t xdiff, ydiff;
- float32_t b1, b2, b3, b4;
-
- xIndex = (int32_t) X;
- yIndex = (int32_t) Y;
-
- /* Care taken for table outside boundary */
- /* Returns zero output when values are outside table boundary */
- if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0
- || yIndex > (S->numCols - 1))
- {
- return (0);
- }
-
- /* Calculation of index for two nearest points in X-direction */
- index = (xIndex - 1) + (yIndex - 1) * S->numCols;
-
-
- /* Read two nearest points in X-direction */
- f00 = pData[index];
- f01 = pData[index + 1];
-
- /* Calculation of index for two nearest points in Y-direction */
- index = (xIndex - 1) + (yIndex) * S->numCols;
-
-
- /* Read two nearest points in Y-direction */
- f10 = pData[index];
- f11 = pData[index + 1];
-
- /* Calculation of intermediate values */
- b1 = f00;
- b2 = f01 - f00;
- b3 = f10 - f00;
- b4 = f00 - f01 - f10 + f11;
-
- /* Calculation of fractional part in X */
- xdiff = X - xIndex;
-
- /* Calculation of fractional part in Y */
- ydiff = Y - yIndex;
-
- /* Calculation of bi-linear interpolated output */
- out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff;
-
- /* return to application */
- return (out);
-
- }
-
- /**
- *
- * @brief Q31 bilinear interpolation.
- * @param[in,out] *S points to an instance of the interpolation structure.
- * @param[in] X interpolation coordinate in 12.20 format.
- * @param[in] Y interpolation coordinate in 12.20 format.
- * @return out interpolated value.
- */
-
- static __INLINE q31_t arm_bilinear_interp_q31(
- arm_bilinear_interp_instance_q31 * S,
- q31_t X,
- q31_t Y)
- {
- q31_t out; /* Temporary output */
- q31_t acc = 0; /* output */
- q31_t xfract, yfract; /* X, Y fractional parts */
- q31_t x1, x2, y1, y2; /* Nearest output values */
- int32_t rI, cI; /* Row and column indices */
- q31_t *pYData = S->pData; /* pointer to output table values */
- uint32_t nCols = S->numCols; /* num of rows */
-
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- rI = ((X & 0xFFF00000) >> 20u);
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- cI = ((Y & 0xFFF00000) >> 20u);
-
- /* Care taken for table outside boundary */
- /* Returns zero output when values are outside table boundary */
- if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
- {
- return (0);
- }
-
- /* 20 bits for the fractional part */
- /* shift left xfract by 11 to keep 1.31 format */
- xfract = (X & 0x000FFFFF) << 11u;
-
- /* Read two nearest output values from the index */
- x1 = pYData[(rI) + nCols * (cI)];
- x2 = pYData[(rI) + nCols * (cI) + 1u];
-
- /* 20 bits for the fractional part */
- /* shift left yfract by 11 to keep 1.31 format */
- yfract = (Y & 0x000FFFFF) << 11u;
-
- /* Read two nearest output values from the index */
- y1 = pYData[(rI) + nCols * (cI + 1)];
- y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
-
- /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */
- out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32));
- acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32));
-
- /* x2 * (xfract) * (1-yfract) in 3.29(q29) and adding to acc */
- out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32));
- acc += ((q31_t) ((q63_t) out * (xfract) >> 32));
-
- /* y1 * (1 - xfract) * (yfract) in 3.29(q29) and adding to acc */
- out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32));
- acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
-
- /* y2 * (xfract) * (yfract) in 3.29(q29) and adding to acc */
- out = ((q31_t) ((q63_t) y2 * (xfract) >> 32));
- acc += ((q31_t) ((q63_t) out * (yfract) >> 32));
-
- /* Convert acc to 1.31(q31) format */
- return (acc << 2u);
-
- }
-
- /**
- * @brief Q15 bilinear interpolation.
- * @param[in,out] *S points to an instance of the interpolation structure.
- * @param[in] X interpolation coordinate in 12.20 format.
- * @param[in] Y interpolation coordinate in 12.20 format.
- * @return out interpolated value.
- */
-
- static __INLINE q15_t arm_bilinear_interp_q15(
- arm_bilinear_interp_instance_q15 * S,
- q31_t X,
- q31_t Y)
- {
- q63_t acc = 0; /* output */
- q31_t out; /* Temporary output */
- q15_t x1, x2, y1, y2; /* Nearest output values */
- q31_t xfract, yfract; /* X, Y fractional parts */
- int32_t rI, cI; /* Row and column indices */
- q15_t *pYData = S->pData; /* pointer to output table values */
- uint32_t nCols = S->numCols; /* num of rows */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- rI = ((X & 0xFFF00000) >> 20);
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- cI = ((Y & 0xFFF00000) >> 20);
-
- /* Care taken for table outside boundary */
- /* Returns zero output when values are outside table boundary */
- if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
- {
- return (0);
- }
-
- /* 20 bits for the fractional part */
- /* xfract should be in 12.20 format */
- xfract = (X & 0x000FFFFF);
-
- /* Read two nearest output values from the index */
- x1 = pYData[(rI) + nCols * (cI)];
- x2 = pYData[(rI) + nCols * (cI) + 1u];
-
-
- /* 20 bits for the fractional part */
- /* yfract should be in 12.20 format */
- yfract = (Y & 0x000FFFFF);
-
- /* Read two nearest output values from the index */
- y1 = pYData[(rI) + nCols * (cI + 1)];
- y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
-
- /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */
-
- /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */
- /* convert 13.35 to 13.31 by right shifting and out is in 1.31 */
- out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u);
- acc = ((q63_t) out * (0xFFFFF - yfract));
-
- /* x2 * (xfract) * (1-yfract) in 1.51 and adding to acc */
- out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u);
- acc += ((q63_t) out * (xfract));
-
- /* y1 * (1 - xfract) * (yfract) in 1.51 and adding to acc */
- out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u);
- acc += ((q63_t) out * (yfract));
-
- /* y2 * (xfract) * (yfract) in 1.51 and adding to acc */
- out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u);
- acc += ((q63_t) out * (yfract));
-
- /* acc is in 13.51 format and down shift acc by 36 times */
- /* Convert out to 1.15 format */
- return (acc >> 36);
-
- }
-
- /**
- * @brief Q7 bilinear interpolation.
- * @param[in,out] *S points to an instance of the interpolation structure.
- * @param[in] X interpolation coordinate in 12.20 format.
- * @param[in] Y interpolation coordinate in 12.20 format.
- * @return out interpolated value.
- */
-
- static __INLINE q7_t arm_bilinear_interp_q7(
- arm_bilinear_interp_instance_q7 * S,
- q31_t X,
- q31_t Y)
- {
- q63_t acc = 0; /* output */
- q31_t out; /* Temporary output */
- q31_t xfract, yfract; /* X, Y fractional parts */
- q7_t x1, x2, y1, y2; /* Nearest output values */
- int32_t rI, cI; /* Row and column indices */
- q7_t *pYData = S->pData; /* pointer to output table values */
- uint32_t nCols = S->numCols; /* num of rows */
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- rI = ((X & 0xFFF00000) >> 20);
-
- /* Input is in 12.20 format */
- /* 12 bits for the table index */
- /* Index value calculation */
- cI = ((Y & 0xFFF00000) >> 20);
-
- /* Care taken for table outside boundary */
- /* Returns zero output when values are outside table boundary */
- if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))
- {
- return (0);
- }
-
- /* 20 bits for the fractional part */
- /* xfract should be in 12.20 format */
- xfract = (X & 0x000FFFFF);
-
- /* Read two nearest output values from the index */
- x1 = pYData[(rI) + nCols * (cI)];
- x2 = pYData[(rI) + nCols * (cI) + 1u];
-
-
- /* 20 bits for the fractional part */
- /* yfract should be in 12.20 format */
- yfract = (Y & 0x000FFFFF);
-
- /* Read two nearest output values from the index */
- y1 = pYData[(rI) + nCols * (cI + 1)];
- y2 = pYData[(rI) + nCols * (cI + 1) + 1u];
-
- /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */
- out = ((x1 * (0xFFFFF - xfract)));
- acc = (((q63_t) out * (0xFFFFF - yfract)));
-
- /* x2 * (xfract) * (1-yfract) in 2.22 and adding to acc */
- out = ((x2 * (0xFFFFF - yfract)));
- acc += (((q63_t) out * (xfract)));
-
- /* y1 * (1 - xfract) * (yfract) in 2.22 and adding to acc */
- out = ((y1 * (0xFFFFF - xfract)));
- acc += (((q63_t) out * (yfract)));
-
- /* y2 * (xfract) * (yfract) in 2.22 and adding to acc */
- out = ((y2 * (yfract)));
- acc += (((q63_t) out * (xfract)));
-
- /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */
- return (acc >> 40);
-
- }
-
- /**
- * @} end of BilinearInterpolate group
- */
-
-
-#if defined ( __CC_ARM ) //Keil
-//SMMLAR
- #define multAcc_32x32_keep32_R(a, x, y) \
- a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32)
-
-//SMMLSR
- #define multSub_32x32_keep32_R(a, x, y) \
- a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32)
-
-//SMMULR
- #define mult_32x32_keep32_R(a, x, y) \
- a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32)
-
-//Enter low optimization region - place directly above function definition
- #define LOW_OPTIMIZATION_ENTER \
- _Pragma ("push") \
- _Pragma ("O1")
-
-//Exit low optimization region - place directly after end of function definition
- #define LOW_OPTIMIZATION_EXIT \
- _Pragma ("pop")
-
-//Enter low optimization region - place directly above function definition
- #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
-
-//Exit low optimization region - place directly after end of function definition
- #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
-
-#elif defined(__ICCARM__) //IAR
- //SMMLA
- #define multAcc_32x32_keep32_R(a, x, y) \
- a += (q31_t) (((q63_t) x * y) >> 32)
-
- //SMMLS
- #define multSub_32x32_keep32_R(a, x, y) \
- a -= (q31_t) (((q63_t) x * y) >> 32)
-
-//SMMUL
- #define mult_32x32_keep32_R(a, x, y) \
- a = (q31_t) (((q63_t) x * y ) >> 32)
-
-//Enter low optimization region - place directly above function definition
- #define LOW_OPTIMIZATION_ENTER \
- _Pragma ("optimize=low")
-
-//Exit low optimization region - place directly after end of function definition
- #define LOW_OPTIMIZATION_EXIT
-
-//Enter low optimization region - place directly above function definition
- #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \
- _Pragma ("optimize=low")
-
-//Exit low optimization region - place directly after end of function definition
- #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
-
-#elif defined(__GNUC__)
- //SMMLA
- #define multAcc_32x32_keep32_R(a, x, y) \
- a += (q31_t) (((q63_t) x * y) >> 32)
-
- //SMMLS
- #define multSub_32x32_keep32_R(a, x, y) \
- a -= (q31_t) (((q63_t) x * y) >> 32)
-
-//SMMUL
- #define mult_32x32_keep32_R(a, x, y) \
- a = (q31_t) (((q63_t) x * y ) >> 32)
-
- #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") ))
-
- #define LOW_OPTIMIZATION_EXIT
-
- #define IAR_ONLY_LOW_OPTIMIZATION_ENTER
-
- #define IAR_ONLY_LOW_OPTIMIZATION_EXIT
-
-#endif
-
-
-
-
-
-#ifdef __cplusplus
-}
-#endif
-
-
-#endif /* _ARM_MATH_H */
-
-
-/**
- *
- * End of file.
- */
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/modules/mathlib/CMSIS/Include/core_cm3.h deleted file mode 100644 index 8ac6dc078..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cm3.h +++ /dev/null @@ -1,1627 +0,0 @@ -/**************************************************************************//**
- * @file core_cm3.h
- * @brief CMSIS Cortex-M3 Core Peripheral Access Layer Header File
- * @version V3.20
- * @date 25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
- 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 ARM 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 COPYRIGHT HOLDERS AND 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.
- ---------------------------------------------------------------------------*/
-
-
-#if defined ( __ICCARM__ )
- #pragma system_include /* treat file as system include file for MISRA check */
-#endif
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-#ifndef __CORE_CM3_H_GENERIC
-#define __CORE_CM3_H_GENERIC
-
-/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions
- CMSIS violates the following MISRA-C:2004 rules:
-
- \li Required Rule 8.5, object/function definition in header file.<br>
- Function definitions in header files are used to allow 'inlining'.
-
- \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
- Unions are used for effective representation of core registers.
-
- \li Advisory Rule 19.7, Function-like macro defined.<br>
- Function-like macros are used to allow more efficient code.
- */
-
-
-/*******************************************************************************
- * CMSIS definitions
- ******************************************************************************/
-/** \ingroup Cortex_M3
- @{
- */
-
-/* CMSIS CM3 definitions */
-#define __CM3_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */
-#define __CM3_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */
-#define __CM3_CMSIS_VERSION ((__CM3_CMSIS_VERSION_MAIN << 16) | \
- __CM3_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */
-
-#define __CORTEX_M (0x03) /*!< Cortex-M Core */
-
-
-#if defined ( __CC_ARM )
- #define __ASM __asm /*!< asm keyword for ARM Compiler */
- #define __INLINE __inline /*!< inline keyword for ARM Compiler */
- #define __STATIC_INLINE static __inline
-
-#elif defined ( __ICCARM__ )
- #define __ASM __asm /*!< asm keyword for IAR Compiler */
- #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
- #define __STATIC_INLINE static inline
-
-#elif defined ( __TMS470__ )
- #define __ASM __asm /*!< asm keyword for TI CCS Compiler */
- #define __STATIC_INLINE static inline
-
-#elif defined ( __GNUC__ )
- #define __ASM __asm /*!< asm keyword for GNU Compiler */
- #define __INLINE inline /*!< inline keyword for GNU Compiler */
- #define __STATIC_INLINE static inline
-
-#elif defined ( __TASKING__ )
- #define __ASM __asm /*!< asm keyword for TASKING Compiler */
- #define __INLINE inline /*!< inline keyword for TASKING Compiler */
- #define __STATIC_INLINE static inline
-
-#endif
-
-/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all
-*/
-#define __FPU_USED 0
-
-#if defined ( __CC_ARM )
- #if defined __TARGET_FPU_VFP
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #endif
-
-#elif defined ( __ICCARM__ )
- #if defined __ARMVFP__
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #endif
-
-#elif defined ( __TMS470__ )
- #if defined __TI__VFP_SUPPORT____
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #endif
-
-#elif defined ( __GNUC__ )
- #if defined (__VFP_FP__) && !defined(__SOFTFP__)
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #endif
-
-#elif defined ( __TASKING__ )
- #if defined __FPU_VFP__
- #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #endif
-#endif
-
-#include <stdint.h> /* standard types definitions */
-#include "core_cmInstr.h" /* Core Instruction Access */
-#include "core_cmFunc.h" /* Core Function Access */
-
-#endif /* __CORE_CM3_H_GENERIC */
-
-#ifndef __CMSIS_GENERIC
-
-#ifndef __CORE_CM3_H_DEPENDANT
-#define __CORE_CM3_H_DEPENDANT
-
-/* check device defines and use defaults */
-#if defined __CHECK_DEVICE_DEFINES
- #ifndef __CM3_REV
- #define __CM3_REV 0x0200
- #warning "__CM3_REV not defined in device header file; using default!"
- #endif
-
- #ifndef __MPU_PRESENT
- #define __MPU_PRESENT 0
- #warning "__MPU_PRESENT not defined in device header file; using default!"
- #endif
-
- #ifndef __NVIC_PRIO_BITS
- #define __NVIC_PRIO_BITS 4
- #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
- #endif
-
- #ifndef __Vendor_SysTickConfig
- #define __Vendor_SysTickConfig 0
- #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
- #endif
-#endif
-
-/* IO definitions (access restrictions to peripheral registers) */
-/**
- \defgroup CMSIS_glob_defs CMSIS Global Defines
-
- <strong>IO Type Qualifiers</strong> are used
- \li to specify the access to peripheral variables.
- \li for automatic generation of peripheral register debug information.
-*/
-#ifdef __cplusplus
- #define __I volatile /*!< Defines 'read only' permissions */
-#else
- #define __I volatile const /*!< Defines 'read only' permissions */
-#endif
-#define __O volatile /*!< Defines 'write only' permissions */
-#define __IO volatile /*!< Defines 'read / write' permissions */
-
-/*@} end of group Cortex_M3 */
-
-
-
-/*******************************************************************************
- * Register Abstraction
- Core Register contain:
- - Core Register
- - Core NVIC Register
- - Core SCB Register
- - Core SysTick Register
- - Core Debug Register
- - Core MPU Register
- ******************************************************************************/
-/** \defgroup CMSIS_core_register Defines and Type Definitions
- \brief Type definitions and defines for Cortex-M processor based devices.
-*/
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_CORE Status and Control Registers
- \brief Core Register type definitions.
- @{
- */
-
-/** \brief Union type to access the Application Program Status Register (APSR).
- */
-typedef union
-{
- struct
- {
-#if (__CORTEX_M != 0x04)
- uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */
-#else
- uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */
- uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
- uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */
-#endif
- uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
- uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
- uint32_t C:1; /*!< bit: 29 Carry condition code flag */
- uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
- uint32_t N:1; /*!< bit: 31 Negative condition code flag */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} APSR_Type;
-
-
-/** \brief Union type to access the Interrupt Program Status Register (IPSR).
- */
-typedef union
-{
- struct
- {
- uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
- uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} IPSR_Type;
-
-
-/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR).
- */
-typedef union
-{
- struct
- {
- uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
-#if (__CORTEX_M != 0x04)
- uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */
-#else
- uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */
- uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
- uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */
-#endif
- uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */
- uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */
- uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
- uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
- uint32_t C:1; /*!< bit: 29 Carry condition code flag */
- uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
- uint32_t N:1; /*!< bit: 31 Negative condition code flag */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} xPSR_Type;
-
-
-/** \brief Union type to access the Control Registers (CONTROL).
- */
-typedef union
-{
- struct
- {
- uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */
- uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */
- uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */
- uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} CONTROL_Type;
-
-/*@} end of group CMSIS_CORE */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC)
- \brief Type definitions for the NVIC Registers
- @{
- */
-
-/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC).
- */
-typedef struct
-{
- __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */
- uint32_t RESERVED0[24];
- __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */
- uint32_t RSERVED1[24];
- __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */
- uint32_t RESERVED2[24];
- __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */
- uint32_t RESERVED3[24];
- __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */
- uint32_t RESERVED4[56];
- __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */
- uint32_t RESERVED5[644];
- __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */
-} NVIC_Type;
-
-/* Software Triggered Interrupt Register Definitions */
-#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */
-#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */
-
-/*@} end of group CMSIS_NVIC */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_SCB System Control Block (SCB)
- \brief Type definitions for the System Control Block Registers
- @{
- */
-
-/** \brief Structure type to access the System Control Block (SCB).
- */
-typedef struct
-{
- __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */
- __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */
- __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */
- __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */
- __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */
- __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */
- __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */
- __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */
- __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */
- __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */
- __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */
- __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */
- __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */
- __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */
- __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */
- __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */
- __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */
- __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */
- __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */
- uint32_t RESERVED0[5];
- __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */
-} SCB_Type;
-
-/* SCB CPUID Register Definitions */
-#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */
-#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */
-
-#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */
-#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */
-
-#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */
-#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */
-
-#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */
-#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */
-
-#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */
-#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */
-
-/* SCB Interrupt Control State Register Definitions */
-#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */
-#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */
-
-#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */
-#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */
-
-#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */
-#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */
-
-#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */
-#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */
-
-#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */
-#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */
-
-#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */
-#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */
-
-#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */
-#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */
-
-#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */
-#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */
-
-#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */
-#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */
-
-#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */
-#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */
-
-/* SCB Vector Table Offset Register Definitions */
-#if (__CM3_REV < 0x0201) /* core r2p1 */
-#define SCB_VTOR_TBLBASE_Pos 29 /*!< SCB VTOR: TBLBASE Position */
-#define SCB_VTOR_TBLBASE_Msk (1UL << SCB_VTOR_TBLBASE_Pos) /*!< SCB VTOR: TBLBASE Mask */
-
-#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */
-#define SCB_VTOR_TBLOFF_Msk (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */
-#else
-#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */
-#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */
-#endif
-
-/* SCB Application Interrupt and Reset Control Register Definitions */
-#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */
-#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */
-
-#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */
-#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */
-
-#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */
-#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */
-
-#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */
-#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */
-
-#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */
-#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
-
-#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */
-#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */
-
-#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */
-#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */
-
-/* SCB System Control Register Definitions */
-#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */
-#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */
-
-#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */
-#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */
-
-#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */
-#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */
-
-/* SCB Configuration Control Register Definitions */
-#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */
-#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */
-
-#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */
-#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */
-
-#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */
-#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */
-
-#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */
-#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */
-
-#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */
-#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */
-
-#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */
-#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */
-
-/* SCB System Handler Control and State Register Definitions */
-#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */
-#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */
-
-#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */
-#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */
-
-#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */
-#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */
-
-#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */
-#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */
-
-#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */
-#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */
-
-#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */
-#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */
-
-#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */
-#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */
-
-#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */
-#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */
-
-#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */
-#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */
-
-#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */
-#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */
-
-#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */
-#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */
-
-#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */
-#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */
-
-#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */
-#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */
-
-#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */
-#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */
-
-/* SCB Configurable Fault Status Registers Definitions */
-#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */
-#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */
-
-#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */
-#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */
-
-#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */
-#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
-
-/* SCB Hard Fault Status Registers Definitions */
-#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */
-#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */
-
-#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */
-#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */
-
-#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */
-#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */
-
-/* SCB Debug Fault Status Register Definitions */
-#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */
-#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */
-
-#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */
-#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */
-
-#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */
-#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */
-
-#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */
-#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */
-
-#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */
-#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */
-
-/*@} end of group CMSIS_SCB */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
- \brief Type definitions for the System Control and ID Register not in the SCB
- @{
- */
-
-/** \brief Structure type to access the System Control and ID Register not in the SCB.
- */
-typedef struct
-{
- uint32_t RESERVED0[1];
- __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */
-#if ((defined __CM3_REV) && (__CM3_REV >= 0x200))
- __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */
-#else
- uint32_t RESERVED1[1];
-#endif
-} SCnSCB_Type;
-
-/* Interrupt Controller Type Register Definitions */
-#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */
-#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */
-
-/* Auxiliary Control Register Definitions */
-
-#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */
-#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */
-
-#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */
-#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */
-
-#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */
-#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */
-
-/*@} end of group CMSIS_SCnotSCB */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_SysTick System Tick Timer (SysTick)
- \brief Type definitions for the System Timer Registers.
- @{
- */
-
-/** \brief Structure type to access the System Timer (SysTick).
- */
-typedef struct
-{
- __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */
- __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */
- __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */
- __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */
-} SysTick_Type;
-
-/* SysTick Control / Status Register Definitions */
-#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */
-#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */
-
-#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */
-#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */
-
-#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */
-#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */
-
-#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */
-#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */
-
-/* SysTick Reload Register Definitions */
-#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */
-#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */
-
-/* SysTick Current Register Definitions */
-#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */
-#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */
-
-/* SysTick Calibration Register Definitions */
-#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */
-#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */
-
-#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */
-#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */
-
-#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */
-#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */
-
-/*@} end of group CMSIS_SysTick */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM)
- \brief Type definitions for the Instrumentation Trace Macrocell (ITM)
- @{
- */
-
-/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM).
- */
-typedef struct
-{
- __O union
- {
- __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */
- __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */
- __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */
- } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */
- uint32_t RESERVED0[864];
- __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */
- uint32_t RESERVED1[15];
- __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */
- uint32_t RESERVED2[15];
- __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */
- uint32_t RESERVED3[29];
- __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */
- __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */
- __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */
- uint32_t RESERVED4[43];
- __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */
- __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */
- uint32_t RESERVED5[6];
- __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */
- __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */
- __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */
- __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */
- __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */
- __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */
- __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */
- __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */
- __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */
- __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */
- __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */
- __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */
-} ITM_Type;
-
-/* ITM Trace Privilege Register Definitions */
-#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */
-#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */
-
-/* ITM Trace Control Register Definitions */
-#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */
-#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */
-
-#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */
-#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */
-
-#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */
-#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */
-
-#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */
-#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */
-
-#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */
-#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */
-
-#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */
-#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */
-
-#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */
-#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */
-
-#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */
-#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */
-
-#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */
-#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */
-
-/* ITM Integration Write Register Definitions */
-#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */
-#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */
-
-/* ITM Integration Read Register Definitions */
-#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */
-#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */
-
-/* ITM Integration Mode Control Register Definitions */
-#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */
-#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */
-
-/* ITM Lock Status Register Definitions */
-#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */
-#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */
-
-#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */
-#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */
-
-#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */
-#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */
-
-/*@}*/ /* end of group CMSIS_ITM */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT)
- \brief Type definitions for the Data Watchpoint and Trace (DWT)
- @{
- */
-
-/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT).
- */
-typedef struct
-{
- __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */
- __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */
- __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */
- __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */
- __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */
- __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */
- __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */
- __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */
- __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */
- __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */
- __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */
- uint32_t RESERVED0[1];
- __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */
- __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */
- __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */
- uint32_t RESERVED1[1];
- __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */
- __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */
- __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */
- uint32_t RESERVED2[1];
- __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */
- __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */
- __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */
-} DWT_Type;
-
-/* DWT Control Register Definitions */
-#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */
-#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */
-
-#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */
-#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */
-
-#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */
-#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */
-
-#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */
-#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */
-
-#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */
-#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */
-
-#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */
-#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */
-
-#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */
-#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */
-
-#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */
-#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */
-
-#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */
-#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */
-
-#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */
-#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */
-
-#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */
-#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */
-
-#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */
-#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */
-
-#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */
-#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */
-
-#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */
-#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */
-
-#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */
-#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */
-
-#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */
-#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */
-
-#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */
-#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */
-
-#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */
-#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */
-
-/* DWT CPI Count Register Definitions */
-#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */
-#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */
-
-/* DWT Exception Overhead Count Register Definitions */
-#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */
-#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */
-
-/* DWT Sleep Count Register Definitions */
-#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */
-#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */
-
-/* DWT LSU Count Register Definitions */
-#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */
-#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */
-
-/* DWT Folded-instruction Count Register Definitions */
-#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */
-#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */
-
-/* DWT Comparator Mask Register Definitions */
-#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */
-#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */
-
-/* DWT Comparator Function Register Definitions */
-#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */
-#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */
-
-#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */
-#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */
-
-#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */
-#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */
-
-#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */
-#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */
-
-#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */
-#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */
-
-#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */
-#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */
-
-#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */
-#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */
-
-#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */
-#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */
-
-#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */
-#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */
-
-/*@}*/ /* end of group CMSIS_DWT */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_TPI Trace Port Interface (TPI)
- \brief Type definitions for the Trace Port Interface (TPI)
- @{
- */
-
-/** \brief Structure type to access the Trace Port Interface Register (TPI).
- */
-typedef struct
-{
- __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */
- __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */
- uint32_t RESERVED0[2];
- __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */
- uint32_t RESERVED1[55];
- __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */
- uint32_t RESERVED2[131];
- __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */
- __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */
- __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */
- uint32_t RESERVED3[759];
- __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */
- __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */
- __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */
- uint32_t RESERVED4[1];
- __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */
- __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */
- __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */
- uint32_t RESERVED5[39];
- __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */
- __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */
- uint32_t RESERVED7[8];
- __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */
- __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */
-} TPI_Type;
-
-/* TPI Asynchronous Clock Prescaler Register Definitions */
-#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */
-#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */
-
-/* TPI Selected Pin Protocol Register Definitions */
-#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */
-#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */
-
-/* TPI Formatter and Flush Status Register Definitions */
-#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */
-#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */
-
-#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */
-#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */
-
-#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */
-#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */
-
-#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */
-#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */
-
-/* TPI Formatter and Flush Control Register Definitions */
-#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */
-#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */
-
-#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */
-#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */
-
-/* TPI TRIGGER Register Definitions */
-#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */
-#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */
-
-/* TPI Integration ETM Data Register Definitions (FIFO0) */
-#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */
-#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */
-
-#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */
-#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */
-
-#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */
-#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */
-
-#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */
-#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */
-
-#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */
-#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */
-
-#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */
-#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */
-
-#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */
-#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */
-
-/* TPI ITATBCTR2 Register Definitions */
-#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */
-#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */
-
-/* TPI Integration ITM Data Register Definitions (FIFO1) */
-#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */
-#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */
-
-#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */
-#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */
-
-#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */
-#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */
-
-#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */
-#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */
-
-#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */
-#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */
-
-#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */
-#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */
-
-#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */
-#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */
-
-/* TPI ITATBCTR0 Register Definitions */
-#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */
-#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */
-
-/* TPI Integration Mode Control Register Definitions */
-#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */
-#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */
-
-/* TPI DEVID Register Definitions */
-#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */
-#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */
-
-#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */
-#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */
-
-#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */
-#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */
-
-#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */
-#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */
-
-#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */
-#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */
-
-#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */
-#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */
-
-/* TPI DEVTYPE Register Definitions */
-#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */
-#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */
-
-#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */
-#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */
-
-/*@}*/ /* end of group CMSIS_TPI */
-
-
-#if (__MPU_PRESENT == 1)
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_MPU Memory Protection Unit (MPU)
- \brief Type definitions for the Memory Protection Unit (MPU)
- @{
- */
-
-/** \brief Structure type to access the Memory Protection Unit (MPU).
- */
-typedef struct
-{
- __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */
- __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */
- __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */
- __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */
- __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */
- __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */
- __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */
- __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */
- __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */
- __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */
- __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */
-} MPU_Type;
-
-/* MPU Type Register */
-#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */
-#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */
-
-#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */
-#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */
-
-#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */
-#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */
-
-/* MPU Control Register */
-#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */
-#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */
-
-#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */
-#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */
-
-#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */
-#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */
-
-/* MPU Region Number Register */
-#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */
-#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */
-
-/* MPU Region Base Address Register */
-#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */
-#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */
-
-#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */
-#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */
-
-#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */
-#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */
-
-/* MPU Region Attribute and Size Register */
-#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */
-#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */
-
-#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */
-#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */
-
-#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */
-#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */
-
-#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */
-#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */
-
-#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */
-#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */
-
-#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */
-#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */
-
-#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */
-#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */
-
-#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */
-#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */
-
-#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */
-#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */
-
-#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */
-#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */
-
-/*@} end of group CMSIS_MPU */
-#endif
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug)
- \brief Type definitions for the Core Debug Registers
- @{
- */
-
-/** \brief Structure type to access the Core Debug Register (CoreDebug).
- */
-typedef struct
-{
- __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */
- __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */
- __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */
- __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */
-} CoreDebug_Type;
-
-/* Debug Halting Control and Status Register */
-#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */
-#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */
-
-#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */
-#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */
-
-#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
-#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
-
-#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */
-#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */
-
-#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */
-#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */
-
-#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */
-#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */
-
-#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */
-#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */
-
-#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
-#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
-
-#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */
-#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */
-
-#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */
-#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */
-
-#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */
-#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */
-
-#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */
-#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
-
-/* Debug Core Register Selector Register */
-#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */
-#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */
-
-#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */
-#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */
-
-/* Debug Exception and Monitor Control Register */
-#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */
-#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */
-
-#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */
-#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */
-
-#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */
-#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */
-
-#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */
-#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */
-
-#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */
-#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */
-
-#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */
-#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */
-
-#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */
-#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */
-
-#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */
-#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */
-
-#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */
-#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */
-
-#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */
-#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */
-
-#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */
-#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
-
-#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */
-#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */
-
-#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */
-#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */
-
-/*@} end of group CMSIS_CoreDebug */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_core_base Core Definitions
- \brief Definitions for base addresses, unions, and structures.
- @{
- */
-
-/* Memory mapping of Cortex-M3 Hardware */
-#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */
-#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */
-#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */
-#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */
-#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */
-#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */
-#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */
-#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */
-
-#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */
-#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */
-#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */
-#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */
-#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */
-#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */
-#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */
-#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */
-
-#if (__MPU_PRESENT == 1)
- #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */
- #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */
-#endif
-
-/*@} */
-
-
-
-/*******************************************************************************
- * Hardware Abstraction Layer
- Core Function Interface contains:
- - Core NVIC Functions
- - Core SysTick Functions
- - Core Debug Functions
- - Core Register Access Functions
- ******************************************************************************/
-/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
-*/
-
-
-
-/* ########################## NVIC functions #################################### */
-/** \ingroup CMSIS_Core_FunctionInterface
- \defgroup CMSIS_Core_NVICFunctions NVIC Functions
- \brief Functions that manage interrupts and exceptions via the NVIC.
- @{
- */
-
-/** \brief Set Priority Grouping
-
- The function sets the priority grouping field using the required unlock sequence.
- The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
- Only values from 0..7 are used.
- In case of a conflict between priority grouping and available
- priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
-
- \param [in] PriorityGroup Priority grouping field.
- */
-__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
-{
- uint32_t reg_value;
- uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */
-
- reg_value = SCB->AIRCR; /* read old register configuration */
- reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */
- reg_value = (reg_value |
- ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
- (PriorityGroupTmp << 8)); /* Insert write key and priorty group */
- SCB->AIRCR = reg_value;
-}
-
-
-/** \brief Get Priority Grouping
-
- The function reads the priority grouping field from the NVIC Interrupt Controller.
-
- \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
- */
-__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
-{
- return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */
-}
-
-
-/** \brief Enable External Interrupt
-
- The function enables a device-specific interrupt in the NVIC interrupt controller.
-
- \param [in] IRQn External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
-{
- NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */
-}
-
-
-/** \brief Disable External Interrupt
-
- The function disables a device-specific interrupt in the NVIC interrupt controller.
-
- \param [in] IRQn External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
-{
- NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
-}
-
-
-/** \brief Get Pending Interrupt
-
- The function reads the pending register in the NVIC and returns the pending bit
- for the specified interrupt.
-
- \param [in] IRQn Interrupt number.
-
- \return 0 Interrupt status is not pending.
- \return 1 Interrupt status is pending.
- */
-__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
-{
- return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
-}
-
-
-/** \brief Set Pending Interrupt
-
- The function sets the pending bit of an external interrupt.
-
- \param [in] IRQn Interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
-{
- NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
-}
-
-
-/** \brief Clear Pending Interrupt
-
- The function clears the pending bit of an external interrupt.
-
- \param [in] IRQn External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
-{
- NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
-}
-
-
-/** \brief Get Active Interrupt
-
- The function reads the active register in NVIC and returns the active bit.
-
- \param [in] IRQn Interrupt number.
-
- \return 0 Interrupt status is not active.
- \return 1 Interrupt status is active.
- */
-__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
-{
- return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
-}
-
-
-/** \brief Set Interrupt Priority
-
- The function sets the priority of an interrupt.
-
- \note The priority cannot be set for every core interrupt.
-
- \param [in] IRQn Interrupt number.
- \param [in] priority Priority to set.
- */
-__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
-{
- if(IRQn < 0) {
- SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */
- else {
- NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */
-}
-
-
-/** \brief Get Interrupt Priority
-
- The function reads the priority of an interrupt. The interrupt
- number can be positive to specify an external (device specific)
- interrupt, or negative to specify an internal (core) interrupt.
-
-
- \param [in] IRQn Interrupt number.
- \return Interrupt Priority. Value is aligned automatically to the implemented
- priority bits of the microcontroller.
- */
-__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
-{
-
- if(IRQn < 0) {
- return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */
- else {
- return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */
-}
-
-
-/** \brief Encode Priority
-
- The function encodes the priority for an interrupt with the given priority group,
- preemptive priority value, and subpriority value.
- In case of a conflict between priority grouping and available
- priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
-
- \param [in] PriorityGroup Used priority group.
- \param [in] PreemptPriority Preemptive priority value (starting from 0).
- \param [in] SubPriority Subpriority value (starting from 0).
- \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
- */
-__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
-{
- uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
- uint32_t PreemptPriorityBits;
- uint32_t SubPriorityBits;
-
- PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
- SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
- return (
- ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
- ((SubPriority & ((1 << (SubPriorityBits )) - 1)))
- );
-}
-
-
-/** \brief Decode Priority
-
- The function decodes an interrupt priority value with a given priority group to
- preemptive priority value and subpriority value.
- In case of a conflict between priority grouping and available
- priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
-
- \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
- \param [in] PriorityGroup Used priority group.
- \param [out] pPreemptPriority Preemptive priority value (starting from 0).
- \param [out] pSubPriority Subpriority value (starting from 0).
- */
-__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
-{
- uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
- uint32_t PreemptPriorityBits;
- uint32_t SubPriorityBits;
-
- PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
- SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
- *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
- *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1);
-}
-
-
-/** \brief System Reset
-
- The function initiates a system reset request to reset the MCU.
- */
-__STATIC_INLINE void NVIC_SystemReset(void)
-{
- __DSB(); /* Ensure all outstanding memory accesses included
- buffered write are completed before reset */
- SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |
- (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
- SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */
- __DSB(); /* Ensure completion of memory access */
- while(1); /* wait until reset */
-}
-
-/*@} end of CMSIS_Core_NVICFunctions */
-
-
-
-/* ################################## SysTick function ############################################ */
-/** \ingroup CMSIS_Core_FunctionInterface
- \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
- \brief Functions that configure the System.
- @{
- */
-
-#if (__Vendor_SysTickConfig == 0)
-
-/** \brief System Tick Configuration
-
- The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
- Counter is in free running mode to generate periodic interrupts.
-
- \param [in] ticks Number of ticks between two interrupts.
-
- \return 0 Function succeeded.
- \return 1 Function failed.
-
- \note When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
- function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
- must contain a vendor-specific implementation of this function.
-
- */
-__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
-{
- if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */
-
- SysTick->LOAD = ticks - 1; /* set reload register */
- NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */
- SysTick->VAL = 0; /* Load the SysTick Counter Value */
- SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
- SysTick_CTRL_TICKINT_Msk |
- SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
- return (0); /* Function successful */
-}
-
-#endif
-
-/*@} end of CMSIS_Core_SysTickFunctions */
-
-
-
-/* ##################################### Debug In/Output function ########################################### */
-/** \ingroup CMSIS_Core_FunctionInterface
- \defgroup CMSIS_core_DebugFunctions ITM Functions
- \brief Functions that access the ITM debug interface.
- @{
- */
-
-extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */
-#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
-
-
-/** \brief ITM Send Character
-
- The function transmits a character via the ITM channel 0, and
- \li Just returns when no debugger is connected that has booked the output.
- \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
-
- \param [in] ch Character to transmit.
-
- \returns Character to transmit.
- */
-__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
-{
- if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */
- (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */
- {
- while (ITM->PORT[0].u32 == 0);
- ITM->PORT[0].u8 = (uint8_t) ch;
- }
- return (ch);
-}
-
-
-/** \brief ITM Receive Character
-
- The function inputs a character via the external variable \ref ITM_RxBuffer.
-
- \return Received character.
- \return -1 No character pending.
- */
-__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
- int32_t ch = -1; /* no character available */
-
- if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
- ch = ITM_RxBuffer;
- ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */
- }
-
- return (ch);
-}
-
-
-/** \brief ITM Check Character
-
- The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
-
- \return 0 No character available.
- \return 1 Character available.
- */
-__STATIC_INLINE int32_t ITM_CheckChar (void) {
-
- if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
- return (0); /* no character available */
- } else {
- return (1); /* character available */
- }
-}
-
-/*@} end of CMSIS_core_DebugFunctions */
-
-#endif /* __CORE_CM3_H_DEPENDANT */
-
-#endif /* __CMSIS_GENERIC */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/modules/mathlib/CMSIS/Include/core_cm4.h deleted file mode 100644 index 93efd3a7a..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cm4.h +++ /dev/null @@ -1,1772 +0,0 @@ -/**************************************************************************//**
- * @file core_cm4.h
- * @brief CMSIS Cortex-M4 Core Peripheral Access Layer Header File
- * @version V3.20
- * @date 25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
- 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 ARM 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 COPYRIGHT HOLDERS AND 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.
- ---------------------------------------------------------------------------*/
-
-
-#if defined ( __ICCARM__ )
- #pragma system_include /* treat file as system include file for MISRA check */
-#endif
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-#ifndef __CORE_CM4_H_GENERIC
-#define __CORE_CM4_H_GENERIC
-
-/** \page CMSIS_MISRA_Exceptions MISRA-C:2004 Compliance Exceptions
- CMSIS violates the following MISRA-C:2004 rules:
-
- \li Required Rule 8.5, object/function definition in header file.<br>
- Function definitions in header files are used to allow 'inlining'.
-
- \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>
- Unions are used for effective representation of core registers.
-
- \li Advisory Rule 19.7, Function-like macro defined.<br>
- Function-like macros are used to allow more efficient code.
- */
-
-
-/*******************************************************************************
- * CMSIS definitions
- ******************************************************************************/
-/** \ingroup Cortex_M4
- @{
- */
-
-/* CMSIS CM4 definitions */
-#define __CM4_CMSIS_VERSION_MAIN (0x03) /*!< [31:16] CMSIS HAL main version */
-#define __CM4_CMSIS_VERSION_SUB (0x20) /*!< [15:0] CMSIS HAL sub version */
-#define __CM4_CMSIS_VERSION ((__CM4_CMSIS_VERSION_MAIN << 16) | \
- __CM4_CMSIS_VERSION_SUB ) /*!< CMSIS HAL version number */
-
-#define __CORTEX_M (0x04) /*!< Cortex-M Core */
-
-
-#if defined ( __CC_ARM )
- #define __ASM __asm /*!< asm keyword for ARM Compiler */
- #define __INLINE __inline /*!< inline keyword for ARM Compiler */
- #define __STATIC_INLINE static __inline
-
-#elif defined ( __ICCARM__ )
- #define __ASM __asm /*!< asm keyword for IAR Compiler */
- #define __INLINE inline /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */
- #define __STATIC_INLINE static inline
-
-#elif defined ( __TMS470__ )
- #define __ASM __asm /*!< asm keyword for TI CCS Compiler */
- #define __STATIC_INLINE static inline
-
-#elif defined ( __GNUC__ )
- #define __ASM __asm /*!< asm keyword for GNU Compiler */
- #define __INLINE inline /*!< inline keyword for GNU Compiler */
- #define __STATIC_INLINE static inline
-
-#elif defined ( __TASKING__ )
- #define __ASM __asm /*!< asm keyword for TASKING Compiler */
- #define __INLINE inline /*!< inline keyword for TASKING Compiler */
- #define __STATIC_INLINE static inline
-
-#endif
-
-/** __FPU_USED indicates whether an FPU is used or not. For this, __FPU_PRESENT has to be checked prior to making use of FPU specific registers and functions.
-*/
-#if defined ( __CC_ARM )
- #if defined __TARGET_FPU_VFP
- #if (__FPU_PRESENT == 1)
- #define __FPU_USED 1
- #else
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #define __FPU_USED 0
- #endif
- #else
- #define __FPU_USED 0
- #endif
-
-#elif defined ( __ICCARM__ )
- #if defined __ARMVFP__
- #if (__FPU_PRESENT == 1)
- #define __FPU_USED 1
- #else
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #define __FPU_USED 0
- #endif
- #else
- #define __FPU_USED 0
- #endif
-
-#elif defined ( __TMS470__ )
- #if defined __TI_VFP_SUPPORT__
- #if (__FPU_PRESENT == 1)
- #define __FPU_USED 1
- #else
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #define __FPU_USED 0
- #endif
- #else
- #define __FPU_USED 0
- #endif
-
-#elif defined ( __GNUC__ )
- #if defined (__VFP_FP__) && !defined(__SOFTFP__)
- #if (__FPU_PRESENT == 1)
- #define __FPU_USED 1
- #else
- #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #define __FPU_USED 0
- #endif
- #else
- #define __FPU_USED 0
- #endif
-
-#elif defined ( __TASKING__ )
- #if defined __FPU_VFP__
- #if (__FPU_PRESENT == 1)
- #define __FPU_USED 1
- #else
- #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"
- #define __FPU_USED 0
- #endif
- #else
- #define __FPU_USED 0
- #endif
-#endif
-
-#include <stdint.h> /* standard types definitions */
-#include "core_cmInstr.h" /* Core Instruction Access */
-#include "core_cmFunc.h" /* Core Function Access */
-#include "core_cm4_simd.h" /* Compiler specific SIMD Intrinsics */
-
-#endif /* __CORE_CM4_H_GENERIC */
-
-#ifndef __CMSIS_GENERIC
-
-#ifndef __CORE_CM4_H_DEPENDANT
-#define __CORE_CM4_H_DEPENDANT
-
-/* check device defines and use defaults */
-#if defined __CHECK_DEVICE_DEFINES
- #ifndef __CM4_REV
- #define __CM4_REV 0x0000
- #warning "__CM4_REV not defined in device header file; using default!"
- #endif
-
- #ifndef __FPU_PRESENT
- #define __FPU_PRESENT 0
- #warning "__FPU_PRESENT not defined in device header file; using default!"
- #endif
-
- #ifndef __MPU_PRESENT
- #define __MPU_PRESENT 0
- #warning "__MPU_PRESENT not defined in device header file; using default!"
- #endif
-
- #ifndef __NVIC_PRIO_BITS
- #define __NVIC_PRIO_BITS 4
- #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"
- #endif
-
- #ifndef __Vendor_SysTickConfig
- #define __Vendor_SysTickConfig 0
- #warning "__Vendor_SysTickConfig not defined in device header file; using default!"
- #endif
-#endif
-
-/* IO definitions (access restrictions to peripheral registers) */
-/**
- \defgroup CMSIS_glob_defs CMSIS Global Defines
-
- <strong>IO Type Qualifiers</strong> are used
- \li to specify the access to peripheral variables.
- \li for automatic generation of peripheral register debug information.
-*/
-#ifdef __cplusplus
- #define __I volatile /*!< Defines 'read only' permissions */
-#else
- #define __I volatile const /*!< Defines 'read only' permissions */
-#endif
-#define __O volatile /*!< Defines 'write only' permissions */
-#define __IO volatile /*!< Defines 'read / write' permissions */
-
-/*@} end of group Cortex_M4 */
-
-
-
-/*******************************************************************************
- * Register Abstraction
- Core Register contain:
- - Core Register
- - Core NVIC Register
- - Core SCB Register
- - Core SysTick Register
- - Core Debug Register
- - Core MPU Register
- - Core FPU Register
- ******************************************************************************/
-/** \defgroup CMSIS_core_register Defines and Type Definitions
- \brief Type definitions and defines for Cortex-M processor based devices.
-*/
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_CORE Status and Control Registers
- \brief Core Register type definitions.
- @{
- */
-
-/** \brief Union type to access the Application Program Status Register (APSR).
- */
-typedef union
-{
- struct
- {
-#if (__CORTEX_M != 0x04)
- uint32_t _reserved0:27; /*!< bit: 0..26 Reserved */
-#else
- uint32_t _reserved0:16; /*!< bit: 0..15 Reserved */
- uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
- uint32_t _reserved1:7; /*!< bit: 20..26 Reserved */
-#endif
- uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
- uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
- uint32_t C:1; /*!< bit: 29 Carry condition code flag */
- uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
- uint32_t N:1; /*!< bit: 31 Negative condition code flag */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} APSR_Type;
-
-
-/** \brief Union type to access the Interrupt Program Status Register (IPSR).
- */
-typedef union
-{
- struct
- {
- uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
- uint32_t _reserved0:23; /*!< bit: 9..31 Reserved */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} IPSR_Type;
-
-
-/** \brief Union type to access the Special-Purpose Program Status Registers (xPSR).
- */
-typedef union
-{
- struct
- {
- uint32_t ISR:9; /*!< bit: 0.. 8 Exception number */
-#if (__CORTEX_M != 0x04)
- uint32_t _reserved0:15; /*!< bit: 9..23 Reserved */
-#else
- uint32_t _reserved0:7; /*!< bit: 9..15 Reserved */
- uint32_t GE:4; /*!< bit: 16..19 Greater than or Equal flags */
- uint32_t _reserved1:4; /*!< bit: 20..23 Reserved */
-#endif
- uint32_t T:1; /*!< bit: 24 Thumb bit (read 0) */
- uint32_t IT:2; /*!< bit: 25..26 saved IT state (read 0) */
- uint32_t Q:1; /*!< bit: 27 Saturation condition flag */
- uint32_t V:1; /*!< bit: 28 Overflow condition code flag */
- uint32_t C:1; /*!< bit: 29 Carry condition code flag */
- uint32_t Z:1; /*!< bit: 30 Zero condition code flag */
- uint32_t N:1; /*!< bit: 31 Negative condition code flag */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} xPSR_Type;
-
-
-/** \brief Union type to access the Control Registers (CONTROL).
- */
-typedef union
-{
- struct
- {
- uint32_t nPRIV:1; /*!< bit: 0 Execution privilege in Thread mode */
- uint32_t SPSEL:1; /*!< bit: 1 Stack to be used */
- uint32_t FPCA:1; /*!< bit: 2 FP extension active flag */
- uint32_t _reserved0:29; /*!< bit: 3..31 Reserved */
- } b; /*!< Structure used for bit access */
- uint32_t w; /*!< Type used for word access */
-} CONTROL_Type;
-
-/*@} end of group CMSIS_CORE */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_NVIC Nested Vectored Interrupt Controller (NVIC)
- \brief Type definitions for the NVIC Registers
- @{
- */
-
-/** \brief Structure type to access the Nested Vectored Interrupt Controller (NVIC).
- */
-typedef struct
-{
- __IO uint32_t ISER[8]; /*!< Offset: 0x000 (R/W) Interrupt Set Enable Register */
- uint32_t RESERVED0[24];
- __IO uint32_t ICER[8]; /*!< Offset: 0x080 (R/W) Interrupt Clear Enable Register */
- uint32_t RSERVED1[24];
- __IO uint32_t ISPR[8]; /*!< Offset: 0x100 (R/W) Interrupt Set Pending Register */
- uint32_t RESERVED2[24];
- __IO uint32_t ICPR[8]; /*!< Offset: 0x180 (R/W) Interrupt Clear Pending Register */
- uint32_t RESERVED3[24];
- __IO uint32_t IABR[8]; /*!< Offset: 0x200 (R/W) Interrupt Active bit Register */
- uint32_t RESERVED4[56];
- __IO uint8_t IP[240]; /*!< Offset: 0x300 (R/W) Interrupt Priority Register (8Bit wide) */
- uint32_t RESERVED5[644];
- __O uint32_t STIR; /*!< Offset: 0xE00 ( /W) Software Trigger Interrupt Register */
-} NVIC_Type;
-
-/* Software Triggered Interrupt Register Definitions */
-#define NVIC_STIR_INTID_Pos 0 /*!< STIR: INTLINESNUM Position */
-#define NVIC_STIR_INTID_Msk (0x1FFUL << NVIC_STIR_INTID_Pos) /*!< STIR: INTLINESNUM Mask */
-
-/*@} end of group CMSIS_NVIC */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_SCB System Control Block (SCB)
- \brief Type definitions for the System Control Block Registers
- @{
- */
-
-/** \brief Structure type to access the System Control Block (SCB).
- */
-typedef struct
-{
- __I uint32_t CPUID; /*!< Offset: 0x000 (R/ ) CPUID Base Register */
- __IO uint32_t ICSR; /*!< Offset: 0x004 (R/W) Interrupt Control and State Register */
- __IO uint32_t VTOR; /*!< Offset: 0x008 (R/W) Vector Table Offset Register */
- __IO uint32_t AIRCR; /*!< Offset: 0x00C (R/W) Application Interrupt and Reset Control Register */
- __IO uint32_t SCR; /*!< Offset: 0x010 (R/W) System Control Register */
- __IO uint32_t CCR; /*!< Offset: 0x014 (R/W) Configuration Control Register */
- __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W) System Handlers Priority Registers (4-7, 8-11, 12-15) */
- __IO uint32_t SHCSR; /*!< Offset: 0x024 (R/W) System Handler Control and State Register */
- __IO uint32_t CFSR; /*!< Offset: 0x028 (R/W) Configurable Fault Status Register */
- __IO uint32_t HFSR; /*!< Offset: 0x02C (R/W) HardFault Status Register */
- __IO uint32_t DFSR; /*!< Offset: 0x030 (R/W) Debug Fault Status Register */
- __IO uint32_t MMFAR; /*!< Offset: 0x034 (R/W) MemManage Fault Address Register */
- __IO uint32_t BFAR; /*!< Offset: 0x038 (R/W) BusFault Address Register */
- __IO uint32_t AFSR; /*!< Offset: 0x03C (R/W) Auxiliary Fault Status Register */
- __I uint32_t PFR[2]; /*!< Offset: 0x040 (R/ ) Processor Feature Register */
- __I uint32_t DFR; /*!< Offset: 0x048 (R/ ) Debug Feature Register */
- __I uint32_t ADR; /*!< Offset: 0x04C (R/ ) Auxiliary Feature Register */
- __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ ) Memory Model Feature Register */
- __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ ) Instruction Set Attributes Register */
- uint32_t RESERVED0[5];
- __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W) Coprocessor Access Control Register */
-} SCB_Type;
-
-/* SCB CPUID Register Definitions */
-#define SCB_CPUID_IMPLEMENTER_Pos 24 /*!< SCB CPUID: IMPLEMENTER Position */
-#define SCB_CPUID_IMPLEMENTER_Msk (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos) /*!< SCB CPUID: IMPLEMENTER Mask */
-
-#define SCB_CPUID_VARIANT_Pos 20 /*!< SCB CPUID: VARIANT Position */
-#define SCB_CPUID_VARIANT_Msk (0xFUL << SCB_CPUID_VARIANT_Pos) /*!< SCB CPUID: VARIANT Mask */
-
-#define SCB_CPUID_ARCHITECTURE_Pos 16 /*!< SCB CPUID: ARCHITECTURE Position */
-#define SCB_CPUID_ARCHITECTURE_Msk (0xFUL << SCB_CPUID_ARCHITECTURE_Pos) /*!< SCB CPUID: ARCHITECTURE Mask */
-
-#define SCB_CPUID_PARTNO_Pos 4 /*!< SCB CPUID: PARTNO Position */
-#define SCB_CPUID_PARTNO_Msk (0xFFFUL << SCB_CPUID_PARTNO_Pos) /*!< SCB CPUID: PARTNO Mask */
-
-#define SCB_CPUID_REVISION_Pos 0 /*!< SCB CPUID: REVISION Position */
-#define SCB_CPUID_REVISION_Msk (0xFUL << SCB_CPUID_REVISION_Pos) /*!< SCB CPUID: REVISION Mask */
-
-/* SCB Interrupt Control State Register Definitions */
-#define SCB_ICSR_NMIPENDSET_Pos 31 /*!< SCB ICSR: NMIPENDSET Position */
-#define SCB_ICSR_NMIPENDSET_Msk (1UL << SCB_ICSR_NMIPENDSET_Pos) /*!< SCB ICSR: NMIPENDSET Mask */
-
-#define SCB_ICSR_PENDSVSET_Pos 28 /*!< SCB ICSR: PENDSVSET Position */
-#define SCB_ICSR_PENDSVSET_Msk (1UL << SCB_ICSR_PENDSVSET_Pos) /*!< SCB ICSR: PENDSVSET Mask */
-
-#define SCB_ICSR_PENDSVCLR_Pos 27 /*!< SCB ICSR: PENDSVCLR Position */
-#define SCB_ICSR_PENDSVCLR_Msk (1UL << SCB_ICSR_PENDSVCLR_Pos) /*!< SCB ICSR: PENDSVCLR Mask */
-
-#define SCB_ICSR_PENDSTSET_Pos 26 /*!< SCB ICSR: PENDSTSET Position */
-#define SCB_ICSR_PENDSTSET_Msk (1UL << SCB_ICSR_PENDSTSET_Pos) /*!< SCB ICSR: PENDSTSET Mask */
-
-#define SCB_ICSR_PENDSTCLR_Pos 25 /*!< SCB ICSR: PENDSTCLR Position */
-#define SCB_ICSR_PENDSTCLR_Msk (1UL << SCB_ICSR_PENDSTCLR_Pos) /*!< SCB ICSR: PENDSTCLR Mask */
-
-#define SCB_ICSR_ISRPREEMPT_Pos 23 /*!< SCB ICSR: ISRPREEMPT Position */
-#define SCB_ICSR_ISRPREEMPT_Msk (1UL << SCB_ICSR_ISRPREEMPT_Pos) /*!< SCB ICSR: ISRPREEMPT Mask */
-
-#define SCB_ICSR_ISRPENDING_Pos 22 /*!< SCB ICSR: ISRPENDING Position */
-#define SCB_ICSR_ISRPENDING_Msk (1UL << SCB_ICSR_ISRPENDING_Pos) /*!< SCB ICSR: ISRPENDING Mask */
-
-#define SCB_ICSR_VECTPENDING_Pos 12 /*!< SCB ICSR: VECTPENDING Position */
-#define SCB_ICSR_VECTPENDING_Msk (0x1FFUL << SCB_ICSR_VECTPENDING_Pos) /*!< SCB ICSR: VECTPENDING Mask */
-
-#define SCB_ICSR_RETTOBASE_Pos 11 /*!< SCB ICSR: RETTOBASE Position */
-#define SCB_ICSR_RETTOBASE_Msk (1UL << SCB_ICSR_RETTOBASE_Pos) /*!< SCB ICSR: RETTOBASE Mask */
-
-#define SCB_ICSR_VECTACTIVE_Pos 0 /*!< SCB ICSR: VECTACTIVE Position */
-#define SCB_ICSR_VECTACTIVE_Msk (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos) /*!< SCB ICSR: VECTACTIVE Mask */
-
-/* SCB Vector Table Offset Register Definitions */
-#define SCB_VTOR_TBLOFF_Pos 7 /*!< SCB VTOR: TBLOFF Position */
-#define SCB_VTOR_TBLOFF_Msk (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos) /*!< SCB VTOR: TBLOFF Mask */
-
-/* SCB Application Interrupt and Reset Control Register Definitions */
-#define SCB_AIRCR_VECTKEY_Pos 16 /*!< SCB AIRCR: VECTKEY Position */
-#define SCB_AIRCR_VECTKEY_Msk (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos) /*!< SCB AIRCR: VECTKEY Mask */
-
-#define SCB_AIRCR_VECTKEYSTAT_Pos 16 /*!< SCB AIRCR: VECTKEYSTAT Position */
-#define SCB_AIRCR_VECTKEYSTAT_Msk (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos) /*!< SCB AIRCR: VECTKEYSTAT Mask */
-
-#define SCB_AIRCR_ENDIANESS_Pos 15 /*!< SCB AIRCR: ENDIANESS Position */
-#define SCB_AIRCR_ENDIANESS_Msk (1UL << SCB_AIRCR_ENDIANESS_Pos) /*!< SCB AIRCR: ENDIANESS Mask */
-
-#define SCB_AIRCR_PRIGROUP_Pos 8 /*!< SCB AIRCR: PRIGROUP Position */
-#define SCB_AIRCR_PRIGROUP_Msk (7UL << SCB_AIRCR_PRIGROUP_Pos) /*!< SCB AIRCR: PRIGROUP Mask */
-
-#define SCB_AIRCR_SYSRESETREQ_Pos 2 /*!< SCB AIRCR: SYSRESETREQ Position */
-#define SCB_AIRCR_SYSRESETREQ_Msk (1UL << SCB_AIRCR_SYSRESETREQ_Pos) /*!< SCB AIRCR: SYSRESETREQ Mask */
-
-#define SCB_AIRCR_VECTCLRACTIVE_Pos 1 /*!< SCB AIRCR: VECTCLRACTIVE Position */
-#define SCB_AIRCR_VECTCLRACTIVE_Msk (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos) /*!< SCB AIRCR: VECTCLRACTIVE Mask */
-
-#define SCB_AIRCR_VECTRESET_Pos 0 /*!< SCB AIRCR: VECTRESET Position */
-#define SCB_AIRCR_VECTRESET_Msk (1UL << SCB_AIRCR_VECTRESET_Pos) /*!< SCB AIRCR: VECTRESET Mask */
-
-/* SCB System Control Register Definitions */
-#define SCB_SCR_SEVONPEND_Pos 4 /*!< SCB SCR: SEVONPEND Position */
-#define SCB_SCR_SEVONPEND_Msk (1UL << SCB_SCR_SEVONPEND_Pos) /*!< SCB SCR: SEVONPEND Mask */
-
-#define SCB_SCR_SLEEPDEEP_Pos 2 /*!< SCB SCR: SLEEPDEEP Position */
-#define SCB_SCR_SLEEPDEEP_Msk (1UL << SCB_SCR_SLEEPDEEP_Pos) /*!< SCB SCR: SLEEPDEEP Mask */
-
-#define SCB_SCR_SLEEPONEXIT_Pos 1 /*!< SCB SCR: SLEEPONEXIT Position */
-#define SCB_SCR_SLEEPONEXIT_Msk (1UL << SCB_SCR_SLEEPONEXIT_Pos) /*!< SCB SCR: SLEEPONEXIT Mask */
-
-/* SCB Configuration Control Register Definitions */
-#define SCB_CCR_STKALIGN_Pos 9 /*!< SCB CCR: STKALIGN Position */
-#define SCB_CCR_STKALIGN_Msk (1UL << SCB_CCR_STKALIGN_Pos) /*!< SCB CCR: STKALIGN Mask */
-
-#define SCB_CCR_BFHFNMIGN_Pos 8 /*!< SCB CCR: BFHFNMIGN Position */
-#define SCB_CCR_BFHFNMIGN_Msk (1UL << SCB_CCR_BFHFNMIGN_Pos) /*!< SCB CCR: BFHFNMIGN Mask */
-
-#define SCB_CCR_DIV_0_TRP_Pos 4 /*!< SCB CCR: DIV_0_TRP Position */
-#define SCB_CCR_DIV_0_TRP_Msk (1UL << SCB_CCR_DIV_0_TRP_Pos) /*!< SCB CCR: DIV_0_TRP Mask */
-
-#define SCB_CCR_UNALIGN_TRP_Pos 3 /*!< SCB CCR: UNALIGN_TRP Position */
-#define SCB_CCR_UNALIGN_TRP_Msk (1UL << SCB_CCR_UNALIGN_TRP_Pos) /*!< SCB CCR: UNALIGN_TRP Mask */
-
-#define SCB_CCR_USERSETMPEND_Pos 1 /*!< SCB CCR: USERSETMPEND Position */
-#define SCB_CCR_USERSETMPEND_Msk (1UL << SCB_CCR_USERSETMPEND_Pos) /*!< SCB CCR: USERSETMPEND Mask */
-
-#define SCB_CCR_NONBASETHRDENA_Pos 0 /*!< SCB CCR: NONBASETHRDENA Position */
-#define SCB_CCR_NONBASETHRDENA_Msk (1UL << SCB_CCR_NONBASETHRDENA_Pos) /*!< SCB CCR: NONBASETHRDENA Mask */
-
-/* SCB System Handler Control and State Register Definitions */
-#define SCB_SHCSR_USGFAULTENA_Pos 18 /*!< SCB SHCSR: USGFAULTENA Position */
-#define SCB_SHCSR_USGFAULTENA_Msk (1UL << SCB_SHCSR_USGFAULTENA_Pos) /*!< SCB SHCSR: USGFAULTENA Mask */
-
-#define SCB_SHCSR_BUSFAULTENA_Pos 17 /*!< SCB SHCSR: BUSFAULTENA Position */
-#define SCB_SHCSR_BUSFAULTENA_Msk (1UL << SCB_SHCSR_BUSFAULTENA_Pos) /*!< SCB SHCSR: BUSFAULTENA Mask */
-
-#define SCB_SHCSR_MEMFAULTENA_Pos 16 /*!< SCB SHCSR: MEMFAULTENA Position */
-#define SCB_SHCSR_MEMFAULTENA_Msk (1UL << SCB_SHCSR_MEMFAULTENA_Pos) /*!< SCB SHCSR: MEMFAULTENA Mask */
-
-#define SCB_SHCSR_SVCALLPENDED_Pos 15 /*!< SCB SHCSR: SVCALLPENDED Position */
-#define SCB_SHCSR_SVCALLPENDED_Msk (1UL << SCB_SHCSR_SVCALLPENDED_Pos) /*!< SCB SHCSR: SVCALLPENDED Mask */
-
-#define SCB_SHCSR_BUSFAULTPENDED_Pos 14 /*!< SCB SHCSR: BUSFAULTPENDED Position */
-#define SCB_SHCSR_BUSFAULTPENDED_Msk (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos) /*!< SCB SHCSR: BUSFAULTPENDED Mask */
-
-#define SCB_SHCSR_MEMFAULTPENDED_Pos 13 /*!< SCB SHCSR: MEMFAULTPENDED Position */
-#define SCB_SHCSR_MEMFAULTPENDED_Msk (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos) /*!< SCB SHCSR: MEMFAULTPENDED Mask */
-
-#define SCB_SHCSR_USGFAULTPENDED_Pos 12 /*!< SCB SHCSR: USGFAULTPENDED Position */
-#define SCB_SHCSR_USGFAULTPENDED_Msk (1UL << SCB_SHCSR_USGFAULTPENDED_Pos) /*!< SCB SHCSR: USGFAULTPENDED Mask */
-
-#define SCB_SHCSR_SYSTICKACT_Pos 11 /*!< SCB SHCSR: SYSTICKACT Position */
-#define SCB_SHCSR_SYSTICKACT_Msk (1UL << SCB_SHCSR_SYSTICKACT_Pos) /*!< SCB SHCSR: SYSTICKACT Mask */
-
-#define SCB_SHCSR_PENDSVACT_Pos 10 /*!< SCB SHCSR: PENDSVACT Position */
-#define SCB_SHCSR_PENDSVACT_Msk (1UL << SCB_SHCSR_PENDSVACT_Pos) /*!< SCB SHCSR: PENDSVACT Mask */
-
-#define SCB_SHCSR_MONITORACT_Pos 8 /*!< SCB SHCSR: MONITORACT Position */
-#define SCB_SHCSR_MONITORACT_Msk (1UL << SCB_SHCSR_MONITORACT_Pos) /*!< SCB SHCSR: MONITORACT Mask */
-
-#define SCB_SHCSR_SVCALLACT_Pos 7 /*!< SCB SHCSR: SVCALLACT Position */
-#define SCB_SHCSR_SVCALLACT_Msk (1UL << SCB_SHCSR_SVCALLACT_Pos) /*!< SCB SHCSR: SVCALLACT Mask */
-
-#define SCB_SHCSR_USGFAULTACT_Pos 3 /*!< SCB SHCSR: USGFAULTACT Position */
-#define SCB_SHCSR_USGFAULTACT_Msk (1UL << SCB_SHCSR_USGFAULTACT_Pos) /*!< SCB SHCSR: USGFAULTACT Mask */
-
-#define SCB_SHCSR_BUSFAULTACT_Pos 1 /*!< SCB SHCSR: BUSFAULTACT Position */
-#define SCB_SHCSR_BUSFAULTACT_Msk (1UL << SCB_SHCSR_BUSFAULTACT_Pos) /*!< SCB SHCSR: BUSFAULTACT Mask */
-
-#define SCB_SHCSR_MEMFAULTACT_Pos 0 /*!< SCB SHCSR: MEMFAULTACT Position */
-#define SCB_SHCSR_MEMFAULTACT_Msk (1UL << SCB_SHCSR_MEMFAULTACT_Pos) /*!< SCB SHCSR: MEMFAULTACT Mask */
-
-/* SCB Configurable Fault Status Registers Definitions */
-#define SCB_CFSR_USGFAULTSR_Pos 16 /*!< SCB CFSR: Usage Fault Status Register Position */
-#define SCB_CFSR_USGFAULTSR_Msk (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos) /*!< SCB CFSR: Usage Fault Status Register Mask */
-
-#define SCB_CFSR_BUSFAULTSR_Pos 8 /*!< SCB CFSR: Bus Fault Status Register Position */
-#define SCB_CFSR_BUSFAULTSR_Msk (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos) /*!< SCB CFSR: Bus Fault Status Register Mask */
-
-#define SCB_CFSR_MEMFAULTSR_Pos 0 /*!< SCB CFSR: Memory Manage Fault Status Register Position */
-#define SCB_CFSR_MEMFAULTSR_Msk (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos) /*!< SCB CFSR: Memory Manage Fault Status Register Mask */
-
-/* SCB Hard Fault Status Registers Definitions */
-#define SCB_HFSR_DEBUGEVT_Pos 31 /*!< SCB HFSR: DEBUGEVT Position */
-#define SCB_HFSR_DEBUGEVT_Msk (1UL << SCB_HFSR_DEBUGEVT_Pos) /*!< SCB HFSR: DEBUGEVT Mask */
-
-#define SCB_HFSR_FORCED_Pos 30 /*!< SCB HFSR: FORCED Position */
-#define SCB_HFSR_FORCED_Msk (1UL << SCB_HFSR_FORCED_Pos) /*!< SCB HFSR: FORCED Mask */
-
-#define SCB_HFSR_VECTTBL_Pos 1 /*!< SCB HFSR: VECTTBL Position */
-#define SCB_HFSR_VECTTBL_Msk (1UL << SCB_HFSR_VECTTBL_Pos) /*!< SCB HFSR: VECTTBL Mask */
-
-/* SCB Debug Fault Status Register Definitions */
-#define SCB_DFSR_EXTERNAL_Pos 4 /*!< SCB DFSR: EXTERNAL Position */
-#define SCB_DFSR_EXTERNAL_Msk (1UL << SCB_DFSR_EXTERNAL_Pos) /*!< SCB DFSR: EXTERNAL Mask */
-
-#define SCB_DFSR_VCATCH_Pos 3 /*!< SCB DFSR: VCATCH Position */
-#define SCB_DFSR_VCATCH_Msk (1UL << SCB_DFSR_VCATCH_Pos) /*!< SCB DFSR: VCATCH Mask */
-
-#define SCB_DFSR_DWTTRAP_Pos 2 /*!< SCB DFSR: DWTTRAP Position */
-#define SCB_DFSR_DWTTRAP_Msk (1UL << SCB_DFSR_DWTTRAP_Pos) /*!< SCB DFSR: DWTTRAP Mask */
-
-#define SCB_DFSR_BKPT_Pos 1 /*!< SCB DFSR: BKPT Position */
-#define SCB_DFSR_BKPT_Msk (1UL << SCB_DFSR_BKPT_Pos) /*!< SCB DFSR: BKPT Mask */
-
-#define SCB_DFSR_HALTED_Pos 0 /*!< SCB DFSR: HALTED Position */
-#define SCB_DFSR_HALTED_Msk (1UL << SCB_DFSR_HALTED_Pos) /*!< SCB DFSR: HALTED Mask */
-
-/*@} end of group CMSIS_SCB */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)
- \brief Type definitions for the System Control and ID Register not in the SCB
- @{
- */
-
-/** \brief Structure type to access the System Control and ID Register not in the SCB.
- */
-typedef struct
-{
- uint32_t RESERVED0[1];
- __I uint32_t ICTR; /*!< Offset: 0x004 (R/ ) Interrupt Controller Type Register */
- __IO uint32_t ACTLR; /*!< Offset: 0x008 (R/W) Auxiliary Control Register */
-} SCnSCB_Type;
-
-/* Interrupt Controller Type Register Definitions */
-#define SCnSCB_ICTR_INTLINESNUM_Pos 0 /*!< ICTR: INTLINESNUM Position */
-#define SCnSCB_ICTR_INTLINESNUM_Msk (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos) /*!< ICTR: INTLINESNUM Mask */
-
-/* Auxiliary Control Register Definitions */
-#define SCnSCB_ACTLR_DISOOFP_Pos 9 /*!< ACTLR: DISOOFP Position */
-#define SCnSCB_ACTLR_DISOOFP_Msk (1UL << SCnSCB_ACTLR_DISOOFP_Pos) /*!< ACTLR: DISOOFP Mask */
-
-#define SCnSCB_ACTLR_DISFPCA_Pos 8 /*!< ACTLR: DISFPCA Position */
-#define SCnSCB_ACTLR_DISFPCA_Msk (1UL << SCnSCB_ACTLR_DISFPCA_Pos) /*!< ACTLR: DISFPCA Mask */
-
-#define SCnSCB_ACTLR_DISFOLD_Pos 2 /*!< ACTLR: DISFOLD Position */
-#define SCnSCB_ACTLR_DISFOLD_Msk (1UL << SCnSCB_ACTLR_DISFOLD_Pos) /*!< ACTLR: DISFOLD Mask */
-
-#define SCnSCB_ACTLR_DISDEFWBUF_Pos 1 /*!< ACTLR: DISDEFWBUF Position */
-#define SCnSCB_ACTLR_DISDEFWBUF_Msk (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos) /*!< ACTLR: DISDEFWBUF Mask */
-
-#define SCnSCB_ACTLR_DISMCYCINT_Pos 0 /*!< ACTLR: DISMCYCINT Position */
-#define SCnSCB_ACTLR_DISMCYCINT_Msk (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos) /*!< ACTLR: DISMCYCINT Mask */
-
-/*@} end of group CMSIS_SCnotSCB */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_SysTick System Tick Timer (SysTick)
- \brief Type definitions for the System Timer Registers.
- @{
- */
-
-/** \brief Structure type to access the System Timer (SysTick).
- */
-typedef struct
-{
- __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) SysTick Control and Status Register */
- __IO uint32_t LOAD; /*!< Offset: 0x004 (R/W) SysTick Reload Value Register */
- __IO uint32_t VAL; /*!< Offset: 0x008 (R/W) SysTick Current Value Register */
- __I uint32_t CALIB; /*!< Offset: 0x00C (R/ ) SysTick Calibration Register */
-} SysTick_Type;
-
-/* SysTick Control / Status Register Definitions */
-#define SysTick_CTRL_COUNTFLAG_Pos 16 /*!< SysTick CTRL: COUNTFLAG Position */
-#define SysTick_CTRL_COUNTFLAG_Msk (1UL << SysTick_CTRL_COUNTFLAG_Pos) /*!< SysTick CTRL: COUNTFLAG Mask */
-
-#define SysTick_CTRL_CLKSOURCE_Pos 2 /*!< SysTick CTRL: CLKSOURCE Position */
-#define SysTick_CTRL_CLKSOURCE_Msk (1UL << SysTick_CTRL_CLKSOURCE_Pos) /*!< SysTick CTRL: CLKSOURCE Mask */
-
-#define SysTick_CTRL_TICKINT_Pos 1 /*!< SysTick CTRL: TICKINT Position */
-#define SysTick_CTRL_TICKINT_Msk (1UL << SysTick_CTRL_TICKINT_Pos) /*!< SysTick CTRL: TICKINT Mask */
-
-#define SysTick_CTRL_ENABLE_Pos 0 /*!< SysTick CTRL: ENABLE Position */
-#define SysTick_CTRL_ENABLE_Msk (1UL << SysTick_CTRL_ENABLE_Pos) /*!< SysTick CTRL: ENABLE Mask */
-
-/* SysTick Reload Register Definitions */
-#define SysTick_LOAD_RELOAD_Pos 0 /*!< SysTick LOAD: RELOAD Position */
-#define SysTick_LOAD_RELOAD_Msk (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos) /*!< SysTick LOAD: RELOAD Mask */
-
-/* SysTick Current Register Definitions */
-#define SysTick_VAL_CURRENT_Pos 0 /*!< SysTick VAL: CURRENT Position */
-#define SysTick_VAL_CURRENT_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick VAL: CURRENT Mask */
-
-/* SysTick Calibration Register Definitions */
-#define SysTick_CALIB_NOREF_Pos 31 /*!< SysTick CALIB: NOREF Position */
-#define SysTick_CALIB_NOREF_Msk (1UL << SysTick_CALIB_NOREF_Pos) /*!< SysTick CALIB: NOREF Mask */
-
-#define SysTick_CALIB_SKEW_Pos 30 /*!< SysTick CALIB: SKEW Position */
-#define SysTick_CALIB_SKEW_Msk (1UL << SysTick_CALIB_SKEW_Pos) /*!< SysTick CALIB: SKEW Mask */
-
-#define SysTick_CALIB_TENMS_Pos 0 /*!< SysTick CALIB: TENMS Position */
-#define SysTick_CALIB_TENMS_Msk (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos) /*!< SysTick CALIB: TENMS Mask */
-
-/*@} end of group CMSIS_SysTick */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_ITM Instrumentation Trace Macrocell (ITM)
- \brief Type definitions for the Instrumentation Trace Macrocell (ITM)
- @{
- */
-
-/** \brief Structure type to access the Instrumentation Trace Macrocell Register (ITM).
- */
-typedef struct
-{
- __O union
- {
- __O uint8_t u8; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 8-bit */
- __O uint16_t u16; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 16-bit */
- __O uint32_t u32; /*!< Offset: 0x000 ( /W) ITM Stimulus Port 32-bit */
- } PORT [32]; /*!< Offset: 0x000 ( /W) ITM Stimulus Port Registers */
- uint32_t RESERVED0[864];
- __IO uint32_t TER; /*!< Offset: 0xE00 (R/W) ITM Trace Enable Register */
- uint32_t RESERVED1[15];
- __IO uint32_t TPR; /*!< Offset: 0xE40 (R/W) ITM Trace Privilege Register */
- uint32_t RESERVED2[15];
- __IO uint32_t TCR; /*!< Offset: 0xE80 (R/W) ITM Trace Control Register */
- uint32_t RESERVED3[29];
- __O uint32_t IWR; /*!< Offset: 0xEF8 ( /W) ITM Integration Write Register */
- __I uint32_t IRR; /*!< Offset: 0xEFC (R/ ) ITM Integration Read Register */
- __IO uint32_t IMCR; /*!< Offset: 0xF00 (R/W) ITM Integration Mode Control Register */
- uint32_t RESERVED4[43];
- __O uint32_t LAR; /*!< Offset: 0xFB0 ( /W) ITM Lock Access Register */
- __I uint32_t LSR; /*!< Offset: 0xFB4 (R/ ) ITM Lock Status Register */
- uint32_t RESERVED5[6];
- __I uint32_t PID4; /*!< Offset: 0xFD0 (R/ ) ITM Peripheral Identification Register #4 */
- __I uint32_t PID5; /*!< Offset: 0xFD4 (R/ ) ITM Peripheral Identification Register #5 */
- __I uint32_t PID6; /*!< Offset: 0xFD8 (R/ ) ITM Peripheral Identification Register #6 */
- __I uint32_t PID7; /*!< Offset: 0xFDC (R/ ) ITM Peripheral Identification Register #7 */
- __I uint32_t PID0; /*!< Offset: 0xFE0 (R/ ) ITM Peripheral Identification Register #0 */
- __I uint32_t PID1; /*!< Offset: 0xFE4 (R/ ) ITM Peripheral Identification Register #1 */
- __I uint32_t PID2; /*!< Offset: 0xFE8 (R/ ) ITM Peripheral Identification Register #2 */
- __I uint32_t PID3; /*!< Offset: 0xFEC (R/ ) ITM Peripheral Identification Register #3 */
- __I uint32_t CID0; /*!< Offset: 0xFF0 (R/ ) ITM Component Identification Register #0 */
- __I uint32_t CID1; /*!< Offset: 0xFF4 (R/ ) ITM Component Identification Register #1 */
- __I uint32_t CID2; /*!< Offset: 0xFF8 (R/ ) ITM Component Identification Register #2 */
- __I uint32_t CID3; /*!< Offset: 0xFFC (R/ ) ITM Component Identification Register #3 */
-} ITM_Type;
-
-/* ITM Trace Privilege Register Definitions */
-#define ITM_TPR_PRIVMASK_Pos 0 /*!< ITM TPR: PRIVMASK Position */
-#define ITM_TPR_PRIVMASK_Msk (0xFUL << ITM_TPR_PRIVMASK_Pos) /*!< ITM TPR: PRIVMASK Mask */
-
-/* ITM Trace Control Register Definitions */
-#define ITM_TCR_BUSY_Pos 23 /*!< ITM TCR: BUSY Position */
-#define ITM_TCR_BUSY_Msk (1UL << ITM_TCR_BUSY_Pos) /*!< ITM TCR: BUSY Mask */
-
-#define ITM_TCR_TraceBusID_Pos 16 /*!< ITM TCR: ATBID Position */
-#define ITM_TCR_TraceBusID_Msk (0x7FUL << ITM_TCR_TraceBusID_Pos) /*!< ITM TCR: ATBID Mask */
-
-#define ITM_TCR_GTSFREQ_Pos 10 /*!< ITM TCR: Global timestamp frequency Position */
-#define ITM_TCR_GTSFREQ_Msk (3UL << ITM_TCR_GTSFREQ_Pos) /*!< ITM TCR: Global timestamp frequency Mask */
-
-#define ITM_TCR_TSPrescale_Pos 8 /*!< ITM TCR: TSPrescale Position */
-#define ITM_TCR_TSPrescale_Msk (3UL << ITM_TCR_TSPrescale_Pos) /*!< ITM TCR: TSPrescale Mask */
-
-#define ITM_TCR_SWOENA_Pos 4 /*!< ITM TCR: SWOENA Position */
-#define ITM_TCR_SWOENA_Msk (1UL << ITM_TCR_SWOENA_Pos) /*!< ITM TCR: SWOENA Mask */
-
-#define ITM_TCR_DWTENA_Pos 3 /*!< ITM TCR: DWTENA Position */
-#define ITM_TCR_DWTENA_Msk (1UL << ITM_TCR_DWTENA_Pos) /*!< ITM TCR: DWTENA Mask */
-
-#define ITM_TCR_SYNCENA_Pos 2 /*!< ITM TCR: SYNCENA Position */
-#define ITM_TCR_SYNCENA_Msk (1UL << ITM_TCR_SYNCENA_Pos) /*!< ITM TCR: SYNCENA Mask */
-
-#define ITM_TCR_TSENA_Pos 1 /*!< ITM TCR: TSENA Position */
-#define ITM_TCR_TSENA_Msk (1UL << ITM_TCR_TSENA_Pos) /*!< ITM TCR: TSENA Mask */
-
-#define ITM_TCR_ITMENA_Pos 0 /*!< ITM TCR: ITM Enable bit Position */
-#define ITM_TCR_ITMENA_Msk (1UL << ITM_TCR_ITMENA_Pos) /*!< ITM TCR: ITM Enable bit Mask */
-
-/* ITM Integration Write Register Definitions */
-#define ITM_IWR_ATVALIDM_Pos 0 /*!< ITM IWR: ATVALIDM Position */
-#define ITM_IWR_ATVALIDM_Msk (1UL << ITM_IWR_ATVALIDM_Pos) /*!< ITM IWR: ATVALIDM Mask */
-
-/* ITM Integration Read Register Definitions */
-#define ITM_IRR_ATREADYM_Pos 0 /*!< ITM IRR: ATREADYM Position */
-#define ITM_IRR_ATREADYM_Msk (1UL << ITM_IRR_ATREADYM_Pos) /*!< ITM IRR: ATREADYM Mask */
-
-/* ITM Integration Mode Control Register Definitions */
-#define ITM_IMCR_INTEGRATION_Pos 0 /*!< ITM IMCR: INTEGRATION Position */
-#define ITM_IMCR_INTEGRATION_Msk (1UL << ITM_IMCR_INTEGRATION_Pos) /*!< ITM IMCR: INTEGRATION Mask */
-
-/* ITM Lock Status Register Definitions */
-#define ITM_LSR_ByteAcc_Pos 2 /*!< ITM LSR: ByteAcc Position */
-#define ITM_LSR_ByteAcc_Msk (1UL << ITM_LSR_ByteAcc_Pos) /*!< ITM LSR: ByteAcc Mask */
-
-#define ITM_LSR_Access_Pos 1 /*!< ITM LSR: Access Position */
-#define ITM_LSR_Access_Msk (1UL << ITM_LSR_Access_Pos) /*!< ITM LSR: Access Mask */
-
-#define ITM_LSR_Present_Pos 0 /*!< ITM LSR: Present Position */
-#define ITM_LSR_Present_Msk (1UL << ITM_LSR_Present_Pos) /*!< ITM LSR: Present Mask */
-
-/*@}*/ /* end of group CMSIS_ITM */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_DWT Data Watchpoint and Trace (DWT)
- \brief Type definitions for the Data Watchpoint and Trace (DWT)
- @{
- */
-
-/** \brief Structure type to access the Data Watchpoint and Trace Register (DWT).
- */
-typedef struct
-{
- __IO uint32_t CTRL; /*!< Offset: 0x000 (R/W) Control Register */
- __IO uint32_t CYCCNT; /*!< Offset: 0x004 (R/W) Cycle Count Register */
- __IO uint32_t CPICNT; /*!< Offset: 0x008 (R/W) CPI Count Register */
- __IO uint32_t EXCCNT; /*!< Offset: 0x00C (R/W) Exception Overhead Count Register */
- __IO uint32_t SLEEPCNT; /*!< Offset: 0x010 (R/W) Sleep Count Register */
- __IO uint32_t LSUCNT; /*!< Offset: 0x014 (R/W) LSU Count Register */
- __IO uint32_t FOLDCNT; /*!< Offset: 0x018 (R/W) Folded-instruction Count Register */
- __I uint32_t PCSR; /*!< Offset: 0x01C (R/ ) Program Counter Sample Register */
- __IO uint32_t COMP0; /*!< Offset: 0x020 (R/W) Comparator Register 0 */
- __IO uint32_t MASK0; /*!< Offset: 0x024 (R/W) Mask Register 0 */
- __IO uint32_t FUNCTION0; /*!< Offset: 0x028 (R/W) Function Register 0 */
- uint32_t RESERVED0[1];
- __IO uint32_t COMP1; /*!< Offset: 0x030 (R/W) Comparator Register 1 */
- __IO uint32_t MASK1; /*!< Offset: 0x034 (R/W) Mask Register 1 */
- __IO uint32_t FUNCTION1; /*!< Offset: 0x038 (R/W) Function Register 1 */
- uint32_t RESERVED1[1];
- __IO uint32_t COMP2; /*!< Offset: 0x040 (R/W) Comparator Register 2 */
- __IO uint32_t MASK2; /*!< Offset: 0x044 (R/W) Mask Register 2 */
- __IO uint32_t FUNCTION2; /*!< Offset: 0x048 (R/W) Function Register 2 */
- uint32_t RESERVED2[1];
- __IO uint32_t COMP3; /*!< Offset: 0x050 (R/W) Comparator Register 3 */
- __IO uint32_t MASK3; /*!< Offset: 0x054 (R/W) Mask Register 3 */
- __IO uint32_t FUNCTION3; /*!< Offset: 0x058 (R/W) Function Register 3 */
-} DWT_Type;
-
-/* DWT Control Register Definitions */
-#define DWT_CTRL_NUMCOMP_Pos 28 /*!< DWT CTRL: NUMCOMP Position */
-#define DWT_CTRL_NUMCOMP_Msk (0xFUL << DWT_CTRL_NUMCOMP_Pos) /*!< DWT CTRL: NUMCOMP Mask */
-
-#define DWT_CTRL_NOTRCPKT_Pos 27 /*!< DWT CTRL: NOTRCPKT Position */
-#define DWT_CTRL_NOTRCPKT_Msk (0x1UL << DWT_CTRL_NOTRCPKT_Pos) /*!< DWT CTRL: NOTRCPKT Mask */
-
-#define DWT_CTRL_NOEXTTRIG_Pos 26 /*!< DWT CTRL: NOEXTTRIG Position */
-#define DWT_CTRL_NOEXTTRIG_Msk (0x1UL << DWT_CTRL_NOEXTTRIG_Pos) /*!< DWT CTRL: NOEXTTRIG Mask */
-
-#define DWT_CTRL_NOCYCCNT_Pos 25 /*!< DWT CTRL: NOCYCCNT Position */
-#define DWT_CTRL_NOCYCCNT_Msk (0x1UL << DWT_CTRL_NOCYCCNT_Pos) /*!< DWT CTRL: NOCYCCNT Mask */
-
-#define DWT_CTRL_NOPRFCNT_Pos 24 /*!< DWT CTRL: NOPRFCNT Position */
-#define DWT_CTRL_NOPRFCNT_Msk (0x1UL << DWT_CTRL_NOPRFCNT_Pos) /*!< DWT CTRL: NOPRFCNT Mask */
-
-#define DWT_CTRL_CYCEVTENA_Pos 22 /*!< DWT CTRL: CYCEVTENA Position */
-#define DWT_CTRL_CYCEVTENA_Msk (0x1UL << DWT_CTRL_CYCEVTENA_Pos) /*!< DWT CTRL: CYCEVTENA Mask */
-
-#define DWT_CTRL_FOLDEVTENA_Pos 21 /*!< DWT CTRL: FOLDEVTENA Position */
-#define DWT_CTRL_FOLDEVTENA_Msk (0x1UL << DWT_CTRL_FOLDEVTENA_Pos) /*!< DWT CTRL: FOLDEVTENA Mask */
-
-#define DWT_CTRL_LSUEVTENA_Pos 20 /*!< DWT CTRL: LSUEVTENA Position */
-#define DWT_CTRL_LSUEVTENA_Msk (0x1UL << DWT_CTRL_LSUEVTENA_Pos) /*!< DWT CTRL: LSUEVTENA Mask */
-
-#define DWT_CTRL_SLEEPEVTENA_Pos 19 /*!< DWT CTRL: SLEEPEVTENA Position */
-#define DWT_CTRL_SLEEPEVTENA_Msk (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos) /*!< DWT CTRL: SLEEPEVTENA Mask */
-
-#define DWT_CTRL_EXCEVTENA_Pos 18 /*!< DWT CTRL: EXCEVTENA Position */
-#define DWT_CTRL_EXCEVTENA_Msk (0x1UL << DWT_CTRL_EXCEVTENA_Pos) /*!< DWT CTRL: EXCEVTENA Mask */
-
-#define DWT_CTRL_CPIEVTENA_Pos 17 /*!< DWT CTRL: CPIEVTENA Position */
-#define DWT_CTRL_CPIEVTENA_Msk (0x1UL << DWT_CTRL_CPIEVTENA_Pos) /*!< DWT CTRL: CPIEVTENA Mask */
-
-#define DWT_CTRL_EXCTRCENA_Pos 16 /*!< DWT CTRL: EXCTRCENA Position */
-#define DWT_CTRL_EXCTRCENA_Msk (0x1UL << DWT_CTRL_EXCTRCENA_Pos) /*!< DWT CTRL: EXCTRCENA Mask */
-
-#define DWT_CTRL_PCSAMPLENA_Pos 12 /*!< DWT CTRL: PCSAMPLENA Position */
-#define DWT_CTRL_PCSAMPLENA_Msk (0x1UL << DWT_CTRL_PCSAMPLENA_Pos) /*!< DWT CTRL: PCSAMPLENA Mask */
-
-#define DWT_CTRL_SYNCTAP_Pos 10 /*!< DWT CTRL: SYNCTAP Position */
-#define DWT_CTRL_SYNCTAP_Msk (0x3UL << DWT_CTRL_SYNCTAP_Pos) /*!< DWT CTRL: SYNCTAP Mask */
-
-#define DWT_CTRL_CYCTAP_Pos 9 /*!< DWT CTRL: CYCTAP Position */
-#define DWT_CTRL_CYCTAP_Msk (0x1UL << DWT_CTRL_CYCTAP_Pos) /*!< DWT CTRL: CYCTAP Mask */
-
-#define DWT_CTRL_POSTINIT_Pos 5 /*!< DWT CTRL: POSTINIT Position */
-#define DWT_CTRL_POSTINIT_Msk (0xFUL << DWT_CTRL_POSTINIT_Pos) /*!< DWT CTRL: POSTINIT Mask */
-
-#define DWT_CTRL_POSTPRESET_Pos 1 /*!< DWT CTRL: POSTPRESET Position */
-#define DWT_CTRL_POSTPRESET_Msk (0xFUL << DWT_CTRL_POSTPRESET_Pos) /*!< DWT CTRL: POSTPRESET Mask */
-
-#define DWT_CTRL_CYCCNTENA_Pos 0 /*!< DWT CTRL: CYCCNTENA Position */
-#define DWT_CTRL_CYCCNTENA_Msk (0x1UL << DWT_CTRL_CYCCNTENA_Pos) /*!< DWT CTRL: CYCCNTENA Mask */
-
-/* DWT CPI Count Register Definitions */
-#define DWT_CPICNT_CPICNT_Pos 0 /*!< DWT CPICNT: CPICNT Position */
-#define DWT_CPICNT_CPICNT_Msk (0xFFUL << DWT_CPICNT_CPICNT_Pos) /*!< DWT CPICNT: CPICNT Mask */
-
-/* DWT Exception Overhead Count Register Definitions */
-#define DWT_EXCCNT_EXCCNT_Pos 0 /*!< DWT EXCCNT: EXCCNT Position */
-#define DWT_EXCCNT_EXCCNT_Msk (0xFFUL << DWT_EXCCNT_EXCCNT_Pos) /*!< DWT EXCCNT: EXCCNT Mask */
-
-/* DWT Sleep Count Register Definitions */
-#define DWT_SLEEPCNT_SLEEPCNT_Pos 0 /*!< DWT SLEEPCNT: SLEEPCNT Position */
-#define DWT_SLEEPCNT_SLEEPCNT_Msk (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos) /*!< DWT SLEEPCNT: SLEEPCNT Mask */
-
-/* DWT LSU Count Register Definitions */
-#define DWT_LSUCNT_LSUCNT_Pos 0 /*!< DWT LSUCNT: LSUCNT Position */
-#define DWT_LSUCNT_LSUCNT_Msk (0xFFUL << DWT_LSUCNT_LSUCNT_Pos) /*!< DWT LSUCNT: LSUCNT Mask */
-
-/* DWT Folded-instruction Count Register Definitions */
-#define DWT_FOLDCNT_FOLDCNT_Pos 0 /*!< DWT FOLDCNT: FOLDCNT Position */
-#define DWT_FOLDCNT_FOLDCNT_Msk (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos) /*!< DWT FOLDCNT: FOLDCNT Mask */
-
-/* DWT Comparator Mask Register Definitions */
-#define DWT_MASK_MASK_Pos 0 /*!< DWT MASK: MASK Position */
-#define DWT_MASK_MASK_Msk (0x1FUL << DWT_MASK_MASK_Pos) /*!< DWT MASK: MASK Mask */
-
-/* DWT Comparator Function Register Definitions */
-#define DWT_FUNCTION_MATCHED_Pos 24 /*!< DWT FUNCTION: MATCHED Position */
-#define DWT_FUNCTION_MATCHED_Msk (0x1UL << DWT_FUNCTION_MATCHED_Pos) /*!< DWT FUNCTION: MATCHED Mask */
-
-#define DWT_FUNCTION_DATAVADDR1_Pos 16 /*!< DWT FUNCTION: DATAVADDR1 Position */
-#define DWT_FUNCTION_DATAVADDR1_Msk (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos) /*!< DWT FUNCTION: DATAVADDR1 Mask */
-
-#define DWT_FUNCTION_DATAVADDR0_Pos 12 /*!< DWT FUNCTION: DATAVADDR0 Position */
-#define DWT_FUNCTION_DATAVADDR0_Msk (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos) /*!< DWT FUNCTION: DATAVADDR0 Mask */
-
-#define DWT_FUNCTION_DATAVSIZE_Pos 10 /*!< DWT FUNCTION: DATAVSIZE Position */
-#define DWT_FUNCTION_DATAVSIZE_Msk (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos) /*!< DWT FUNCTION: DATAVSIZE Mask */
-
-#define DWT_FUNCTION_LNK1ENA_Pos 9 /*!< DWT FUNCTION: LNK1ENA Position */
-#define DWT_FUNCTION_LNK1ENA_Msk (0x1UL << DWT_FUNCTION_LNK1ENA_Pos) /*!< DWT FUNCTION: LNK1ENA Mask */
-
-#define DWT_FUNCTION_DATAVMATCH_Pos 8 /*!< DWT FUNCTION: DATAVMATCH Position */
-#define DWT_FUNCTION_DATAVMATCH_Msk (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos) /*!< DWT FUNCTION: DATAVMATCH Mask */
-
-#define DWT_FUNCTION_CYCMATCH_Pos 7 /*!< DWT FUNCTION: CYCMATCH Position */
-#define DWT_FUNCTION_CYCMATCH_Msk (0x1UL << DWT_FUNCTION_CYCMATCH_Pos) /*!< DWT FUNCTION: CYCMATCH Mask */
-
-#define DWT_FUNCTION_EMITRANGE_Pos 5 /*!< DWT FUNCTION: EMITRANGE Position */
-#define DWT_FUNCTION_EMITRANGE_Msk (0x1UL << DWT_FUNCTION_EMITRANGE_Pos) /*!< DWT FUNCTION: EMITRANGE Mask */
-
-#define DWT_FUNCTION_FUNCTION_Pos 0 /*!< DWT FUNCTION: FUNCTION Position */
-#define DWT_FUNCTION_FUNCTION_Msk (0xFUL << DWT_FUNCTION_FUNCTION_Pos) /*!< DWT FUNCTION: FUNCTION Mask */
-
-/*@}*/ /* end of group CMSIS_DWT */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_TPI Trace Port Interface (TPI)
- \brief Type definitions for the Trace Port Interface (TPI)
- @{
- */
-
-/** \brief Structure type to access the Trace Port Interface Register (TPI).
- */
-typedef struct
-{
- __IO uint32_t SSPSR; /*!< Offset: 0x000 (R/ ) Supported Parallel Port Size Register */
- __IO uint32_t CSPSR; /*!< Offset: 0x004 (R/W) Current Parallel Port Size Register */
- uint32_t RESERVED0[2];
- __IO uint32_t ACPR; /*!< Offset: 0x010 (R/W) Asynchronous Clock Prescaler Register */
- uint32_t RESERVED1[55];
- __IO uint32_t SPPR; /*!< Offset: 0x0F0 (R/W) Selected Pin Protocol Register */
- uint32_t RESERVED2[131];
- __I uint32_t FFSR; /*!< Offset: 0x300 (R/ ) Formatter and Flush Status Register */
- __IO uint32_t FFCR; /*!< Offset: 0x304 (R/W) Formatter and Flush Control Register */
- __I uint32_t FSCR; /*!< Offset: 0x308 (R/ ) Formatter Synchronization Counter Register */
- uint32_t RESERVED3[759];
- __I uint32_t TRIGGER; /*!< Offset: 0xEE8 (R/ ) TRIGGER */
- __I uint32_t FIFO0; /*!< Offset: 0xEEC (R/ ) Integration ETM Data */
- __I uint32_t ITATBCTR2; /*!< Offset: 0xEF0 (R/ ) ITATBCTR2 */
- uint32_t RESERVED4[1];
- __I uint32_t ITATBCTR0; /*!< Offset: 0xEF8 (R/ ) ITATBCTR0 */
- __I uint32_t FIFO1; /*!< Offset: 0xEFC (R/ ) Integration ITM Data */
- __IO uint32_t ITCTRL; /*!< Offset: 0xF00 (R/W) Integration Mode Control */
- uint32_t RESERVED5[39];
- __IO uint32_t CLAIMSET; /*!< Offset: 0xFA0 (R/W) Claim tag set */
- __IO uint32_t CLAIMCLR; /*!< Offset: 0xFA4 (R/W) Claim tag clear */
- uint32_t RESERVED7[8];
- __I uint32_t DEVID; /*!< Offset: 0xFC8 (R/ ) TPIU_DEVID */
- __I uint32_t DEVTYPE; /*!< Offset: 0xFCC (R/ ) TPIU_DEVTYPE */
-} TPI_Type;
-
-/* TPI Asynchronous Clock Prescaler Register Definitions */
-#define TPI_ACPR_PRESCALER_Pos 0 /*!< TPI ACPR: PRESCALER Position */
-#define TPI_ACPR_PRESCALER_Msk (0x1FFFUL << TPI_ACPR_PRESCALER_Pos) /*!< TPI ACPR: PRESCALER Mask */
-
-/* TPI Selected Pin Protocol Register Definitions */
-#define TPI_SPPR_TXMODE_Pos 0 /*!< TPI SPPR: TXMODE Position */
-#define TPI_SPPR_TXMODE_Msk (0x3UL << TPI_SPPR_TXMODE_Pos) /*!< TPI SPPR: TXMODE Mask */
-
-/* TPI Formatter and Flush Status Register Definitions */
-#define TPI_FFSR_FtNonStop_Pos 3 /*!< TPI FFSR: FtNonStop Position */
-#define TPI_FFSR_FtNonStop_Msk (0x1UL << TPI_FFSR_FtNonStop_Pos) /*!< TPI FFSR: FtNonStop Mask */
-
-#define TPI_FFSR_TCPresent_Pos 2 /*!< TPI FFSR: TCPresent Position */
-#define TPI_FFSR_TCPresent_Msk (0x1UL << TPI_FFSR_TCPresent_Pos) /*!< TPI FFSR: TCPresent Mask */
-
-#define TPI_FFSR_FtStopped_Pos 1 /*!< TPI FFSR: FtStopped Position */
-#define TPI_FFSR_FtStopped_Msk (0x1UL << TPI_FFSR_FtStopped_Pos) /*!< TPI FFSR: FtStopped Mask */
-
-#define TPI_FFSR_FlInProg_Pos 0 /*!< TPI FFSR: FlInProg Position */
-#define TPI_FFSR_FlInProg_Msk (0x1UL << TPI_FFSR_FlInProg_Pos) /*!< TPI FFSR: FlInProg Mask */
-
-/* TPI Formatter and Flush Control Register Definitions */
-#define TPI_FFCR_TrigIn_Pos 8 /*!< TPI FFCR: TrigIn Position */
-#define TPI_FFCR_TrigIn_Msk (0x1UL << TPI_FFCR_TrigIn_Pos) /*!< TPI FFCR: TrigIn Mask */
-
-#define TPI_FFCR_EnFCont_Pos 1 /*!< TPI FFCR: EnFCont Position */
-#define TPI_FFCR_EnFCont_Msk (0x1UL << TPI_FFCR_EnFCont_Pos) /*!< TPI FFCR: EnFCont Mask */
-
-/* TPI TRIGGER Register Definitions */
-#define TPI_TRIGGER_TRIGGER_Pos 0 /*!< TPI TRIGGER: TRIGGER Position */
-#define TPI_TRIGGER_TRIGGER_Msk (0x1UL << TPI_TRIGGER_TRIGGER_Pos) /*!< TPI TRIGGER: TRIGGER Mask */
-
-/* TPI Integration ETM Data Register Definitions (FIFO0) */
-#define TPI_FIFO0_ITM_ATVALID_Pos 29 /*!< TPI FIFO0: ITM_ATVALID Position */
-#define TPI_FIFO0_ITM_ATVALID_Msk (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos) /*!< TPI FIFO0: ITM_ATVALID Mask */
-
-#define TPI_FIFO0_ITM_bytecount_Pos 27 /*!< TPI FIFO0: ITM_bytecount Position */
-#define TPI_FIFO0_ITM_bytecount_Msk (0x3UL << TPI_FIFO0_ITM_bytecount_Pos) /*!< TPI FIFO0: ITM_bytecount Mask */
-
-#define TPI_FIFO0_ETM_ATVALID_Pos 26 /*!< TPI FIFO0: ETM_ATVALID Position */
-#define TPI_FIFO0_ETM_ATVALID_Msk (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos) /*!< TPI FIFO0: ETM_ATVALID Mask */
-
-#define TPI_FIFO0_ETM_bytecount_Pos 24 /*!< TPI FIFO0: ETM_bytecount Position */
-#define TPI_FIFO0_ETM_bytecount_Msk (0x3UL << TPI_FIFO0_ETM_bytecount_Pos) /*!< TPI FIFO0: ETM_bytecount Mask */
-
-#define TPI_FIFO0_ETM2_Pos 16 /*!< TPI FIFO0: ETM2 Position */
-#define TPI_FIFO0_ETM2_Msk (0xFFUL << TPI_FIFO0_ETM2_Pos) /*!< TPI FIFO0: ETM2 Mask */
-
-#define TPI_FIFO0_ETM1_Pos 8 /*!< TPI FIFO0: ETM1 Position */
-#define TPI_FIFO0_ETM1_Msk (0xFFUL << TPI_FIFO0_ETM1_Pos) /*!< TPI FIFO0: ETM1 Mask */
-
-#define TPI_FIFO0_ETM0_Pos 0 /*!< TPI FIFO0: ETM0 Position */
-#define TPI_FIFO0_ETM0_Msk (0xFFUL << TPI_FIFO0_ETM0_Pos) /*!< TPI FIFO0: ETM0 Mask */
-
-/* TPI ITATBCTR2 Register Definitions */
-#define TPI_ITATBCTR2_ATREADY_Pos 0 /*!< TPI ITATBCTR2: ATREADY Position */
-#define TPI_ITATBCTR2_ATREADY_Msk (0x1UL << TPI_ITATBCTR2_ATREADY_Pos) /*!< TPI ITATBCTR2: ATREADY Mask */
-
-/* TPI Integration ITM Data Register Definitions (FIFO1) */
-#define TPI_FIFO1_ITM_ATVALID_Pos 29 /*!< TPI FIFO1: ITM_ATVALID Position */
-#define TPI_FIFO1_ITM_ATVALID_Msk (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos) /*!< TPI FIFO1: ITM_ATVALID Mask */
-
-#define TPI_FIFO1_ITM_bytecount_Pos 27 /*!< TPI FIFO1: ITM_bytecount Position */
-#define TPI_FIFO1_ITM_bytecount_Msk (0x3UL << TPI_FIFO1_ITM_bytecount_Pos) /*!< TPI FIFO1: ITM_bytecount Mask */
-
-#define TPI_FIFO1_ETM_ATVALID_Pos 26 /*!< TPI FIFO1: ETM_ATVALID Position */
-#define TPI_FIFO1_ETM_ATVALID_Msk (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos) /*!< TPI FIFO1: ETM_ATVALID Mask */
-
-#define TPI_FIFO1_ETM_bytecount_Pos 24 /*!< TPI FIFO1: ETM_bytecount Position */
-#define TPI_FIFO1_ETM_bytecount_Msk (0x3UL << TPI_FIFO1_ETM_bytecount_Pos) /*!< TPI FIFO1: ETM_bytecount Mask */
-
-#define TPI_FIFO1_ITM2_Pos 16 /*!< TPI FIFO1: ITM2 Position */
-#define TPI_FIFO1_ITM2_Msk (0xFFUL << TPI_FIFO1_ITM2_Pos) /*!< TPI FIFO1: ITM2 Mask */
-
-#define TPI_FIFO1_ITM1_Pos 8 /*!< TPI FIFO1: ITM1 Position */
-#define TPI_FIFO1_ITM1_Msk (0xFFUL << TPI_FIFO1_ITM1_Pos) /*!< TPI FIFO1: ITM1 Mask */
-
-#define TPI_FIFO1_ITM0_Pos 0 /*!< TPI FIFO1: ITM0 Position */
-#define TPI_FIFO1_ITM0_Msk (0xFFUL << TPI_FIFO1_ITM0_Pos) /*!< TPI FIFO1: ITM0 Mask */
-
-/* TPI ITATBCTR0 Register Definitions */
-#define TPI_ITATBCTR0_ATREADY_Pos 0 /*!< TPI ITATBCTR0: ATREADY Position */
-#define TPI_ITATBCTR0_ATREADY_Msk (0x1UL << TPI_ITATBCTR0_ATREADY_Pos) /*!< TPI ITATBCTR0: ATREADY Mask */
-
-/* TPI Integration Mode Control Register Definitions */
-#define TPI_ITCTRL_Mode_Pos 0 /*!< TPI ITCTRL: Mode Position */
-#define TPI_ITCTRL_Mode_Msk (0x1UL << TPI_ITCTRL_Mode_Pos) /*!< TPI ITCTRL: Mode Mask */
-
-/* TPI DEVID Register Definitions */
-#define TPI_DEVID_NRZVALID_Pos 11 /*!< TPI DEVID: NRZVALID Position */
-#define TPI_DEVID_NRZVALID_Msk (0x1UL << TPI_DEVID_NRZVALID_Pos) /*!< TPI DEVID: NRZVALID Mask */
-
-#define TPI_DEVID_MANCVALID_Pos 10 /*!< TPI DEVID: MANCVALID Position */
-#define TPI_DEVID_MANCVALID_Msk (0x1UL << TPI_DEVID_MANCVALID_Pos) /*!< TPI DEVID: MANCVALID Mask */
-
-#define TPI_DEVID_PTINVALID_Pos 9 /*!< TPI DEVID: PTINVALID Position */
-#define TPI_DEVID_PTINVALID_Msk (0x1UL << TPI_DEVID_PTINVALID_Pos) /*!< TPI DEVID: PTINVALID Mask */
-
-#define TPI_DEVID_MinBufSz_Pos 6 /*!< TPI DEVID: MinBufSz Position */
-#define TPI_DEVID_MinBufSz_Msk (0x7UL << TPI_DEVID_MinBufSz_Pos) /*!< TPI DEVID: MinBufSz Mask */
-
-#define TPI_DEVID_AsynClkIn_Pos 5 /*!< TPI DEVID: AsynClkIn Position */
-#define TPI_DEVID_AsynClkIn_Msk (0x1UL << TPI_DEVID_AsynClkIn_Pos) /*!< TPI DEVID: AsynClkIn Mask */
-
-#define TPI_DEVID_NrTraceInput_Pos 0 /*!< TPI DEVID: NrTraceInput Position */
-#define TPI_DEVID_NrTraceInput_Msk (0x1FUL << TPI_DEVID_NrTraceInput_Pos) /*!< TPI DEVID: NrTraceInput Mask */
-
-/* TPI DEVTYPE Register Definitions */
-#define TPI_DEVTYPE_SubType_Pos 0 /*!< TPI DEVTYPE: SubType Position */
-#define TPI_DEVTYPE_SubType_Msk (0xFUL << TPI_DEVTYPE_SubType_Pos) /*!< TPI DEVTYPE: SubType Mask */
-
-#define TPI_DEVTYPE_MajorType_Pos 4 /*!< TPI DEVTYPE: MajorType Position */
-#define TPI_DEVTYPE_MajorType_Msk (0xFUL << TPI_DEVTYPE_MajorType_Pos) /*!< TPI DEVTYPE: MajorType Mask */
-
-/*@}*/ /* end of group CMSIS_TPI */
-
-
-#if (__MPU_PRESENT == 1)
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_MPU Memory Protection Unit (MPU)
- \brief Type definitions for the Memory Protection Unit (MPU)
- @{
- */
-
-/** \brief Structure type to access the Memory Protection Unit (MPU).
- */
-typedef struct
-{
- __I uint32_t TYPE; /*!< Offset: 0x000 (R/ ) MPU Type Register */
- __IO uint32_t CTRL; /*!< Offset: 0x004 (R/W) MPU Control Register */
- __IO uint32_t RNR; /*!< Offset: 0x008 (R/W) MPU Region RNRber Register */
- __IO uint32_t RBAR; /*!< Offset: 0x00C (R/W) MPU Region Base Address Register */
- __IO uint32_t RASR; /*!< Offset: 0x010 (R/W) MPU Region Attribute and Size Register */
- __IO uint32_t RBAR_A1; /*!< Offset: 0x014 (R/W) MPU Alias 1 Region Base Address Register */
- __IO uint32_t RASR_A1; /*!< Offset: 0x018 (R/W) MPU Alias 1 Region Attribute and Size Register */
- __IO uint32_t RBAR_A2; /*!< Offset: 0x01C (R/W) MPU Alias 2 Region Base Address Register */
- __IO uint32_t RASR_A2; /*!< Offset: 0x020 (R/W) MPU Alias 2 Region Attribute and Size Register */
- __IO uint32_t RBAR_A3; /*!< Offset: 0x024 (R/W) MPU Alias 3 Region Base Address Register */
- __IO uint32_t RASR_A3; /*!< Offset: 0x028 (R/W) MPU Alias 3 Region Attribute and Size Register */
-} MPU_Type;
-
-/* MPU Type Register */
-#define MPU_TYPE_IREGION_Pos 16 /*!< MPU TYPE: IREGION Position */
-#define MPU_TYPE_IREGION_Msk (0xFFUL << MPU_TYPE_IREGION_Pos) /*!< MPU TYPE: IREGION Mask */
-
-#define MPU_TYPE_DREGION_Pos 8 /*!< MPU TYPE: DREGION Position */
-#define MPU_TYPE_DREGION_Msk (0xFFUL << MPU_TYPE_DREGION_Pos) /*!< MPU TYPE: DREGION Mask */
-
-#define MPU_TYPE_SEPARATE_Pos 0 /*!< MPU TYPE: SEPARATE Position */
-#define MPU_TYPE_SEPARATE_Msk (1UL << MPU_TYPE_SEPARATE_Pos) /*!< MPU TYPE: SEPARATE Mask */
-
-/* MPU Control Register */
-#define MPU_CTRL_PRIVDEFENA_Pos 2 /*!< MPU CTRL: PRIVDEFENA Position */
-#define MPU_CTRL_PRIVDEFENA_Msk (1UL << MPU_CTRL_PRIVDEFENA_Pos) /*!< MPU CTRL: PRIVDEFENA Mask */
-
-#define MPU_CTRL_HFNMIENA_Pos 1 /*!< MPU CTRL: HFNMIENA Position */
-#define MPU_CTRL_HFNMIENA_Msk (1UL << MPU_CTRL_HFNMIENA_Pos) /*!< MPU CTRL: HFNMIENA Mask */
-
-#define MPU_CTRL_ENABLE_Pos 0 /*!< MPU CTRL: ENABLE Position */
-#define MPU_CTRL_ENABLE_Msk (1UL << MPU_CTRL_ENABLE_Pos) /*!< MPU CTRL: ENABLE Mask */
-
-/* MPU Region Number Register */
-#define MPU_RNR_REGION_Pos 0 /*!< MPU RNR: REGION Position */
-#define MPU_RNR_REGION_Msk (0xFFUL << MPU_RNR_REGION_Pos) /*!< MPU RNR: REGION Mask */
-
-/* MPU Region Base Address Register */
-#define MPU_RBAR_ADDR_Pos 5 /*!< MPU RBAR: ADDR Position */
-#define MPU_RBAR_ADDR_Msk (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos) /*!< MPU RBAR: ADDR Mask */
-
-#define MPU_RBAR_VALID_Pos 4 /*!< MPU RBAR: VALID Position */
-#define MPU_RBAR_VALID_Msk (1UL << MPU_RBAR_VALID_Pos) /*!< MPU RBAR: VALID Mask */
-
-#define MPU_RBAR_REGION_Pos 0 /*!< MPU RBAR: REGION Position */
-#define MPU_RBAR_REGION_Msk (0xFUL << MPU_RBAR_REGION_Pos) /*!< MPU RBAR: REGION Mask */
-
-/* MPU Region Attribute and Size Register */
-#define MPU_RASR_ATTRS_Pos 16 /*!< MPU RASR: MPU Region Attribute field Position */
-#define MPU_RASR_ATTRS_Msk (0xFFFFUL << MPU_RASR_ATTRS_Pos) /*!< MPU RASR: MPU Region Attribute field Mask */
-
-#define MPU_RASR_XN_Pos 28 /*!< MPU RASR: ATTRS.XN Position */
-#define MPU_RASR_XN_Msk (1UL << MPU_RASR_XN_Pos) /*!< MPU RASR: ATTRS.XN Mask */
-
-#define MPU_RASR_AP_Pos 24 /*!< MPU RASR: ATTRS.AP Position */
-#define MPU_RASR_AP_Msk (0x7UL << MPU_RASR_AP_Pos) /*!< MPU RASR: ATTRS.AP Mask */
-
-#define MPU_RASR_TEX_Pos 19 /*!< MPU RASR: ATTRS.TEX Position */
-#define MPU_RASR_TEX_Msk (0x7UL << MPU_RASR_TEX_Pos) /*!< MPU RASR: ATTRS.TEX Mask */
-
-#define MPU_RASR_S_Pos 18 /*!< MPU RASR: ATTRS.S Position */
-#define MPU_RASR_S_Msk (1UL << MPU_RASR_S_Pos) /*!< MPU RASR: ATTRS.S Mask */
-
-#define MPU_RASR_C_Pos 17 /*!< MPU RASR: ATTRS.C Position */
-#define MPU_RASR_C_Msk (1UL << MPU_RASR_C_Pos) /*!< MPU RASR: ATTRS.C Mask */
-
-#define MPU_RASR_B_Pos 16 /*!< MPU RASR: ATTRS.B Position */
-#define MPU_RASR_B_Msk (1UL << MPU_RASR_B_Pos) /*!< MPU RASR: ATTRS.B Mask */
-
-#define MPU_RASR_SRD_Pos 8 /*!< MPU RASR: Sub-Region Disable Position */
-#define MPU_RASR_SRD_Msk (0xFFUL << MPU_RASR_SRD_Pos) /*!< MPU RASR: Sub-Region Disable Mask */
-
-#define MPU_RASR_SIZE_Pos 1 /*!< MPU RASR: Region Size Field Position */
-#define MPU_RASR_SIZE_Msk (0x1FUL << MPU_RASR_SIZE_Pos) /*!< MPU RASR: Region Size Field Mask */
-
-#define MPU_RASR_ENABLE_Pos 0 /*!< MPU RASR: Region enable bit Position */
-#define MPU_RASR_ENABLE_Msk (1UL << MPU_RASR_ENABLE_Pos) /*!< MPU RASR: Region enable bit Disable Mask */
-
-/*@} end of group CMSIS_MPU */
-#endif
-
-
-#if (__FPU_PRESENT == 1)
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_FPU Floating Point Unit (FPU)
- \brief Type definitions for the Floating Point Unit (FPU)
- @{
- */
-
-/** \brief Structure type to access the Floating Point Unit (FPU).
- */
-typedef struct
-{
- uint32_t RESERVED0[1];
- __IO uint32_t FPCCR; /*!< Offset: 0x004 (R/W) Floating-Point Context Control Register */
- __IO uint32_t FPCAR; /*!< Offset: 0x008 (R/W) Floating-Point Context Address Register */
- __IO uint32_t FPDSCR; /*!< Offset: 0x00C (R/W) Floating-Point Default Status Control Register */
- __I uint32_t MVFR0; /*!< Offset: 0x010 (R/ ) Media and FP Feature Register 0 */
- __I uint32_t MVFR1; /*!< Offset: 0x014 (R/ ) Media and FP Feature Register 1 */
-} FPU_Type;
-
-/* Floating-Point Context Control Register */
-#define FPU_FPCCR_ASPEN_Pos 31 /*!< FPCCR: ASPEN bit Position */
-#define FPU_FPCCR_ASPEN_Msk (1UL << FPU_FPCCR_ASPEN_Pos) /*!< FPCCR: ASPEN bit Mask */
-
-#define FPU_FPCCR_LSPEN_Pos 30 /*!< FPCCR: LSPEN Position */
-#define FPU_FPCCR_LSPEN_Msk (1UL << FPU_FPCCR_LSPEN_Pos) /*!< FPCCR: LSPEN bit Mask */
-
-#define FPU_FPCCR_MONRDY_Pos 8 /*!< FPCCR: MONRDY Position */
-#define FPU_FPCCR_MONRDY_Msk (1UL << FPU_FPCCR_MONRDY_Pos) /*!< FPCCR: MONRDY bit Mask */
-
-#define FPU_FPCCR_BFRDY_Pos 6 /*!< FPCCR: BFRDY Position */
-#define FPU_FPCCR_BFRDY_Msk (1UL << FPU_FPCCR_BFRDY_Pos) /*!< FPCCR: BFRDY bit Mask */
-
-#define FPU_FPCCR_MMRDY_Pos 5 /*!< FPCCR: MMRDY Position */
-#define FPU_FPCCR_MMRDY_Msk (1UL << FPU_FPCCR_MMRDY_Pos) /*!< FPCCR: MMRDY bit Mask */
-
-#define FPU_FPCCR_HFRDY_Pos 4 /*!< FPCCR: HFRDY Position */
-#define FPU_FPCCR_HFRDY_Msk (1UL << FPU_FPCCR_HFRDY_Pos) /*!< FPCCR: HFRDY bit Mask */
-
-#define FPU_FPCCR_THREAD_Pos 3 /*!< FPCCR: processor mode bit Position */
-#define FPU_FPCCR_THREAD_Msk (1UL << FPU_FPCCR_THREAD_Pos) /*!< FPCCR: processor mode active bit Mask */
-
-#define FPU_FPCCR_USER_Pos 1 /*!< FPCCR: privilege level bit Position */
-#define FPU_FPCCR_USER_Msk (1UL << FPU_FPCCR_USER_Pos) /*!< FPCCR: privilege level bit Mask */
-
-#define FPU_FPCCR_LSPACT_Pos 0 /*!< FPCCR: Lazy state preservation active bit Position */
-#define FPU_FPCCR_LSPACT_Msk (1UL << FPU_FPCCR_LSPACT_Pos) /*!< FPCCR: Lazy state preservation active bit Mask */
-
-/* Floating-Point Context Address Register */
-#define FPU_FPCAR_ADDRESS_Pos 3 /*!< FPCAR: ADDRESS bit Position */
-#define FPU_FPCAR_ADDRESS_Msk (0x1FFFFFFFUL << FPU_FPCAR_ADDRESS_Pos) /*!< FPCAR: ADDRESS bit Mask */
-
-/* Floating-Point Default Status Control Register */
-#define FPU_FPDSCR_AHP_Pos 26 /*!< FPDSCR: AHP bit Position */
-#define FPU_FPDSCR_AHP_Msk (1UL << FPU_FPDSCR_AHP_Pos) /*!< FPDSCR: AHP bit Mask */
-
-#define FPU_FPDSCR_DN_Pos 25 /*!< FPDSCR: DN bit Position */
-#define FPU_FPDSCR_DN_Msk (1UL << FPU_FPDSCR_DN_Pos) /*!< FPDSCR: DN bit Mask */
-
-#define FPU_FPDSCR_FZ_Pos 24 /*!< FPDSCR: FZ bit Position */
-#define FPU_FPDSCR_FZ_Msk (1UL << FPU_FPDSCR_FZ_Pos) /*!< FPDSCR: FZ bit Mask */
-
-#define FPU_FPDSCR_RMode_Pos 22 /*!< FPDSCR: RMode bit Position */
-#define FPU_FPDSCR_RMode_Msk (3UL << FPU_FPDSCR_RMode_Pos) /*!< FPDSCR: RMode bit Mask */
-
-/* Media and FP Feature Register 0 */
-#define FPU_MVFR0_FP_rounding_modes_Pos 28 /*!< MVFR0: FP rounding modes bits Position */
-#define FPU_MVFR0_FP_rounding_modes_Msk (0xFUL << FPU_MVFR0_FP_rounding_modes_Pos) /*!< MVFR0: FP rounding modes bits Mask */
-
-#define FPU_MVFR0_Short_vectors_Pos 24 /*!< MVFR0: Short vectors bits Position */
-#define FPU_MVFR0_Short_vectors_Msk (0xFUL << FPU_MVFR0_Short_vectors_Pos) /*!< MVFR0: Short vectors bits Mask */
-
-#define FPU_MVFR0_Square_root_Pos 20 /*!< MVFR0: Square root bits Position */
-#define FPU_MVFR0_Square_root_Msk (0xFUL << FPU_MVFR0_Square_root_Pos) /*!< MVFR0: Square root bits Mask */
-
-#define FPU_MVFR0_Divide_Pos 16 /*!< MVFR0: Divide bits Position */
-#define FPU_MVFR0_Divide_Msk (0xFUL << FPU_MVFR0_Divide_Pos) /*!< MVFR0: Divide bits Mask */
-
-#define FPU_MVFR0_FP_excep_trapping_Pos 12 /*!< MVFR0: FP exception trapping bits Position */
-#define FPU_MVFR0_FP_excep_trapping_Msk (0xFUL << FPU_MVFR0_FP_excep_trapping_Pos) /*!< MVFR0: FP exception trapping bits Mask */
-
-#define FPU_MVFR0_Double_precision_Pos 8 /*!< MVFR0: Double-precision bits Position */
-#define FPU_MVFR0_Double_precision_Msk (0xFUL << FPU_MVFR0_Double_precision_Pos) /*!< MVFR0: Double-precision bits Mask */
-
-#define FPU_MVFR0_Single_precision_Pos 4 /*!< MVFR0: Single-precision bits Position */
-#define FPU_MVFR0_Single_precision_Msk (0xFUL << FPU_MVFR0_Single_precision_Pos) /*!< MVFR0: Single-precision bits Mask */
-
-#define FPU_MVFR0_A_SIMD_registers_Pos 0 /*!< MVFR0: A_SIMD registers bits Position */
-#define FPU_MVFR0_A_SIMD_registers_Msk (0xFUL << FPU_MVFR0_A_SIMD_registers_Pos) /*!< MVFR0: A_SIMD registers bits Mask */
-
-/* Media and FP Feature Register 1 */
-#define FPU_MVFR1_FP_fused_MAC_Pos 28 /*!< MVFR1: FP fused MAC bits Position */
-#define FPU_MVFR1_FP_fused_MAC_Msk (0xFUL << FPU_MVFR1_FP_fused_MAC_Pos) /*!< MVFR1: FP fused MAC bits Mask */
-
-#define FPU_MVFR1_FP_HPFP_Pos 24 /*!< MVFR1: FP HPFP bits Position */
-#define FPU_MVFR1_FP_HPFP_Msk (0xFUL << FPU_MVFR1_FP_HPFP_Pos) /*!< MVFR1: FP HPFP bits Mask */
-
-#define FPU_MVFR1_D_NaN_mode_Pos 4 /*!< MVFR1: D_NaN mode bits Position */
-#define FPU_MVFR1_D_NaN_mode_Msk (0xFUL << FPU_MVFR1_D_NaN_mode_Pos) /*!< MVFR1: D_NaN mode bits Mask */
-
-#define FPU_MVFR1_FtZ_mode_Pos 0 /*!< MVFR1: FtZ mode bits Position */
-#define FPU_MVFR1_FtZ_mode_Msk (0xFUL << FPU_MVFR1_FtZ_mode_Pos) /*!< MVFR1: FtZ mode bits Mask */
-
-/*@} end of group CMSIS_FPU */
-#endif
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_CoreDebug Core Debug Registers (CoreDebug)
- \brief Type definitions for the Core Debug Registers
- @{
- */
-
-/** \brief Structure type to access the Core Debug Register (CoreDebug).
- */
-typedef struct
-{
- __IO uint32_t DHCSR; /*!< Offset: 0x000 (R/W) Debug Halting Control and Status Register */
- __O uint32_t DCRSR; /*!< Offset: 0x004 ( /W) Debug Core Register Selector Register */
- __IO uint32_t DCRDR; /*!< Offset: 0x008 (R/W) Debug Core Register Data Register */
- __IO uint32_t DEMCR; /*!< Offset: 0x00C (R/W) Debug Exception and Monitor Control Register */
-} CoreDebug_Type;
-
-/* Debug Halting Control and Status Register */
-#define CoreDebug_DHCSR_DBGKEY_Pos 16 /*!< CoreDebug DHCSR: DBGKEY Position */
-#define CoreDebug_DHCSR_DBGKEY_Msk (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos) /*!< CoreDebug DHCSR: DBGKEY Mask */
-
-#define CoreDebug_DHCSR_S_RESET_ST_Pos 25 /*!< CoreDebug DHCSR: S_RESET_ST Position */
-#define CoreDebug_DHCSR_S_RESET_ST_Msk (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos) /*!< CoreDebug DHCSR: S_RESET_ST Mask */
-
-#define CoreDebug_DHCSR_S_RETIRE_ST_Pos 24 /*!< CoreDebug DHCSR: S_RETIRE_ST Position */
-#define CoreDebug_DHCSR_S_RETIRE_ST_Msk (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos) /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */
-
-#define CoreDebug_DHCSR_S_LOCKUP_Pos 19 /*!< CoreDebug DHCSR: S_LOCKUP Position */
-#define CoreDebug_DHCSR_S_LOCKUP_Msk (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos) /*!< CoreDebug DHCSR: S_LOCKUP Mask */
-
-#define CoreDebug_DHCSR_S_SLEEP_Pos 18 /*!< CoreDebug DHCSR: S_SLEEP Position */
-#define CoreDebug_DHCSR_S_SLEEP_Msk (1UL << CoreDebug_DHCSR_S_SLEEP_Pos) /*!< CoreDebug DHCSR: S_SLEEP Mask */
-
-#define CoreDebug_DHCSR_S_HALT_Pos 17 /*!< CoreDebug DHCSR: S_HALT Position */
-#define CoreDebug_DHCSR_S_HALT_Msk (1UL << CoreDebug_DHCSR_S_HALT_Pos) /*!< CoreDebug DHCSR: S_HALT Mask */
-
-#define CoreDebug_DHCSR_S_REGRDY_Pos 16 /*!< CoreDebug DHCSR: S_REGRDY Position */
-#define CoreDebug_DHCSR_S_REGRDY_Msk (1UL << CoreDebug_DHCSR_S_REGRDY_Pos) /*!< CoreDebug DHCSR: S_REGRDY Mask */
-
-#define CoreDebug_DHCSR_C_SNAPSTALL_Pos 5 /*!< CoreDebug DHCSR: C_SNAPSTALL Position */
-#define CoreDebug_DHCSR_C_SNAPSTALL_Msk (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos) /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */
-
-#define CoreDebug_DHCSR_C_MASKINTS_Pos 3 /*!< CoreDebug DHCSR: C_MASKINTS Position */
-#define CoreDebug_DHCSR_C_MASKINTS_Msk (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos) /*!< CoreDebug DHCSR: C_MASKINTS Mask */
-
-#define CoreDebug_DHCSR_C_STEP_Pos 2 /*!< CoreDebug DHCSR: C_STEP Position */
-#define CoreDebug_DHCSR_C_STEP_Msk (1UL << CoreDebug_DHCSR_C_STEP_Pos) /*!< CoreDebug DHCSR: C_STEP Mask */
-
-#define CoreDebug_DHCSR_C_HALT_Pos 1 /*!< CoreDebug DHCSR: C_HALT Position */
-#define CoreDebug_DHCSR_C_HALT_Msk (1UL << CoreDebug_DHCSR_C_HALT_Pos) /*!< CoreDebug DHCSR: C_HALT Mask */
-
-#define CoreDebug_DHCSR_C_DEBUGEN_Pos 0 /*!< CoreDebug DHCSR: C_DEBUGEN Position */
-#define CoreDebug_DHCSR_C_DEBUGEN_Msk (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos) /*!< CoreDebug DHCSR: C_DEBUGEN Mask */
-
-/* Debug Core Register Selector Register */
-#define CoreDebug_DCRSR_REGWnR_Pos 16 /*!< CoreDebug DCRSR: REGWnR Position */
-#define CoreDebug_DCRSR_REGWnR_Msk (1UL << CoreDebug_DCRSR_REGWnR_Pos) /*!< CoreDebug DCRSR: REGWnR Mask */
-
-#define CoreDebug_DCRSR_REGSEL_Pos 0 /*!< CoreDebug DCRSR: REGSEL Position */
-#define CoreDebug_DCRSR_REGSEL_Msk (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos) /*!< CoreDebug DCRSR: REGSEL Mask */
-
-/* Debug Exception and Monitor Control Register */
-#define CoreDebug_DEMCR_TRCENA_Pos 24 /*!< CoreDebug DEMCR: TRCENA Position */
-#define CoreDebug_DEMCR_TRCENA_Msk (1UL << CoreDebug_DEMCR_TRCENA_Pos) /*!< CoreDebug DEMCR: TRCENA Mask */
-
-#define CoreDebug_DEMCR_MON_REQ_Pos 19 /*!< CoreDebug DEMCR: MON_REQ Position */
-#define CoreDebug_DEMCR_MON_REQ_Msk (1UL << CoreDebug_DEMCR_MON_REQ_Pos) /*!< CoreDebug DEMCR: MON_REQ Mask */
-
-#define CoreDebug_DEMCR_MON_STEP_Pos 18 /*!< CoreDebug DEMCR: MON_STEP Position */
-#define CoreDebug_DEMCR_MON_STEP_Msk (1UL << CoreDebug_DEMCR_MON_STEP_Pos) /*!< CoreDebug DEMCR: MON_STEP Mask */
-
-#define CoreDebug_DEMCR_MON_PEND_Pos 17 /*!< CoreDebug DEMCR: MON_PEND Position */
-#define CoreDebug_DEMCR_MON_PEND_Msk (1UL << CoreDebug_DEMCR_MON_PEND_Pos) /*!< CoreDebug DEMCR: MON_PEND Mask */
-
-#define CoreDebug_DEMCR_MON_EN_Pos 16 /*!< CoreDebug DEMCR: MON_EN Position */
-#define CoreDebug_DEMCR_MON_EN_Msk (1UL << CoreDebug_DEMCR_MON_EN_Pos) /*!< CoreDebug DEMCR: MON_EN Mask */
-
-#define CoreDebug_DEMCR_VC_HARDERR_Pos 10 /*!< CoreDebug DEMCR: VC_HARDERR Position */
-#define CoreDebug_DEMCR_VC_HARDERR_Msk (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos) /*!< CoreDebug DEMCR: VC_HARDERR Mask */
-
-#define CoreDebug_DEMCR_VC_INTERR_Pos 9 /*!< CoreDebug DEMCR: VC_INTERR Position */
-#define CoreDebug_DEMCR_VC_INTERR_Msk (1UL << CoreDebug_DEMCR_VC_INTERR_Pos) /*!< CoreDebug DEMCR: VC_INTERR Mask */
-
-#define CoreDebug_DEMCR_VC_BUSERR_Pos 8 /*!< CoreDebug DEMCR: VC_BUSERR Position */
-#define CoreDebug_DEMCR_VC_BUSERR_Msk (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos) /*!< CoreDebug DEMCR: VC_BUSERR Mask */
-
-#define CoreDebug_DEMCR_VC_STATERR_Pos 7 /*!< CoreDebug DEMCR: VC_STATERR Position */
-#define CoreDebug_DEMCR_VC_STATERR_Msk (1UL << CoreDebug_DEMCR_VC_STATERR_Pos) /*!< CoreDebug DEMCR: VC_STATERR Mask */
-
-#define CoreDebug_DEMCR_VC_CHKERR_Pos 6 /*!< CoreDebug DEMCR: VC_CHKERR Position */
-#define CoreDebug_DEMCR_VC_CHKERR_Msk (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos) /*!< CoreDebug DEMCR: VC_CHKERR Mask */
-
-#define CoreDebug_DEMCR_VC_NOCPERR_Pos 5 /*!< CoreDebug DEMCR: VC_NOCPERR Position */
-#define CoreDebug_DEMCR_VC_NOCPERR_Msk (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos) /*!< CoreDebug DEMCR: VC_NOCPERR Mask */
-
-#define CoreDebug_DEMCR_VC_MMERR_Pos 4 /*!< CoreDebug DEMCR: VC_MMERR Position */
-#define CoreDebug_DEMCR_VC_MMERR_Msk (1UL << CoreDebug_DEMCR_VC_MMERR_Pos) /*!< CoreDebug DEMCR: VC_MMERR Mask */
-
-#define CoreDebug_DEMCR_VC_CORERESET_Pos 0 /*!< CoreDebug DEMCR: VC_CORERESET Position */
-#define CoreDebug_DEMCR_VC_CORERESET_Msk (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos) /*!< CoreDebug DEMCR: VC_CORERESET Mask */
-
-/*@} end of group CMSIS_CoreDebug */
-
-
-/** \ingroup CMSIS_core_register
- \defgroup CMSIS_core_base Core Definitions
- \brief Definitions for base addresses, unions, and structures.
- @{
- */
-
-/* Memory mapping of Cortex-M4 Hardware */
-#define SCS_BASE (0xE000E000UL) /*!< System Control Space Base Address */
-#define ITM_BASE (0xE0000000UL) /*!< ITM Base Address */
-#define DWT_BASE (0xE0001000UL) /*!< DWT Base Address */
-#define TPI_BASE (0xE0040000UL) /*!< TPI Base Address */
-#define CoreDebug_BASE (0xE000EDF0UL) /*!< Core Debug Base Address */
-#define SysTick_BASE (SCS_BASE + 0x0010UL) /*!< SysTick Base Address */
-#define NVIC_BASE (SCS_BASE + 0x0100UL) /*!< NVIC Base Address */
-#define SCB_BASE (SCS_BASE + 0x0D00UL) /*!< System Control Block Base Address */
-
-#define SCnSCB ((SCnSCB_Type *) SCS_BASE ) /*!< System control Register not in SCB */
-#define SCB ((SCB_Type *) SCB_BASE ) /*!< SCB configuration struct */
-#define SysTick ((SysTick_Type *) SysTick_BASE ) /*!< SysTick configuration struct */
-#define NVIC ((NVIC_Type *) NVIC_BASE ) /*!< NVIC configuration struct */
-#define ITM ((ITM_Type *) ITM_BASE ) /*!< ITM configuration struct */
-#define DWT ((DWT_Type *) DWT_BASE ) /*!< DWT configuration struct */
-#define TPI ((TPI_Type *) TPI_BASE ) /*!< TPI configuration struct */
-#define CoreDebug ((CoreDebug_Type *) CoreDebug_BASE) /*!< Core Debug configuration struct */
-
-#if (__MPU_PRESENT == 1)
- #define MPU_BASE (SCS_BASE + 0x0D90UL) /*!< Memory Protection Unit */
- #define MPU ((MPU_Type *) MPU_BASE ) /*!< Memory Protection Unit */
-#endif
-
-#if (__FPU_PRESENT == 1)
- #define FPU_BASE (SCS_BASE + 0x0F30UL) /*!< Floating Point Unit */
- #define FPU ((FPU_Type *) FPU_BASE ) /*!< Floating Point Unit */
-#endif
-
-/*@} */
-
-
-
-/*******************************************************************************
- * Hardware Abstraction Layer
- Core Function Interface contains:
- - Core NVIC Functions
- - Core SysTick Functions
- - Core Debug Functions
- - Core Register Access Functions
- ******************************************************************************/
-/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference
-*/
-
-
-
-/* ########################## NVIC functions #################################### */
-/** \ingroup CMSIS_Core_FunctionInterface
- \defgroup CMSIS_Core_NVICFunctions NVIC Functions
- \brief Functions that manage interrupts and exceptions via the NVIC.
- @{
- */
-
-/** \brief Set Priority Grouping
-
- The function sets the priority grouping field using the required unlock sequence.
- The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.
- Only values from 0..7 are used.
- In case of a conflict between priority grouping and available
- priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.
-
- \param [in] PriorityGroup Priority grouping field.
- */
-__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)
-{
- uint32_t reg_value;
- uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07); /* only values 0..7 are used */
-
- reg_value = SCB->AIRCR; /* read old register configuration */
- reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk); /* clear bits to change */
- reg_value = (reg_value |
- ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |
- (PriorityGroupTmp << 8)); /* Insert write key and priorty group */
- SCB->AIRCR = reg_value;
-}
-
-
-/** \brief Get Priority Grouping
-
- The function reads the priority grouping field from the NVIC Interrupt Controller.
-
- \return Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).
- */
-__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)
-{
- return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos); /* read priority grouping field */
-}
-
-
-/** \brief Enable External Interrupt
-
- The function enables a device-specific interrupt in the NVIC interrupt controller.
-
- \param [in] IRQn External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)
-{
-/* NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); enable interrupt */
- NVIC->ISER[(uint32_t)((int32_t)IRQn) >> 5] = (uint32_t)(1 << ((uint32_t)((int32_t)IRQn) & (uint32_t)0x1F)); /* enable interrupt */
-}
-
-
-/** \brief Disable External Interrupt
-
- The function disables a device-specific interrupt in the NVIC interrupt controller.
-
- \param [in] IRQn External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)
-{
- NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */
-}
-
-
-/** \brief Get Pending Interrupt
-
- The function reads the pending register in the NVIC and returns the pending bit
- for the specified interrupt.
-
- \param [in] IRQn Interrupt number.
-
- \return 0 Interrupt status is not pending.
- \return 1 Interrupt status is pending.
- */
-__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)
-{
- return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */
-}
-
-
-/** \brief Set Pending Interrupt
-
- The function sets the pending bit of an external interrupt.
-
- \param [in] IRQn Interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)
-{
- NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */
-}
-
-
-/** \brief Clear Pending Interrupt
-
- The function clears the pending bit of an external interrupt.
-
- \param [in] IRQn External interrupt number. Value cannot be negative.
- */
-__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)
-{
- NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */
-}
-
-
-/** \brief Get Active Interrupt
-
- The function reads the active register in NVIC and returns the active bit.
-
- \param [in] IRQn Interrupt number.
-
- \return 0 Interrupt status is not active.
- \return 1 Interrupt status is active.
- */
-__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)
-{
- return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */
-}
-
-
-/** \brief Set Interrupt Priority
-
- The function sets the priority of an interrupt.
-
- \note The priority cannot be set for every core interrupt.
-
- \param [in] IRQn Interrupt number.
- \param [in] priority Priority to set.
- */
-__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)
-{
- if(IRQn < 0) {
- SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M System Interrupts */
- else {
- NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for device specific Interrupts */
-}
-
-
-/** \brief Get Interrupt Priority
-
- The function reads the priority of an interrupt. The interrupt
- number can be positive to specify an external (device specific)
- interrupt, or negative to specify an internal (core) interrupt.
-
-
- \param [in] IRQn Interrupt number.
- \return Interrupt Priority. Value is aligned automatically to the implemented
- priority bits of the microcontroller.
- */
-__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)
-{
-
- if(IRQn < 0) {
- return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for Cortex-M system interrupts */
- else {
- return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)] >> (8 - __NVIC_PRIO_BITS))); } /* get priority for device specific interrupts */
-}
-
-
-/** \brief Encode Priority
-
- The function encodes the priority for an interrupt with the given priority group,
- preemptive priority value, and subpriority value.
- In case of a conflict between priority grouping and available
- priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.
-
- \param [in] PriorityGroup Used priority group.
- \param [in] PreemptPriority Preemptive priority value (starting from 0).
- \param [in] SubPriority Subpriority value (starting from 0).
- \return Encoded priority. Value can be used in the function \ref NVIC_SetPriority().
- */
-__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)
-{
- uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
- uint32_t PreemptPriorityBits;
- uint32_t SubPriorityBits;
-
- PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
- SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
- return (
- ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |
- ((SubPriority & ((1 << (SubPriorityBits )) - 1)))
- );
-}
-
-
-/** \brief Decode Priority
-
- The function decodes an interrupt priority value with a given priority group to
- preemptive priority value and subpriority value.
- In case of a conflict between priority grouping and available
- priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.
-
- \param [in] Priority Priority value, which can be retrieved with the function \ref NVIC_GetPriority().
- \param [in] PriorityGroup Used priority group.
- \param [out] pPreemptPriority Preemptive priority value (starting from 0).
- \param [out] pSubPriority Subpriority value (starting from 0).
- */
-__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)
-{
- uint32_t PriorityGroupTmp = (PriorityGroup & 0x07); /* only values 0..7 are used */
- uint32_t PreemptPriorityBits;
- uint32_t SubPriorityBits;
-
- PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;
- SubPriorityBits = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;
-
- *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);
- *pSubPriority = (Priority ) & ((1 << (SubPriorityBits )) - 1);
-}
-
-
-/** \brief System Reset
-
- The function initiates a system reset request to reset the MCU.
- */
-__STATIC_INLINE void NVIC_SystemReset(void)
-{
- __DSB(); /* Ensure all outstanding memory accesses included
- buffered write are completed before reset */
- SCB->AIRCR = ((0x5FA << SCB_AIRCR_VECTKEY_Pos) |
- (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |
- SCB_AIRCR_SYSRESETREQ_Msk); /* Keep priority group unchanged */
- __DSB(); /* Ensure completion of memory access */
- while(1); /* wait until reset */
-}
-
-/*@} end of CMSIS_Core_NVICFunctions */
-
-
-
-/* ################################## SysTick function ############################################ */
-/** \ingroup CMSIS_Core_FunctionInterface
- \defgroup CMSIS_Core_SysTickFunctions SysTick Functions
- \brief Functions that configure the System.
- @{
- */
-
-#if (__Vendor_SysTickConfig == 0)
-
-/** \brief System Tick Configuration
-
- The function initializes the System Timer and its interrupt, and starts the System Tick Timer.
- Counter is in free running mode to generate periodic interrupts.
-
- \param [in] ticks Number of ticks between two interrupts.
-
- \return 0 Function succeeded.
- \return 1 Function failed.
-
- \note When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the
- function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>
- must contain a vendor-specific implementation of this function.
-
- */
-__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)
-{
- if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */
-
- SysTick->LOAD = ticks - 1; /* set reload register */
- NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Systick Interrupt */
- SysTick->VAL = 0; /* Load the SysTick Counter Value */
- SysTick->CTRL = SysTick_CTRL_CLKSOURCE_Msk |
- SysTick_CTRL_TICKINT_Msk |
- SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */
- return (0); /* Function successful */
-}
-
-#endif
-
-/*@} end of CMSIS_Core_SysTickFunctions */
-
-
-
-/* ##################################### Debug In/Output function ########################################### */
-/** \ingroup CMSIS_Core_FunctionInterface
- \defgroup CMSIS_core_DebugFunctions ITM Functions
- \brief Functions that access the ITM debug interface.
- @{
- */
-
-extern volatile int32_t ITM_RxBuffer; /*!< External variable to receive characters. */
-#define ITM_RXBUFFER_EMPTY 0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */
-
-
-/** \brief ITM Send Character
-
- The function transmits a character via the ITM channel 0, and
- \li Just returns when no debugger is connected that has booked the output.
- \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.
-
- \param [in] ch Character to transmit.
-
- \returns Character to transmit.
- */
-__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)
-{
- if ((ITM->TCR & ITM_TCR_ITMENA_Msk) && /* ITM enabled */
- (ITM->TER & (1UL << 0) ) ) /* ITM Port #0 enabled */
- {
- while (ITM->PORT[0].u32 == 0);
- ITM->PORT[0].u8 = (uint8_t) ch;
- }
- return (ch);
-}
-
-
-/** \brief ITM Receive Character
-
- The function inputs a character via the external variable \ref ITM_RxBuffer.
-
- \return Received character.
- \return -1 No character pending.
- */
-__STATIC_INLINE int32_t ITM_ReceiveChar (void) {
- int32_t ch = -1; /* no character available */
-
- if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {
- ch = ITM_RxBuffer;
- ITM_RxBuffer = ITM_RXBUFFER_EMPTY; /* ready for next character */
- }
-
- return (ch);
-}
-
-
-/** \brief ITM Check Character
-
- The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.
-
- \return 0 No character available.
- \return 1 Character available.
- */
-__STATIC_INLINE int32_t ITM_CheckChar (void) {
-
- if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {
- return (0); /* no character available */
- } else {
- return (1); /* character available */
- }
-}
-
-/*@} end of CMSIS_core_DebugFunctions */
-
-#endif /* __CORE_CM4_H_DEPENDANT */
-
-#endif /* __CMSIS_GENERIC */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h deleted file mode 100644 index af1831ee1..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h +++ /dev/null @@ -1,673 +0,0 @@ -/**************************************************************************//**
- * @file core_cm4_simd.h
- * @brief CMSIS Cortex-M4 SIMD Header File
- * @version V3.20
- * @date 25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
- 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 ARM 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 COPYRIGHT HOLDERS AND 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.
- ---------------------------------------------------------------------------*/
-
-
-#ifdef __cplusplus
- extern "C" {
-#endif
-
-#ifndef __CORE_CM4_SIMD_H
-#define __CORE_CM4_SIMD_H
-
-
-/*******************************************************************************
- * Hardware Abstraction Layer
- ******************************************************************************/
-
-
-/* ################### Compiler specific Intrinsics ########################### */
-/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
- Access to dedicated SIMD instructions
- @{
-*/
-
-#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
-/* ARM armcc specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-#define __SADD8 __sadd8
-#define __QADD8 __qadd8
-#define __SHADD8 __shadd8
-#define __UADD8 __uadd8
-#define __UQADD8 __uqadd8
-#define __UHADD8 __uhadd8
-#define __SSUB8 __ssub8
-#define __QSUB8 __qsub8
-#define __SHSUB8 __shsub8
-#define __USUB8 __usub8
-#define __UQSUB8 __uqsub8
-#define __UHSUB8 __uhsub8
-#define __SADD16 __sadd16
-#define __QADD16 __qadd16
-#define __SHADD16 __shadd16
-#define __UADD16 __uadd16
-#define __UQADD16 __uqadd16
-#define __UHADD16 __uhadd16
-#define __SSUB16 __ssub16
-#define __QSUB16 __qsub16
-#define __SHSUB16 __shsub16
-#define __USUB16 __usub16
-#define __UQSUB16 __uqsub16
-#define __UHSUB16 __uhsub16
-#define __SASX __sasx
-#define __QASX __qasx
-#define __SHASX __shasx
-#define __UASX __uasx
-#define __UQASX __uqasx
-#define __UHASX __uhasx
-#define __SSAX __ssax
-#define __QSAX __qsax
-#define __SHSAX __shsax
-#define __USAX __usax
-#define __UQSAX __uqsax
-#define __UHSAX __uhsax
-#define __USAD8 __usad8
-#define __USADA8 __usada8
-#define __SSAT16 __ssat16
-#define __USAT16 __usat16
-#define __UXTB16 __uxtb16
-#define __UXTAB16 __uxtab16
-#define __SXTB16 __sxtb16
-#define __SXTAB16 __sxtab16
-#define __SMUAD __smuad
-#define __SMUADX __smuadx
-#define __SMLAD __smlad
-#define __SMLADX __smladx
-#define __SMLALD __smlald
-#define __SMLALDX __smlaldx
-#define __SMUSD __smusd
-#define __SMUSDX __smusdx
-#define __SMLSD __smlsd
-#define __SMLSDX __smlsdx
-#define __SMLSLD __smlsld
-#define __SMLSLDX __smlsldx
-#define __SEL __sel
-#define __QADD __qadd
-#define __QSUB __qsub
-
-#define __PKHBT(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0x0000FFFFUL) | \
- ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL) )
-
-#define __PKHTB(ARG1,ARG2,ARG3) ( ((((uint32_t)(ARG1)) ) & 0xFFFF0000UL) | \
- ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL) )
-
-#define __SMMLA(ARG1,ARG2,ARG3) ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \
- ((int64_t)(ARG3) << 32) ) >> 32))
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
-/* IAR iccarm specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-#include <cmsis_iar.h>
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
-/* TI CCS specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-#include <cmsis_ccs.h>
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
-/* GNU gcc specific functions */
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
-{
- uint32_t result;
-
- __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
- return(result);
-}
-
-#define __SSAT16(ARG1,ARG2) \
-({ \
- uint32_t __RES, __ARG1 = (ARG1); \
- __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
- __RES; \
- })
-
-#define __USAT16(ARG1,ARG2) \
-({ \
- uint32_t __RES, __ARG1 = (ARG1); \
- __ASM ("usat16 %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
- __RES; \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1)
-{
- uint32_t result;
-
- __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1)
-{
- uint32_t result;
-
- __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD (uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)
-{
- uint32_t result;
-
- __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)
-{
- uint32_t result;
-
- __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
- return(result);
-}
-
-#define __SMLALD(ARG1,ARG2,ARG3) \
-({ \
- uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
- __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
- (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-#define __SMLALDX(ARG1,ARG2,ARG3) \
-({ \
- uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
- __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
- (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD (uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)
-{
- uint32_t result;
-
- __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)
-{
- uint32_t result;
-
- __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
- return(result);
-}
-
-#define __SMLSLD(ARG1,ARG2,ARG3) \
-({ \
- uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
- __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
- (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-#define __SMLSLDX(ARG1,ARG2,ARG3) \
-({ \
- uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
- __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
- (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL (uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2)
-{
- uint32_t result;
-
- __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
- return(result);
-}
-
-#define __PKHBT(ARG1,ARG2,ARG3) \
-({ \
- uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
- __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \
- __RES; \
- })
-
-#define __PKHTB(ARG1,ARG2,ARG3) \
-({ \
- uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
- if (ARG3 == 0) \
- __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2) ); \
- else \
- __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) : "r" (__ARG1), "r" (__ARG2), "I" (ARG3) ); \
- __RES; \
- })
-
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
-{
- int32_t result;
-
- __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r" (op1), "r" (op2), "r" (op3) );
- return(result);
-}
-
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-
-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
-/* TASKING carm specific functions */
-
-
-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
-/* not yet supported */
-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
-
-
-#endif
-
-/*@} end of group CMSIS_SIMD_intrinsics */
-
-
-#endif /* __CORE_CM4_SIMD_H */
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h deleted file mode 100644 index 139bc3c5e..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h +++ /dev/null @@ -1,636 +0,0 @@ -/**************************************************************************//**
- * @file core_cmFunc.h
- * @brief CMSIS Cortex-M Core Function Access Header File
- * @version V3.20
- * @date 25. February 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
- 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 ARM 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 COPYRIGHT HOLDERS AND 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 __CORE_CMFUNC_H
-#define __CORE_CMFUNC_H
-
-
-/* ########################### Core Function Access ########################### */
-/** \ingroup CMSIS_Core_FunctionInterface
- \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
- @{
- */
-
-#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
-/* ARM armcc specific functions */
-
-#if (__ARMCC_VERSION < 400677)
- #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
-#endif
-
-/* intrinsic void __enable_irq(); */
-/* intrinsic void __disable_irq(); */
-
-/** \brief Get Control Register
-
- This function returns the content of the Control Register.
-
- \return Control Register value
- */
-__STATIC_INLINE uint32_t __get_CONTROL(void)
-{
- register uint32_t __regControl __ASM("control");
- return(__regControl);
-}
-
-
-/** \brief Set Control Register
-
- This function writes the given value to the Control Register.
-
- \param [in] control Control Register value to set
- */
-__STATIC_INLINE void __set_CONTROL(uint32_t control)
-{
- register uint32_t __regControl __ASM("control");
- __regControl = control;
-}
-
-
-/** \brief Get IPSR Register
-
- This function returns the content of the IPSR Register.
-
- \return IPSR Register value
- */
-__STATIC_INLINE uint32_t __get_IPSR(void)
-{
- register uint32_t __regIPSR __ASM("ipsr");
- return(__regIPSR);
-}
-
-
-/** \brief Get APSR Register
-
- This function returns the content of the APSR Register.
-
- \return APSR Register value
- */
-__STATIC_INLINE uint32_t __get_APSR(void)
-{
- register uint32_t __regAPSR __ASM("apsr");
- return(__regAPSR);
-}
-
-
-/** \brief Get xPSR Register
-
- This function returns the content of the xPSR Register.
-
- \return xPSR Register value
- */
-__STATIC_INLINE uint32_t __get_xPSR(void)
-{
- register uint32_t __regXPSR __ASM("xpsr");
- return(__regXPSR);
-}
-
-
-/** \brief Get Process Stack Pointer
-
- This function returns the current value of the Process Stack Pointer (PSP).
-
- \return PSP Register value
- */
-__STATIC_INLINE uint32_t __get_PSP(void)
-{
- register uint32_t __regProcessStackPointer __ASM("psp");
- return(__regProcessStackPointer);
-}
-
-
-/** \brief Set Process Stack Pointer
-
- This function assigns the given value to the Process Stack Pointer (PSP).
-
- \param [in] topOfProcStack Process Stack Pointer value to set
- */
-__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
-{
- register uint32_t __regProcessStackPointer __ASM("psp");
- __regProcessStackPointer = topOfProcStack;
-}
-
-
-/** \brief Get Main Stack Pointer
-
- This function returns the current value of the Main Stack Pointer (MSP).
-
- \return MSP Register value
- */
-__STATIC_INLINE uint32_t __get_MSP(void)
-{
- register uint32_t __regMainStackPointer __ASM("msp");
- return(__regMainStackPointer);
-}
-
-
-/** \brief Set Main Stack Pointer
-
- This function assigns the given value to the Main Stack Pointer (MSP).
-
- \param [in] topOfMainStack Main Stack Pointer value to set
- */
-__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
-{
- register uint32_t __regMainStackPointer __ASM("msp");
- __regMainStackPointer = topOfMainStack;
-}
-
-
-/** \brief Get Priority Mask
-
- This function returns the current state of the priority mask bit from the Priority Mask Register.
-
- \return Priority Mask value
- */
-__STATIC_INLINE uint32_t __get_PRIMASK(void)
-{
- register uint32_t __regPriMask __ASM("primask");
- return(__regPriMask);
-}
-
-
-/** \brief Set Priority Mask
-
- This function assigns the given value to the Priority Mask Register.
-
- \param [in] priMask Priority Mask
- */
-__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
-{
- register uint32_t __regPriMask __ASM("primask");
- __regPriMask = (priMask);
-}
-
-
-#if (__CORTEX_M >= 0x03)
-
-/** \brief Enable FIQ
-
- This function enables FIQ interrupts by clearing the F-bit in the CPSR.
- Can only be executed in Privileged modes.
- */
-#define __enable_fault_irq __enable_fiq
-
-
-/** \brief Disable FIQ
-
- This function disables FIQ interrupts by setting the F-bit in the CPSR.
- Can only be executed in Privileged modes.
- */
-#define __disable_fault_irq __disable_fiq
-
-
-/** \brief Get Base Priority
-
- This function returns the current value of the Base Priority register.
-
- \return Base Priority register value
- */
-__STATIC_INLINE uint32_t __get_BASEPRI(void)
-{
- register uint32_t __regBasePri __ASM("basepri");
- return(__regBasePri);
-}
-
-
-/** \brief Set Base Priority
-
- This function assigns the given value to the Base Priority register.
-
- \param [in] basePri Base Priority value to set
- */
-__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
-{
- register uint32_t __regBasePri __ASM("basepri");
- __regBasePri = (basePri & 0xff);
-}
-
-
-/** \brief Get Fault Mask
-
- This function returns the current value of the Fault Mask register.
-
- \return Fault Mask register value
- */
-__STATIC_INLINE uint32_t __get_FAULTMASK(void)
-{
- register uint32_t __regFaultMask __ASM("faultmask");
- return(__regFaultMask);
-}
-
-
-/** \brief Set Fault Mask
-
- This function assigns the given value to the Fault Mask register.
-
- \param [in] faultMask Fault Mask value to set
- */
-__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
-{
- register uint32_t __regFaultMask __ASM("faultmask");
- __regFaultMask = (faultMask & (uint32_t)1);
-}
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-#if (__CORTEX_M == 0x04)
-
-/** \brief Get FPSCR
-
- This function returns the current value of the Floating Point Status/Control register.
-
- \return Floating Point Status/Control register value
- */
-__STATIC_INLINE uint32_t __get_FPSCR(void)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
- register uint32_t __regfpscr __ASM("fpscr");
- return(__regfpscr);
-#else
- return(0);
-#endif
-}
-
-
-/** \brief Set FPSCR
-
- This function assigns the given value to the Floating Point Status/Control register.
-
- \param [in] fpscr Floating Point Status/Control value to set
- */
-__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
- register uint32_t __regfpscr __ASM("fpscr");
- __regfpscr = (fpscr);
-#endif
-}
-
-#endif /* (__CORTEX_M == 0x04) */
-
-
-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
-/* IAR iccarm specific functions */
-
-#include <cmsis_iar.h>
-
-
-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
-/* TI CCS specific functions */
-
-#include <cmsis_ccs.h>
-
-
-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
-/* GNU gcc specific functions */
-
-/** \brief Enable IRQ Interrupts
-
- This function enables IRQ interrupts by clearing the I-bit in the CPSR.
- Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)
-{
- __ASM volatile ("cpsie i" : : : "memory");
-}
-
-
-/** \brief Disable IRQ Interrupts
-
- This function disables IRQ interrupts by setting the I-bit in the CPSR.
- Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)
-{
- __ASM volatile ("cpsid i" : : : "memory");
-}
-
-
-/** \brief Get Control Register
-
- This function returns the content of the Control Register.
-
- \return Control Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)
-{
- uint32_t result;
-
- __ASM volatile ("MRS %0, control" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Set Control Register
-
- This function writes the given value to the Control Register.
-
- \param [in] control Control Register value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)
-{
- __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
-}
-
-
-/** \brief Get IPSR Register
-
- This function returns the content of the IPSR Register.
-
- \return IPSR Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)
-{
- uint32_t result;
-
- __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Get APSR Register
-
- This function returns the content of the APSR Register.
-
- \return APSR Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)
-{
- uint32_t result;
-
- __ASM volatile ("MRS %0, apsr" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Get xPSR Register
-
- This function returns the content of the xPSR Register.
-
- \return xPSR Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)
-{
- uint32_t result;
-
- __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Get Process Stack Pointer
-
- This function returns the current value of the Process Stack Pointer (PSP).
-
- \return PSP Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
-{
- register uint32_t result;
-
- __ASM volatile ("MRS %0, psp\n" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Set Process Stack Pointer
-
- This function assigns the given value to the Process Stack Pointer (PSP).
-
- \param [in] topOfProcStack Process Stack Pointer value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
-{
- __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");
-}
-
-
-/** \brief Get Main Stack Pointer
-
- This function returns the current value of the Main Stack Pointer (MSP).
-
- \return MSP Register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
-{
- register uint32_t result;
-
- __ASM volatile ("MRS %0, msp\n" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Set Main Stack Pointer
-
- This function assigns the given value to the Main Stack Pointer (MSP).
-
- \param [in] topOfMainStack Main Stack Pointer value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
-{
- __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");
-}
-
-
-/** \brief Get Priority Mask
-
- This function returns the current state of the priority mask bit from the Priority Mask Register.
-
- \return Priority Mask value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)
-{
- uint32_t result;
-
- __ASM volatile ("MRS %0, primask" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Set Priority Mask
-
- This function assigns the given value to the Priority Mask Register.
-
- \param [in] priMask Priority Mask
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
-{
- __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
-}
-
-
-#if (__CORTEX_M >= 0x03)
-
-/** \brief Enable FIQ
-
- This function enables FIQ interrupts by clearing the F-bit in the CPSR.
- Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)
-{
- __ASM volatile ("cpsie f" : : : "memory");
-}
-
-
-/** \brief Disable FIQ
-
- This function disables FIQ interrupts by setting the F-bit in the CPSR.
- Can only be executed in Privileged modes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)
-{
- __ASM volatile ("cpsid f" : : : "memory");
-}
-
-
-/** \brief Get Base Priority
-
- This function returns the current value of the Base Priority register.
-
- \return Base Priority register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)
-{
- uint32_t result;
-
- __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Set Base Priority
-
- This function assigns the given value to the Base Priority register.
-
- \param [in] basePri Base Priority value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)
-{
- __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory");
-}
-
-
-/** \brief Get Fault Mask
-
- This function returns the current value of the Fault Mask register.
-
- \return Fault Mask register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)
-{
- uint32_t result;
-
- __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
- return(result);
-}
-
-
-/** \brief Set Fault Mask
-
- This function assigns the given value to the Fault Mask register.
-
- \param [in] faultMask Fault Mask value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
-{
- __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
-}
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-#if (__CORTEX_M == 0x04)
-
-/** \brief Get FPSCR
-
- This function returns the current value of the Floating Point Status/Control register.
-
- \return Floating Point Status/Control register value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
- uint32_t result;
-
- /* Empty asm statement works as a scheduling barrier */
- __ASM volatile ("");
- __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
- __ASM volatile ("");
- return(result);
-#else
- return(0);
-#endif
-}
-
-
-/** \brief Set FPSCR
-
- This function assigns the given value to the Floating Point Status/Control register.
-
- \param [in] fpscr Floating Point Status/Control value to set
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
-{
-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
- /* Empty asm statement works as a scheduling barrier */
- __ASM volatile ("");
- __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc");
- __ASM volatile ("");
-#endif
-}
-
-#endif /* (__CORTEX_M == 0x04) */
-
-
-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
-/* TASKING carm specific functions */
-
-/*
- * The CMSIS functions have been implemented as intrinsics in the compiler.
- * Please use "carm -?i" to get an up to date list of all instrinsics,
- * Including the CMSIS ones.
- */
-
-#endif
-
-/*@} end of CMSIS_Core_RegAccFunctions */
-
-
-#endif /* __CORE_CMFUNC_H */
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h deleted file mode 100644 index 8946c2c49..000000000 --- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h +++ /dev/null @@ -1,688 +0,0 @@ -/**************************************************************************//**
- * @file core_cmInstr.h
- * @brief CMSIS Cortex-M Core Instruction Access Header File
- * @version V3.20
- * @date 05. March 2013
- *
- * @note
- *
- ******************************************************************************/
-/* Copyright (c) 2009 - 2013 ARM LIMITED
-
- 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 ARM 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 COPYRIGHT HOLDERS AND 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 __CORE_CMINSTR_H
-#define __CORE_CMINSTR_H
-
-
-/* ########################## Core Instruction Access ######################### */
-/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
- Access to dedicated instructions
- @{
-*/
-
-#if defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
-/* ARM armcc specific functions */
-
-#if (__ARMCC_VERSION < 400677)
- #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
-#endif
-
-
-/** \brief No Operation
-
- No Operation does nothing. This instruction can be used for code alignment purposes.
- */
-#define __NOP __nop
-
-
-/** \brief Wait For Interrupt
-
- Wait For Interrupt is a hint instruction that suspends execution
- until one of a number of events occurs.
- */
-#define __WFI __wfi
-
-
-/** \brief Wait For Event
-
- Wait For Event is a hint instruction that permits the processor to enter
- a low-power state until one of a number of events occurs.
- */
-#define __WFE __wfe
-
-
-/** \brief Send Event
-
- Send Event is a hint instruction. It causes an event to be signaled to the CPU.
- */
-#define __SEV __sev
-
-
-/** \brief Instruction Synchronization Barrier
-
- Instruction Synchronization Barrier flushes the pipeline in the processor,
- so that all instructions following the ISB are fetched from cache or
- memory, after the instruction has been completed.
- */
-#define __ISB() __isb(0xF)
-
-
-/** \brief Data Synchronization Barrier
-
- This function acts as a special kind of Data Memory Barrier.
- It completes when all explicit memory accesses before this instruction complete.
- */
-#define __DSB() __dsb(0xF)
-
-
-/** \brief Data Memory Barrier
-
- This function ensures the apparent order of the explicit memory operations before
- and after the instruction, without ensuring their completion.
- */
-#define __DMB() __dmb(0xF)
-
-
-/** \brief Reverse byte order (32 bit)
-
- This function reverses the byte order in integer value.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-#define __REV __rev
-
-
-/** \brief Reverse byte order (16 bit)
-
- This function reverses the byte order in two unsigned short values.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-#ifndef __NO_EMBEDDED_ASM
-__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
-{
- rev16 r0, r0
- bx lr
-}
-#endif
-
-/** \brief Reverse byte order in signed short value
-
- This function reverses the byte order in a signed short value with sign extension to integer.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-#ifndef __NO_EMBEDDED_ASM
-__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value)
-{
- revsh r0, r0
- bx lr
-}
-#endif
-
-
-/** \brief Rotate Right in unsigned value (32 bit)
-
- This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
-
- \param [in] value Value to rotate
- \param [in] value Number of Bits to rotate
- \return Rotated value
- */
-#define __ROR __ror
-
-
-/** \brief Breakpoint
-
- This function causes the processor to enter Debug state.
- Debug tools can use this to investigate system state when the instruction at a particular address is reached.
-
- \param [in] value is ignored by the processor.
- If required, a debugger can use it to store additional information about the breakpoint.
- */
-#define __BKPT(value) __breakpoint(value)
-
-
-#if (__CORTEX_M >= 0x03)
-
-/** \brief Reverse bit order of value
-
- This function reverses the bit order of the given value.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-#define __RBIT __rbit
-
-
-/** \brief LDR Exclusive (8 bit)
-
- This function performs a exclusive LDR command for 8 bit value.
-
- \param [in] ptr Pointer to data
- \return value of type uint8_t at (*ptr)
- */
-#define __LDREXB(ptr) ((uint8_t ) __ldrex(ptr))
-
-
-/** \brief LDR Exclusive (16 bit)
-
- This function performs a exclusive LDR command for 16 bit values.
-
- \param [in] ptr Pointer to data
- \return value of type uint16_t at (*ptr)
- */
-#define __LDREXH(ptr) ((uint16_t) __ldrex(ptr))
-
-
-/** \brief LDR Exclusive (32 bit)
-
- This function performs a exclusive LDR command for 32 bit values.
-
- \param [in] ptr Pointer to data
- \return value of type uint32_t at (*ptr)
- */
-#define __LDREXW(ptr) ((uint32_t ) __ldrex(ptr))
-
-
-/** \brief STR Exclusive (8 bit)
-
- This function performs a exclusive STR command for 8 bit values.
-
- \param [in] value Value to store
- \param [in] ptr Pointer to location
- \return 0 Function succeeded
- \return 1 Function failed
- */
-#define __STREXB(value, ptr) __strex(value, ptr)
-
-
-/** \brief STR Exclusive (16 bit)
-
- This function performs a exclusive STR command for 16 bit values.
-
- \param [in] value Value to store
- \param [in] ptr Pointer to location
- \return 0 Function succeeded
- \return 1 Function failed
- */
-#define __STREXH(value, ptr) __strex(value, ptr)
-
-
-/** \brief STR Exclusive (32 bit)
-
- This function performs a exclusive STR command for 32 bit values.
-
- \param [in] value Value to store
- \param [in] ptr Pointer to location
- \return 0 Function succeeded
- \return 1 Function failed
- */
-#define __STREXW(value, ptr) __strex(value, ptr)
-
-
-/** \brief Remove the exclusive lock
-
- This function removes the exclusive lock which is created by LDREX.
-
- */
-#define __CLREX __clrex
-
-
-/** \brief Signed Saturate
-
- This function saturates a signed value.
-
- \param [in] value Value to be saturated
- \param [in] sat Bit position to saturate to (1..32)
- \return Saturated value
- */
-#define __SSAT __ssat
-
-
-/** \brief Unsigned Saturate
-
- This function saturates an unsigned value.
-
- \param [in] value Value to be saturated
- \param [in] sat Bit position to saturate to (0..31)
- \return Saturated value
- */
-#define __USAT __usat
-
-
-/** \brief Count leading zeros
-
- This function counts the number of leading zeros of a data value.
-
- \param [in] value Value to count the leading zeros
- \return number of leading zeros in value
- */
-#define __CLZ __clz
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-
-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
-/* IAR iccarm specific functions */
-
-#include <cmsis_iar.h>
-
-
-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
-/* TI CCS specific functions */
-
-#include <cmsis_ccs.h>
-
-
-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
-/* GNU gcc specific functions */
-
-/* Define macros for porting to both thumb1 and thumb2.
- * For thumb1, use low register (r0-r7), specified by constrant "l"
- * Otherwise, use general registers, specified by constrant "r" */
-#if defined (__thumb__) && !defined (__thumb2__)
-#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
-#define __CMSIS_GCC_USE_REG(r) "l" (r)
-#else
-#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
-#define __CMSIS_GCC_USE_REG(r) "r" (r)
-#endif
-
-/** \brief No Operation
-
- No Operation does nothing. This instruction can be used for code alignment purposes.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)
-{
- __ASM volatile ("nop");
-}
-
-
-/** \brief Wait For Interrupt
-
- Wait For Interrupt is a hint instruction that suspends execution
- until one of a number of events occurs.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)
-{
- __ASM volatile ("wfi");
-}
-
-
-/** \brief Wait For Event
-
- Wait For Event is a hint instruction that permits the processor to enter
- a low-power state until one of a number of events occurs.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)
-{
- __ASM volatile ("wfe");
-}
-
-
-/** \brief Send Event
-
- Send Event is a hint instruction. It causes an event to be signaled to the CPU.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)
-{
- __ASM volatile ("sev");
-}
-
-
-/** \brief Instruction Synchronization Barrier
-
- Instruction Synchronization Barrier flushes the pipeline in the processor,
- so that all instructions following the ISB are fetched from cache or
- memory, after the instruction has been completed.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)
-{
- __ASM volatile ("isb");
-}
-
-
-/** \brief Data Synchronization Barrier
-
- This function acts as a special kind of Data Memory Barrier.
- It completes when all explicit memory accesses before this instruction complete.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)
-{
- __ASM volatile ("dsb");
-}
-
-
-/** \brief Data Memory Barrier
-
- This function ensures the apparent order of the explicit memory operations before
- and after the instruction, without ensuring their completion.
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)
-{
- __ASM volatile ("dmb");
-}
-
-
-/** \brief Reverse byte order (32 bit)
-
- This function reverses the byte order in integer value.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value)
-{
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
- return __builtin_bswap32(value);
-#else
- uint32_t result;
-
- __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
- return(result);
-#endif
-}
-
-
-/** \brief Reverse byte order (16 bit)
-
- This function reverses the byte order in two unsigned short values.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value)
-{
- uint32_t result;
-
- __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
- return(result);
-}
-
-
-/** \brief Reverse byte order in signed short value
-
- This function reverses the byte order in a signed short value with sign extension to integer.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)
-{
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
- return (short)__builtin_bswap16(value);
-#else
- uint32_t result;
-
- __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
- return(result);
-#endif
-}
-
-
-/** \brief Rotate Right in unsigned value (32 bit)
-
- This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
-
- \param [in] value Value to rotate
- \param [in] value Number of Bits to rotate
- \return Rotated value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
-{
- return (op1 >> op2) | (op1 << (32 - op2));
-}
-
-
-/** \brief Breakpoint
-
- This function causes the processor to enter Debug state.
- Debug tools can use this to investigate system state when the instruction at a particular address is reached.
-
- \param [in] value is ignored by the processor.
- If required, a debugger can use it to store additional information about the breakpoint.
- */
-#define __BKPT(value) __ASM volatile ("bkpt "#value)
-
-
-#if (__CORTEX_M >= 0x03)
-
-/** \brief Reverse bit order of value
-
- This function reverses the bit order of the given value.
-
- \param [in] value Value to reverse
- \return Reversed value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
-{
- uint32_t result;
-
- __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
- return(result);
-}
-
-
-/** \brief LDR Exclusive (8 bit)
-
- This function performs a exclusive LDR command for 8 bit value.
-
- \param [in] ptr Pointer to data
- \return value of type uint8_t at (*ptr)
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr)
-{
- uint32_t result;
-
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
- __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );
-#else
- /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
- accepted by assembler. So has to use following less efficient pattern.
- */
- __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
-#endif
- return(result);
-}
-
-
-/** \brief LDR Exclusive (16 bit)
-
- This function performs a exclusive LDR command for 16 bit values.
-
- \param [in] ptr Pointer to data
- \return value of type uint16_t at (*ptr)
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr)
-{
- uint32_t result;
-
-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
- __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );
-#else
- /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
- accepted by assembler. So has to use following less efficient pattern.
- */
- __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
-#endif
- return(result);
-}
-
-
-/** \brief LDR Exclusive (32 bit)
-
- This function performs a exclusive LDR command for 32 bit values.
-
- \param [in] ptr Pointer to data
- \return value of type uint32_t at (*ptr)
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr)
-{
- uint32_t result;
-
- __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );
- return(result);
-}
-
-
-/** \brief STR Exclusive (8 bit)
-
- This function performs a exclusive STR command for 8 bit values.
-
- \param [in] value Value to store
- \param [in] ptr Pointer to location
- \return 0 Function succeeded
- \return 1 Function failed
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
-{
- uint32_t result;
-
- __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
- return(result);
-}
-
-
-/** \brief STR Exclusive (16 bit)
-
- This function performs a exclusive STR command for 16 bit values.
-
- \param [in] value Value to store
- \param [in] ptr Pointer to location
- \return 0 Function succeeded
- \return 1 Function failed
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
-{
- uint32_t result;
-
- __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
- return(result);
-}
-
-
-/** \brief STR Exclusive (32 bit)
-
- This function performs a exclusive STR command for 32 bit values.
-
- \param [in] value Value to store
- \param [in] ptr Pointer to location
- \return 0 Function succeeded
- \return 1 Function failed
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
-{
- uint32_t result;
-
- __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
- return(result);
-}
-
-
-/** \brief Remove the exclusive lock
-
- This function removes the exclusive lock which is created by LDREX.
-
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)
-{
- __ASM volatile ("clrex" ::: "memory");
-}
-
-
-/** \brief Signed Saturate
-
- This function saturates a signed value.
-
- \param [in] value Value to be saturated
- \param [in] sat Bit position to saturate to (1..32)
- \return Saturated value
- */
-#define __SSAT(ARG1,ARG2) \
-({ \
- uint32_t __RES, __ARG1 = (ARG1); \
- __ASM ("ssat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
- __RES; \
- })
-
-
-/** \brief Unsigned Saturate
-
- This function saturates an unsigned value.
-
- \param [in] value Value to be saturated
- \param [in] sat Bit position to saturate to (0..31)
- \return Saturated value
- */
-#define __USAT(ARG1,ARG2) \
-({ \
- uint32_t __RES, __ARG1 = (ARG1); \
- __ASM ("usat %0, %1, %2" : "=r" (__RES) : "I" (ARG2), "r" (__ARG1) ); \
- __RES; \
- })
-
-
-/** \brief Count leading zeros
-
- This function counts the number of leading zeros of a data value.
-
- \param [in] value Value to count the leading zeros
- \return number of leading zeros in value
- */
-__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)
-{
- uint32_t result;
-
- __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
- return(result);
-}
-
-#endif /* (__CORTEX_M >= 0x03) */
-
-
-
-
-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
-/* TASKING carm specific functions */
-
-/*
- * The CMSIS functions have been implemented as intrinsics in the compiler.
- * Please use "carm -?i" to get an up to date list of all intrinsics,
- * Including the CMSIS ones.
- */
-
-#endif
-
-/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
-
-#endif /* __CORE_CMINSTR_H */
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a Binary files differdeleted file mode 100644 index 6898bc27d..000000000 --- a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a +++ /dev/null diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a Binary files differdeleted file mode 100755 index a0185eaa9..000000000 --- a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a +++ /dev/null diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a Binary files differdeleted file mode 100755 index 94525528e..000000000 --- a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a +++ /dev/null diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/modules/mathlib/CMSIS/license.txt deleted file mode 100644 index 31afac1ec..000000000 --- a/src/modules/mathlib/CMSIS/license.txt +++ /dev/null @@ -1,27 +0,0 @@ -All pre-built libraries are guided by the following license:
-
-Copyright (C) 2009-2012 ARM Limited.
-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 ARM 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 COPYRIGHT HOLDERS AND 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.
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp deleted file mode 100644 index f509f7081..000000000 --- a/src/modules/mathlib/math/Dcm.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Dcm.cpp - * - * math direction cosine matrix - */ - -#include <mathlib/math/test/test.hpp> - -#include "Dcm.hpp" -#include "Quaternion.hpp" -#include "EulerAngles.hpp" -#include "Vector3.hpp" - -namespace math -{ - -Dcm::Dcm() : - Matrix(Matrix::identity(3)) -{ -} - -Dcm::Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - dcm(0, 0) = c00; - dcm(0, 1) = c01; - dcm(0, 2) = c02; - dcm(1, 0) = c10; - dcm(1, 1) = c11; - dcm(1, 2) = c12; - dcm(2, 0) = c20; - dcm(2, 1) = c21; - dcm(2, 2) = c22; -} - -Dcm::Dcm(const float data[3][3]) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - /* set rotation matrix */ - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - dcm(i, j) = data[i][j]; -} - -Dcm::Dcm(const float *data) : - Matrix(3, 3, data) -{ -} - -Dcm::Dcm(const Quaternion &q) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double a = q.getA(); - double b = q.getB(); - double c = q.getC(); - double d = q.getD(); - double aSq = a * a; - double bSq = b * b; - double cSq = c * c; - double dSq = d * d; - dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2.0 * (b * c - a * d); - dcm(0, 2) = 2.0 * (a * c + b * d); - dcm(1, 0) = 2.0 * (b * c + a * d); - dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2.0 * (c * d - a * b); - dcm(2, 0) = 2.0 * (b * d - a * c); - dcm(2, 1) = 2.0 * (a * b + c * d); - dcm(2, 2) = aSq - bSq - cSq + dSq; -} - -Dcm::Dcm(const EulerAngles &euler) : - Matrix(3, 3) -{ - Dcm &dcm = *this; - double cosPhi = cos(euler.getPhi()); - double sinPhi = sin(euler.getPhi()); - double cosThe = cos(euler.getTheta()); - double sinThe = sin(euler.getTheta()); - double cosPsi = cos(euler.getPsi()); - double sinPsi = sin(euler.getPsi()); - - dcm(0, 0) = cosThe * cosPsi; - dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; - dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi; - - dcm(1, 0) = cosThe * sinPsi; - dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi; - dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi; - - dcm(2, 0) = -sinThe; - dcm(2, 1) = sinPhi * cosThe; - dcm(2, 2) = cosPhi * cosThe; -} - -Dcm::Dcm(const Dcm &right) : - Matrix(right) -{ -} - -Dcm::~Dcm() -{ -} - -int __EXPORT dcmTest() -{ - printf("Test DCM\t\t: "); - // default ctor - ASSERT(matrixEqual(Dcm(), - Matrix::identity(3))); - // quaternion ctor - ASSERT(matrixEqual( - Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // euler angle ctor - ASSERT(matrixEqual( - Dcm(EulerAngles(0.1f, 0.2f, 0.3f)), - Dcm(0.9362934f, -0.2750958f, 0.2183507f, - 0.2896295f, 0.9564251f, -0.0369570f, - -0.1986693f, 0.0978434f, 0.9751703f))); - // rotations - Vector3 vB(1, 2, 3); - ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f), - Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f), - Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB)); - ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f), - Dcm(EulerAngles( - M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB)); - printf("PASS\n"); - return 0; -} -} // namespace math diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/modules/mathlib/math/EulerAngles.cpp deleted file mode 100644 index e733d23bb..000000000 --- a/src/modules/mathlib/math/EulerAngles.cpp +++ /dev/null @@ -1,126 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Vector.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "EulerAngles.hpp" -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "Vector3.hpp" - -namespace math -{ - -EulerAngles::EulerAngles() : - Vector(3) -{ - setPhi(0.0f); - setTheta(0.0f); - setPsi(0.0f); -} - -EulerAngles::EulerAngles(float phi, float theta, float psi) : - Vector(3) -{ - setPhi(phi); - setTheta(theta); - setPsi(psi); -} - -EulerAngles::EulerAngles(const Quaternion &q) : - Vector(3) -{ - (*this) = EulerAngles(Dcm(q)); -} - -EulerAngles::EulerAngles(const Dcm &dcm) : - Vector(3) -{ - setTheta(asinf(-dcm(2, 0))); - - if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) + getPhi()); - - } else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) { - setPhi(0.0f); - setPsi(atan2f(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 1)) - getPhi()); - - } else { - setPhi(atan2f(dcm(2, 1), dcm(2, 2))); - setPsi(atan2f(dcm(1, 0), dcm(0, 0))); - } -} - -EulerAngles::~EulerAngles() -{ -} - -int __EXPORT eulerAnglesTest() -{ - printf("Test EulerAngles\t: "); - EulerAngles euler(0.1f, 0.2f, 0.3f); - - // test ctor - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - ASSERT(equal(euler.getPhi(), 0.1f)); - ASSERT(equal(euler.getTheta(), 0.2f)); - ASSERT(equal(euler.getPsi(), 0.3f)); - - // test dcm ctor - euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test quat ctor - euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler)); - - // test assignment - euler.setPhi(0.4f); - euler.setTheta(0.5f); - euler.setPsi(0.6f); - ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler)); - - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/modules/mathlib/math/EulerAngles.hpp deleted file mode 100644 index 399eecfa7..000000000 --- a/src/modules/mathlib/math/EulerAngles.hpp +++ /dev/null @@ -1,74 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class Quaternion; -class Dcm; - -class __EXPORT EulerAngles : public Vector -{ -public: - EulerAngles(); - EulerAngles(float phi, float theta, float psi); - EulerAngles(const Quaternion &q); - EulerAngles(const Dcm &dcm); - virtual ~EulerAngles(); - - // alias - void setPhi(float phi) { (*this)(0) = phi; } - void setTheta(float theta) { (*this)(1) = theta; } - void setPsi(float psi) { (*this)(2) = psi; } - - // const accessors - const float &getPhi() const { return (*this)(0); } - const float &getTheta() const { return (*this)(1); } - const float &getPsi() const { return (*this)(2); } - -}; - -int __EXPORT eulerAnglesTest(); - -} // math - diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp deleted file mode 100644 index d4c892d8a..000000000 --- a/src/modules/mathlib/math/Limits.cpp +++ /dev/null @@ -1,146 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Limits.cpp - * - * Limiting / constrain helper functions - */ - - -#include <math.h> -#include <stdint.h> - -#include "Limits.hpp" - - -namespace math { - - -float __EXPORT min(float val1, float val2) -{ - return (val1 < val2) ? val1 : val2; -} - -int __EXPORT min(int val1, int val2) -{ - return (val1 < val2) ? val1 : val2; -} - -unsigned __EXPORT min(unsigned val1, unsigned val2) -{ - return (val1 < val2) ? val1 : val2; -} - -uint64_t __EXPORT min(uint64_t val1, uint64_t val2) -{ - return (val1 < val2) ? val1 : val2; -} - -double __EXPORT min(double val1, double val2) -{ - return (val1 < val2) ? val1 : val2; -} - -float __EXPORT max(float val1, float val2) -{ - return (val1 > val2) ? val1 : val2; -} - -int __EXPORT max(int val1, int val2) -{ - return (val1 > val2) ? val1 : val2; -} - -unsigned __EXPORT max(unsigned val1, unsigned val2) -{ - return (val1 > val2) ? val1 : val2; -} - -uint64_t __EXPORT max(uint64_t val1, uint64_t val2) -{ - return (val1 > val2) ? val1 : val2; -} - -double __EXPORT max(double val1, double val2) -{ - return (val1 > val2) ? val1 : val2; -} - - -float __EXPORT constrain(float val, float min, float max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -int __EXPORT constrain(int val, int min, int max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -double __EXPORT constrain(double val, double min, double max) -{ - return (val < min) ? min : ((val > max) ? max : val); -} - -float __EXPORT radians(float degrees) -{ - return (degrees / 180.0f) * M_PI_F; -} - -double __EXPORT radians(double degrees) -{ - return (degrees / 180.0) * M_PI; -} - -float __EXPORT degrees(float radians) -{ - return (radians / M_PI_F) * 180.0f; -} - -double __EXPORT degrees(double radians) -{ - return (radians / M_PI) * 180.0; -} - -}
\ No newline at end of file diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/modules/mathlib/math/Matrix.cpp deleted file mode 100644 index ebd1aeda3..000000000 --- a/src/modules/mathlib/math/Matrix.cpp +++ /dev/null @@ -1,193 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Matrix.cpp - * - * matrix code - */ - -#include "test/test.hpp" -#include <math.h> - -#include "Matrix.hpp" - -namespace math -{ - -static const float data_testA[] = { - 1, 2, 3, - 4, 5, 6 -}; -static Matrix testA(2, 3, data_testA); - -static const float data_testB[] = { - 0, 1, 3, - 7, -1, 2 -}; -static Matrix testB(2, 3, data_testB); - -static const float data_testC[] = { - 0, 1, - 2, 1, - 3, 2 -}; -static Matrix testC(3, 2, data_testC); - -static const float data_testD[] = { - 0, 1, 2, - 2, 1, 4, - 5, 2, 0 -}; -static Matrix testD(3, 3, data_testD); - -static const float data_testE[] = { - 1, -1, 2, - 0, 2, 3, - 2, -1, 1 -}; -static Matrix testE(3, 3, data_testE); - -static const float data_testF[] = { - 3.777e006f, 2.915e007f, 0.000e000f, - 2.938e007f, 2.267e008f, 0.000e000f, - 0.000e000f, 0.000e000f, 6.033e008f -}; -static Matrix testF(3, 3, data_testF); - -int __EXPORT matrixTest() -{ - matrixAddTest(); - matrixSubTest(); - matrixMultTest(); - matrixInvTest(); - matrixDivTest(); - return 0; -} - -int matrixAddTest() -{ - printf("Test Matrix Add\t\t: "); - Matrix r = testA + testB; - float data_test[] = { - 1.0f, 3.0f, 6.0f, - 11.0f, 4.0f, 8.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixSubTest() -{ - printf("Test Matrix Sub\t\t: "); - Matrix r = testA - testB; - float data_test[] = { - 1.0f, 1.0f, 0.0f, - -3.0f, 6.0f, 4.0f - }; - ASSERT(matrixEqual(Matrix(2, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixMultTest() -{ - printf("Test Matrix Mult\t: "); - Matrix r = testC * testB; - float data_test[] = { - 7.0f, -1.0f, 2.0f, - 7.0f, 1.0f, 8.0f, - 14.0f, 1.0f, 13.0f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -int matrixInvTest() -{ - printf("Test Matrix Inv\t\t: "); - Matrix origF = testF; - Matrix r = testF.inverse(); - float data_test[] = { - -0.0012518f, 0.0001610f, 0.0000000f, - 0.0001622f, -0.0000209f, 0.0000000f, - 0.0000000f, 0.0000000f, 1.6580e-9f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - // make sure F in unchanged - ASSERT(matrixEqual(origF, testF)); - printf("PASS\n"); - return 0; -} - -int matrixDivTest() -{ - printf("Test Matrix Div\t\t: "); - Matrix r = testD / testE; - float data_test[] = { - 0.2222222f, 0.5555556f, -0.1111111f, - 0.0f, 1.0f, 1.0, - -4.1111111f, 1.2222222f, 4.5555556f - }; - ASSERT(matrixEqual(Matrix(3, 3, data_test), r)); - printf("PASS\n"); - return 0; -} - -bool matrixEqual(const Matrix &a, const Matrix &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - - } else if (a.getCols() != b.getCols()) { - printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols()); - return false; - } - - bool ret = true; - - for (size_t i = 0; i < a.getRows(); i++) - for (size_t j = 0; j < a.getCols(); j++) { - if (!equal(a(i, j), b(i, j), eps)) { - printf("element mismatch (%d, %d)\n", i, j); - ret = false; - } - } - - return ret; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/modules/mathlib/math/Quaternion.cpp deleted file mode 100644 index 02fec4ca6..000000000 --- a/src/modules/mathlib/math/Quaternion.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Quaternion.cpp - * - * math vector - */ - -#include "test/test.hpp" - - -#include "Quaternion.hpp" -#include "Dcm.hpp" -#include "EulerAngles.hpp" - -namespace math -{ - -Quaternion::Quaternion() : - Vector(4) -{ - setA(1.0f); - setB(0.0f); - setC(0.0f); - setD(0.0f); -} - -Quaternion::Quaternion(float a, float b, - float c, float d) : - Vector(4) -{ - setA(a); - setB(b); - setC(c); - setD(d); -} - -Quaternion::Quaternion(const float *data) : - Vector(4, data) -{ -} - -Quaternion::Quaternion(const Vector &v) : - Vector(v) -{ -} - -Quaternion::Quaternion(const Dcm &dcm) : - Vector(4) -{ - // avoiding singularities by not using - // division equations - setA(0.5 * sqrt(1.0 + - double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); - setB(0.5 * sqrt(1.0 + - double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); - setC(0.5 * sqrt(1.0 + - double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); - setD(0.5 * sqrt(1.0 + - double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); -} - -Quaternion::Quaternion(const EulerAngles &euler) : - Vector(4) -{ - double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); - double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); - double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); - double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); - double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); - double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); - setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + - sinPhi_2 * sinTheta_2 * sinPsi_2); - setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - - cosPhi_2 * sinTheta_2 * sinPsi_2); - setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + - sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - - sinPhi_2 * sinTheta_2 * cosPsi_2); -} - -Quaternion::Quaternion(const Quaternion &right) : - Vector(right) -{ -} - -Quaternion::~Quaternion() -{ -} - -Vector Quaternion::derivative(const Vector &w) -{ -#ifdef QUATERNION_ASSERT - ASSERT(w.getRows() == 3); -#endif - float dataQ[] = { - getA(), -getB(), -getC(), -getD(), - getB(), getA(), -getD(), getC(), - getC(), getD(), getA(), -getB(), - getD(), -getC(), getB(), getA() - }; - Vector v(4); - v(0) = 0.0f; - v(1) = w(0); - v(2) = w(1); - v(3) = w(2); - Matrix Q(4, 4, dataQ); - return Q * v * 0.5f; -} - -int __EXPORT quaternionTest() -{ - printf("Test Quaternion\t\t: "); - // test default ctor - Quaternion q; - ASSERT(equal(q.getA(), 1.0f)); - ASSERT(equal(q.getB(), 0.0f)); - ASSERT(equal(q.getC(), 0.0f)); - ASSERT(equal(q.getD(), 0.0f)); - // test float ctor - q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - ASSERT(equal(q.getA(), 0.1825742f)); - ASSERT(equal(q.getB(), 0.3651484f)); - ASSERT(equal(q.getC(), 0.5477226f)); - ASSERT(equal(q.getD(), 0.7302967f)); - // test euler ctor - q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f)); - ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f))); - // test dcm ctor - q = Quaternion(Dcm()); - ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f))); - // TODO test derivative - // test accessors - q.setA(0.1f); - q.setB(0.2f); - q.setC(0.3f); - q.setD(0.4f); - ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f))); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp deleted file mode 100644 index 68e741817..000000000 --- a/src/modules/mathlib/math/Vector2f.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Vector2f.cpp - * - * math vector - */ - -#include "test/test.hpp" - -#include "Vector2f.hpp" - -namespace math -{ - -Vector2f::Vector2f() : - Vector(2) -{ -} - -Vector2f::Vector2f(const Vector &right) : - Vector(right) -{ -#ifdef VECTOR_ASSERT - ASSERT(right.getRows() == 2); -#endif -} - -Vector2f::Vector2f(float x, float y) : - Vector(2) -{ - setX(x); - setY(y); -} - -Vector2f::Vector2f(const float *data) : - Vector(2, data) -{ -} - -Vector2f::~Vector2f() -{ -} - -float Vector2f::cross(const Vector2f &b) const -{ - const Vector2f &a = *this; - return a(0)*b(1) - a(1)*b(0); -} - -float Vector2f::operator %(const Vector2f &v) const -{ - return cross(v); -} - -float Vector2f::operator *(const Vector2f &v) const -{ - return dot(v); -} - -int __EXPORT vector2fTest() -{ - printf("Test Vector2f\t\t: "); - // test float ctor - Vector2f v(1, 2); - ASSERT(equal(v(0), 1)); - ASSERT(equal(v(1), 2)); - printf("PASS\n"); - return 0; -} - -} // namespace math diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp deleted file mode 100644 index ecd62e81c..000000000 --- a/src/modules/mathlib/math/Vector2f.hpp +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Vector2f.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector2f : - public Vector -{ -public: - Vector2f(); - Vector2f(const Vector &right); - Vector2f(float x, float y); - Vector2f(const float *data); - virtual ~Vector2f(); - float cross(const Vector2f &b) const; - float operator %(const Vector2f &v) const; - float operator *(const Vector2f &v) const; - inline Vector2f operator*(const float &right) const { - return Vector::operator*(right); - } - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } -}; - -class __EXPORT Vector2 : - public Vector2f -{ -}; - -int __EXPORT vector2fTest(); -} // math - diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp deleted file mode 100644 index 568d9669a..000000000 --- a/src/modules/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,76 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Vector3.hpp - * - * math 3 vector - */ - -#pragma once - -#include "Vector.hpp" - -namespace math -{ - -class __EXPORT Vector3 : - public Vector -{ -public: - Vector3(); - Vector3(const Vector &right); - Vector3(float x, float y, float z); - Vector3(const float *data); - virtual ~Vector3(); - Vector3 cross(const Vector3 &b) const; - - /** - * accessors - */ - void setX(float x) { (*this)(0) = x; } - void setY(float y) { (*this)(1) = y; } - void setZ(float z) { (*this)(2) = z; } - const float &getX() const { return (*this)(0); } - const float &getY() const { return (*this)(1); } - const float &getZ() const { return (*this)(2); } -}; - -class __EXPORT Vector3f : - public Vector3 -{ -}; - -int __EXPORT vector3Test(); -} // math - diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/modules/mathlib/math/arm/Matrix.hpp deleted file mode 100644 index 715fd3a5e..000000000 --- a/src/modules/mathlib/math/arm/Matrix.hpp +++ /dev/null @@ -1,292 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)calloc(rows * cols, sizeof(float))); - } - Matrix(size_t rows, size_t cols, const float *data) : - _matrix() { - arm_mat_init_f32(&_matrix, - rows, cols, - (float *)malloc(rows * cols * sizeof(float))); - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] _matrix.pData; - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _matrix() { - arm_mat_init_f32(&_matrix, - right.getRows(), right.getCols(), - (float *)malloc(right.getRows()* - right.getCols()*sizeof(float))); - memcpy(getData(), right.getData(), - getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator-(float right) const { - Matrix result(getRows(), getCols()); - arm_offset_f32((float *)getData(), -right, - (float *)result.getData(), getRows()*getCols()); - return result; - } - inline Matrix operator*(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, right, - &(result._matrix)); - return result; - } - inline Matrix operator/(float right) const { - Matrix result(getRows(), getCols()); - arm_mat_scale_f32(&_matrix, 1.0f / right, - &(result._matrix)); - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix resultMat = (*this) * - Matrix(right.getRows(), 1, right.getData()); - return Vector(getRows(), resultMat.getData()); - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_add_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - arm_mat_sub_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - arm_mat_mult_f32(&_matrix, &(right._matrix), - &(result._matrix)); - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - arm_mat_trans_f32(&_matrix, &(result._matrix)); - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - Matrix result(getRows(), getCols()); - Matrix work = (*this); - arm_mat_inverse_f32(&(work._matrix), - &(result._matrix)); - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _matrix.numRows; } - inline size_t getCols() const { return _matrix.numCols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _matrix.pData; } - inline const float *getData() const { return _matrix.pData; } - inline void setData(float *data) { _matrix.pData = data; } -private: - arm_matrix_instance_f32 _matrix; -}; - -} // namespace math diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp deleted file mode 100644 index 4155800e8..000000000 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ /dev/null @@ -1,236 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" -#include "../test/test.hpp" - -// arm specific -#include "../../CMSIS/Include/arm_math.h" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator-(float right) const { - Vector result(getRows()); - arm_offset_f32((float *)getData(), - -right, result.getData(), - getRows()); - return result; - } - inline Vector operator*(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - right, result.getData(), - getRows()); - return result; - } - inline Vector operator/(float right) const { - Vector result(getRows()); - arm_scale_f32((float *)getData(), - 1.0f / right, result.getData(), - getRows()); - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_add_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - arm_sub_f32((float *)getData(), - (float *)right.getData(), - result.getData(), - getRows()); - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - arm_negate_f32((float *)getData(), - result.getData(), - getRows()); - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - arm_dot_prod_f32((float *)getData(), - (float *)right.getData(), - getRows(), - &result); - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline const float *getData() const { return _data; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp deleted file mode 100644 index efb17225d..000000000 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp +++ /dev/null @@ -1,77 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/// @file LowPassFilter.cpp -/// @brief A class to implement a second order low pass filter -/// Author: Leonard Hall <LeonardTHall@gmail.com> - -#include "LowPassFilter2p.hpp" -#include "math.h" - -namespace math -{ - -void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq) -{ - _cutoff_freq = cutoff_freq; - float fr = sample_freq/_cutoff_freq; - float ohm = tanf(M_PI_F/fr); - float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm; - _b0 = ohm*ohm/c; - _b1 = 2.0f*_b0; - _b2 = _b0; - _a1 = 2.0f*(ohm*ohm-1.0f)/c; - _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c; -} - -float LowPassFilter2p::apply(float sample) -{ - // do the filtering - float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; - if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter - delay_element_0 = sample; - } - float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; - - _delay_element_2 = _delay_element_1; - _delay_element_1 = delay_element_0; - - // return the value. Should be no need to check limits - return output; -} - -} // namespace math - diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp deleted file mode 100644 index 208ec98d4..000000000 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp +++ /dev/null @@ -1,78 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/// @file LowPassFilter.h -/// @brief A class to implement a second order low pass filter -/// Author: Leonard Hall <LeonardTHall@gmail.com> -/// Adapted for PX4 by Andrew Tridgell - -#pragma once - -namespace math -{ -class __EXPORT LowPassFilter2p -{ -public: - // constructor - LowPassFilter2p(float sample_freq, float cutoff_freq) { - // set initial parameters - set_cutoff_frequency(sample_freq, cutoff_freq); - _delay_element_1 = _delay_element_2 = 0; - } - - // change parameters - void set_cutoff_frequency(float sample_freq, float cutoff_freq); - - // apply - Add a new raw value to the filter - // and retrieve the filtered result - float apply(float sample); - - // return the cutoff frequency - float get_cutoff_freq(void) const { - return _cutoff_freq; - } - -private: - float _cutoff_freq; - float _a1; - float _a2; - float _b0; - float _b1; - float _b2; - float _delay_element_1; // buffered sample -1 - float _delay_element_2; // buffered sample -2 -}; - -} // namespace math diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/modules/mathlib/math/generic/Matrix.hpp deleted file mode 100644 index 5601a3447..000000000 --- a/src/modules/mathlib/math/generic/Matrix.hpp +++ /dev/null @@ -1,437 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Matrix.h - * - * matrix code - */ - -#pragma once - - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" -#include "../Matrix.hpp" - -namespace math -{ - -class __EXPORT Matrix -{ -public: - // constructor - Matrix(size_t rows, size_t cols) : - _rows(rows), - _cols(cols), - _data((float *)calloc(rows *cols, sizeof(float))) { - } - Matrix(size_t rows, size_t cols, const float *data) : - _rows(rows), - _cols(cols), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Matrix() { - delete [] getData(); - } - // copy constructor (deep) - Matrix(const Matrix &right) : - _rows(right.getRows()), - _cols(right.getCols()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Matrix &operator=(const Matrix &right) { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i, size_t j) { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - inline const float &operator()(size_t i, size_t j) const { -#ifdef MATRIX_ASSERT - ASSERT(i < getRows()); - ASSERT(j < getCols()); -#endif - return getData()[i * getCols() + j]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - float sig; - int exp; - float num = (*this)(i, j); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - } - // boolean ops - inline bool operator==(const Matrix &right) const { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f) - return false; - } - } - - return true; - } - // scalar ops - inline Matrix operator+(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right; - } - } - - return result; - } - inline Matrix operator-(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right; - } - } - - return result; - } - inline Matrix operator*(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) * right; - } - } - - return result; - } - inline Matrix operator/(const float &right) const { - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) / right; - } - } - - return result; - } - // vector ops - inline Vector operator*(const Vector &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i) += (*this)(i, j) * right(j); - } - } - - return result; - } - // matrix ops - inline Matrix operator+(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) + right(i, j); - } - } - - return result; - } - inline Matrix operator-(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == right.getRows()); - ASSERT(getCols() == right.getCols()); -#endif - Matrix result(getRows(), getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(i, j) = (*this)(i, j) - right(i, j); - } - } - - return result; - } - inline Matrix operator*(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(getCols() == right.getRows()); -#endif - Matrix result(getRows(), right.getCols()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < right.getCols(); j++) { - for (size_t k = 0; k < right.getRows(); k++) { - result(i, j) += (*this)(i, k) * right(k, j); - } - } - } - - return result; - } - inline Matrix operator/(const Matrix &right) const { -#ifdef MATRIX_ASSERT - ASSERT(right.getRows() == right.getCols()); - ASSERT(getCols() == right.getCols()); -#endif - return (*this) * right.inverse(); - } - // other functions - inline Matrix transpose() const { - Matrix result(getCols(), getRows()); - - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - result(j, i) = (*this)(i, j); - } - } - - return result; - } - inline void swapRows(size_t a, size_t b) { - if (a == b) return; - - for (size_t j = 0; j < getCols(); j++) { - float tmp = (*this)(a, j); - (*this)(a, j) = (*this)(b, j); - (*this)(b, j) = tmp; - } - } - inline void swapCols(size_t a, size_t b) { - if (a == b) return; - - for (size_t i = 0; i < getRows(); i++) { - float tmp = (*this)(i, a); - (*this)(i, a) = (*this)(i, b); - (*this)(i, b) = tmp; - } - } - /** - * inverse based on LU factorization with partial pivotting - */ - Matrix inverse() const { -#ifdef MATRIX_ASSERT - ASSERT(getRows() == getCols()); -#endif - size_t N = getRows(); - Matrix L = identity(N); - const Matrix &A = (*this); - Matrix U = A; - Matrix P = identity(N); - - //printf("A:\n"); A.print(); - - // for all diagonal elements - for (size_t n = 0; n < N; n++) { - - // if diagonal is zero, swap with row below - if (fabsf(U(n, n)) < 1e-8f) { - //printf("trying pivot for row %d\n",n); - for (size_t i = 0; i < N; i++) { - if (i == n) continue; - - //printf("\ttrying row %d\n",i); - if (fabsf(U(i, n)) > 1e-8f) { - //printf("swapped %d\n",i); - U.swapRows(i, n); - P.swapRows(i, n); - } - } - } - -#ifdef MATRIX_ASSERT - //printf("A:\n"); A.print(); - //printf("U:\n"); U.print(); - //printf("P:\n"); P.print(); - //fflush(stdout); - ASSERT(fabsf(U(n, n)) > 1e-8f); -#endif - - // failsafe, return zero matrix - if (fabsf(U(n, n)) < 1e-8f) { - return Matrix::zero(n); - } - - // for all rows below diagonal - for (size_t i = (n + 1); i < N; i++) { - L(i, n) = U(i, n) / U(n, n); - - // add i-th row and n-th row - // multiplied by: -a(i,n)/a(n,n) - for (size_t k = n; k < N; k++) { - U(i, k) -= L(i, n) * U(n, k); - } - } - } - - //printf("L:\n"); L.print(); - //printf("U:\n"); U.print(); - - // solve LY=P*I for Y by forward subst - Matrix Y = P; - - // for all columns of Y - for (size_t c = 0; c < N; c++) { - // for all rows of L - for (size_t i = 0; i < N; i++) { - // for all columns of L - for (size_t j = 0; j < i; j++) { - // for all existing y - // subtract the component they - // contribute to the solution - Y(i, c) -= L(i, j) * Y(j, c); - } - - // divide by the factor - // on current - // term to be solved - // Y(i,c) /= L(i,i); - // but L(i,i) = 1.0 - } - } - - //printf("Y:\n"); Y.print(); - - // solve Ux=y for x by back subst - Matrix X = Y; - - // for all columns of X - for (size_t c = 0; c < N; c++) { - // for all rows of U - for (size_t k = 0; k < N; k++) { - // have to go in reverse order - size_t i = N - 1 - k; - - // for all columns of U - for (size_t j = i + 1; j < N; j++) { - // for all existing x - // subtract the component they - // contribute to the solution - X(i, c) -= U(i, j) * X(j, c); - } - - // divide by the factor - // on current - // term to be solved - X(i, c) /= U(i, i); - } - } - - //printf("X:\n"); X.print(); - return X; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - for (size_t j = 0; j < getCols(); j++) { - (*this)(i, j) = val; - } - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } - inline size_t getCols() const { return _cols; } - inline static Matrix identity(size_t size) { - Matrix result(size, size); - - for (size_t i = 0; i < size; i++) { - result(i, i) = 1.0f; - } - - return result; - } - inline static Matrix zero(size_t size) { - Matrix result(size, size); - result.setAll(0.0f); - return result; - } - inline static Matrix zero(size_t m, size_t n) { - Matrix result(m, n); - result.setAll(0.0f); - return result; - } -protected: - inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - size_t _cols; - float *_data; -}; - -} // namespace math diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp deleted file mode 100644 index 8cfdc676d..000000000 --- a/src/modules/mathlib/math/generic/Vector.hpp +++ /dev/null @@ -1,245 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file Vector.h - * - * math vector - */ - -#pragma once - -#include <inttypes.h> -#include <assert.h> -#include <stdlib.h> -#include <string.h> -#include <stdio.h> -#include <math.h> - -#include "../Vector.hpp" - -namespace math -{ - -class __EXPORT Vector -{ -public: - // constructor - Vector(size_t rows) : - _rows(rows), - _data((float *)calloc(rows, sizeof(float))) { - } - Vector(size_t rows, const float *data) : - _rows(rows), - _data((float *)malloc(getSize())) { - memcpy(getData(), data, getSize()); - } - // deconstructor - virtual ~Vector() { - delete [] getData(); - } - // copy constructor (deep) - Vector(const Vector &right) : - _rows(right.getRows()), - _data((float *)malloc(getSize())) { - memcpy(getData(), right.getData(), - right.getSize()); - } - // assignment - inline Vector &operator=(const Vector &right) { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - - if (this != &right) { - memcpy(getData(), right.getData(), - right.getSize()); - } - - return *this; - } - // element accessors - inline float &operator()(size_t i) { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - inline const float &operator()(size_t i) const { -#ifdef VECTOR_ASSERT - ASSERT(i < getRows()); -#endif - return getData()[i]; - } - // output - inline void print() const { - for (size_t i = 0; i < getRows(); i++) { - float sig; - int exp; - float num = (*this)(i); - float2SigExp(num, sig, exp); - printf("%6.3fe%03.3d,", (double)sig, exp); - } - - printf("\n"); - } - // boolean ops - inline bool operator==(const Vector &right) const { - for (size_t i = 0; i < getRows(); i++) { - if (fabsf(((*this)(i) - right(i))) > 1e-30f) - return false; - } - - return true; - } - // scalar ops - inline Vector operator+(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right; - } - - return result; - } - inline Vector operator-(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right; - } - - return result; - } - inline Vector operator*(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) * right; - } - - return result; - } - inline Vector operator/(const float &right) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) / right; - } - - return result; - } - // vector ops - inline Vector operator+(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) + right(i); - } - - return result; - } - inline Vector operator-(const Vector &right) const { -#ifdef VECTOR_ASSERT - ASSERT(getRows() == right.getRows()); -#endif - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = (*this)(i) - right(i); - } - - return result; - } - inline Vector operator-(void) const { - Vector result(getRows()); - - for (size_t i = 0; i < getRows(); i++) { - result(i) = -((*this)(i)); - } - - return result; - } - // other functions - inline float dot(const Vector &right) const { - float result = 0; - - for (size_t i = 0; i < getRows(); i++) { - result += (*this)(i) * (*this)(i); - } - - return result; - } - inline float norm() const { - return sqrtf(dot(*this)); - } - inline float length() const { - return norm(); - } - inline Vector unit() const { - return (*this) / norm(); - } - inline Vector normalized() const { - return unit(); - } - inline void normalize() { - (*this) = (*this) / norm(); - } - inline static Vector zero(size_t rows) { - Vector result(rows); - // calloc returns zeroed memory - return result; - } - inline void setAll(const float &val) { - for (size_t i = 0; i < getRows(); i++) { - (*this)(i) = val; - } - } - inline void set(const float *data) { - memcpy(getData(), data, getSize()); - } - inline size_t getRows() const { return _rows; } -protected: - inline size_t getSize() const { return sizeof(float) * getRows(); } - inline float *getData() { return _data; } - inline const float *getData() const { return _data; } - inline void setData(float *data) { _data = data; } -private: - size_t _rows; - float *_data; -}; - -} // math diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/modules/mathlib/math/nasa_rotation_def.pdf Binary files differdeleted file mode 100644 index eb67a4bfc..000000000 --- a/src/modules/mathlib/math/nasa_rotation_def.pdf +++ /dev/null diff --git a/src/modules/mathlib/math/test/test.cpp b/src/modules/mathlib/math/test/test.cpp deleted file mode 100644 index 2fa2f7e7c..000000000 --- a/src/modules/mathlib/math/test/test.cpp +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file test.cpp - * - * Test library code - */ - -#include <stdio.h> -#include <math.h> -#include <stdlib.h> - -#include "test.hpp" - -bool __EXPORT equal(float a, float b, float epsilon) -{ - float diff = fabsf(a - b); - - if (diff > epsilon) { - printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); - return false; - - } else return true; -} - -void __EXPORT float2SigExp( - const float &num, - float &sig, - int &exp) -{ - if (isnan(num) || isinf(num)) { - sig = 0.0f; - exp = -99; - return; - } - - if (fabsf(num) < 1.0e-38f) { - sig = 0; - exp = 0; - return; - } - - exp = log10f(fabsf(num)); - - if (exp > 0) { - exp = ceil(exp); - - } else { - exp = floor(exp); - } - - sig = num; - - // cheap power since it is integer - if (exp > 0) { - for (int i = 0; i < abs(exp); i++) sig /= 10; - - } else { - for (int i = 0; i < abs(exp); i++) sig *= 10; - } -} - - diff --git a/src/modules/mathlib/math/test_math.sce b/src/modules/mathlib/math/test_math.sce deleted file mode 100644 index c3fba4729..000000000 --- a/src/modules/mathlib/math/test_math.sce +++ /dev/null @@ -1,63 +0,0 @@ -clc -clear -function out = float_truncate(in, digits) - out = round(in*10^digits) - out = out/10^digits -endfunction - -phi = 0.1 -theta = 0.2 -psi = 0.3 - -cosPhi = cos(phi) -cosPhi_2 = cos(phi/2) -sinPhi = sin(phi) -sinPhi_2 = sin(phi/2) - -cosTheta = cos(theta) -cosTheta_2 = cos(theta/2) -sinTheta = sin(theta) -sinTheta_2 = sin(theta/2) - -cosPsi = cos(psi) -cosPsi_2 = cos(psi/2) -sinPsi = sin(psi) -sinPsi_2 = sin(psi/2) - -C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi; - cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi; - -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta] - -disp(C_nb) -//C_nb = float_truncate(C_nb,3) -//disp(C_nb) - -theta = asin(-C_nb(3,1)) -phi = atan(C_nb(3,2), C_nb(3,3)) -psi = atan(C_nb(2,1), C_nb(1,1)) -printf('phi %f\n', phi) -printf('theta %f\n', theta) -printf('psi %f\n', psi) - -q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2; - sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2; - cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2; - cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2] - -//q = float_truncate(q,3) - -a = q(1) -b = q(2) -c = q(3) -d = q(4) -printf('q: %f %f %f %f\n', a, b, c, d) -a2 = a*a -b2 = b*b -c2 = c*c -d2 = d*d - -C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c); - 2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b); - 2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2] - -disp(C2_nb) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 919d01561..78e01cefb 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,6 +64,7 @@ #include <systemlib/systemlib.h> #include <systemlib/err.h> #include <mavlink/mavlink_log.h> +#include <commander/px4_custom_mode.h> #include "waypoints.h" #include "orb_topics.h" @@ -181,102 +182,80 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; - /* set mode flags independent of system state */ + /** + * Set mode flags + **/ /* HIL */ - if (v_status.flag_hil_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + if (v_status.hil_state == HIL_STATE_ON) { + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + /* arming state */ + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { - *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; - } - - /* set arming state */ - if (armed.armed) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; - } - - switch (v_status.state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - - } else { - *mavlink_state = MAV_STATE_UNINIT; + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } - - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_AUTO: + } + *mavlink_custom_mode = custom_mode.data; + + /** + * Set mavlink state + **/ + + /* set calibration state */ + if (v_status.arming_state == ARMING_STATE_INIT + || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE + || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: + } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + } else if (v_status.arming_state == ARMING_STATE_REBOOT) { *mavlink_state = MAV_STATE_POWEROFF; - break; + } else { + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; } - } @@ -312,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->act_2_sub, min_interval); orb_set_interval(subs->act_3_sub, min_interval); orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->actuators_effective_sub, min_interval); + orb_set_interval(subs->spa_sub, min_interval); + orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: @@ -534,7 +516,7 @@ void mavlink_update_system(void) int mavlink_thread_main(int argc, char *argv[]) { /* initialize mavlink text message buffering */ - mavlink_logbuffer_init(&lb, 5); + mavlink_logbuffer_init(&lb, 2); int ch; char *device_name = "/dev/ttyS1"; @@ -568,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[]) default: usage(); + break; } } @@ -674,23 +657,27 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.load * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, @@ -751,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* Reset the UART flags to original state */ tcsetattr(uart, TCSANOW, &uart_config_original); + /* destroy log buffer */ + mavlink_logbuffer_destroy(&lb); + thread_running = false; exit(0); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46..af43542da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -50,6 +50,10 @@ #include <mqueue.h> #include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> #include <time.h> #include <float.h> #include <unistd.h> @@ -67,6 +71,7 @@ #include <systemlib/err.h> #include <systemlib/airspeed.h> #include <mavlink/mavlink_log.h> +#include <commander/px4_custom_mode.h> __BEGIN_DECLS @@ -101,6 +106,10 @@ static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_gyro = -1; +static orb_advert_t pub_hil_accel = -1; +static orb_advert_t pub_hil_mag = -1; +static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t cmd_pub = -1; @@ -188,9 +197,11 @@ handle_message(mavlink_message_t *msg) mavlink_set_mode_t new_mode; mavlink_msg_set_mode_decode(msg, &new_mode); + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; + vcmd.param2 = custom_mode.main_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; @@ -412,12 +423,12 @@ handle_message(mavlink_message_t *msg) /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); float ias = calc_indicated_airspeed(imu.diff_pressure); // XXX need to fix this float tas = ias; + airspeed.timestamp = hrt_absolute_time(); airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; @@ -428,7 +439,67 @@ handle_message(mavlink_message_t *msg) } //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); - /* publish */ + /* individual sensor publications */ + struct gyro_report gyro; + gyro.x_raw = imu.xgyro / mrad2rad; + gyro.y_raw = imu.ygyro / mrad2rad; + gyro.z_raw = imu.zgyro / mrad2rad; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + gyro.timestamp = hrt_absolute_time(); + + if (pub_hil_gyro < 0) { + pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { + orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + } + + struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } + + struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); + + if (pub_hil_mag < 0) { + pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { + orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + } + + struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); + + if (pub_hil_baro < 0) { + pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { + orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); + } + + /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); } else { @@ -552,6 +623,22 @@ handle_message(mavlink_message_t *msg) } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } + + struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index bfccb2d38..5d3d6a73c 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ - mavlink_log.c \ mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index edb8761b8..53d86ec00 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_0; +struct actuator_controls_effective_s actuators_effective_0; +struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); @@ -138,6 +140,7 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, @@ -272,19 +275,23 @@ l_vehicle_status(const struct listener *l) orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, - mavlink_mode, - v_status.state_machine, + mavlink_base_mode, + mavlink_custom_mode, mavlink_state); } @@ -334,7 +341,7 @@ l_global_position(const struct listener *l) int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, @@ -470,8 +477,9 @@ l_actuator_outputs(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -488,7 +496,7 @@ l_actuator_outputs(const struct listener *l) -1, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { @@ -502,7 +510,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { @@ -516,7 +524,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, + mavlink_base_mode, 0); } else { @@ -530,7 +538,7 @@ l_actuator_outputs(const struct listener *l) (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, + mavlink_base_mode, 0); } } @@ -562,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +l_vehicle_attitude_controls_effective(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators_0.control_effective[0]); + actuators_effective_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators_0.control_effective[1]); + actuators_effective_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators_0.control_effective[2]); + actuators_effective_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators_0.control_effective[3]); + actuators_effective_0.control_effective[3]); + } +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators_0.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators_0.control[3]); } } @@ -632,12 +666,12 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.relative_alt; + float climb = -global_pos.vz; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } static void * @@ -673,7 +707,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); @@ -768,7 +802,10 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ + + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 73e278dc6..e2e630046 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -55,10 +55,12 @@ #include <uORB/topics/vehicle_global_position_set_triplet.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls_effective.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/telemetry_status.h> #include <uORB/topics/debug_key_value.h> @@ -75,8 +77,10 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; + int armed_sub; + int actuators_effective_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index a4ff06a88..5e5ee8261 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index eea928a17..16a7c2d35 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) */ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) { - //TODO: implement for z once altidude contoller is implemented - static uint16_t counter; -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; + double current_x_rad = wp->x / 180.0 * M_PI; + double current_y_rad = wp->y / 180.0 * M_PI; double x_rad = lat / 180.0 * M_PI; double y_rad = lon / 180.0 * M_PI; @@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float const double radius_earth = 6371000.0; - return radius_earth * c; + float dxy = radius_earth * c; + float dz = alt - wp->z; + + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; @@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ // XXX TODO } - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - + if (dist >= 0.f && dist <= orbit) { wpm->pos_reached = true; - } - -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } + // check if required yaw reached + float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); + float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); + if (fabsf(yaw_err) < 0.05f) { + wpm->yaw_reached = true; + } } //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { if (wpm->current_active_wp_id < wpm->size) { mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); @@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ bool time_elapsed = false; - if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } - } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { time_elapsed = true; } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { time_elapsed = true; @@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); // } - check_waypoints_reached(now, global_position , local_position); + check_waypoints_reached(now, global_position, local_position); return OK; } diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 20fb11b2c..e71344982 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -273,18 +273,18 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status->flag_control_manual_enabled) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (control_mode->flag_system_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -295,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status->state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status->flag_preflight_gyro_calibration || - v_status->flag_preflight_mag_calibration || - v_status->flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else { - *mavlink_state = MAV_STATE_UNINIT; - } - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; } +// switch (v_status->state_machine) { +// case SYSTEM_STATE_PREFLIGHT: +// if (v_status->flag_preflight_gyro_calibration || +// v_status->flag_preflight_mag_calibration || +// v_status->flag_preflight_accel_calibration) { +// *mavlink_state = MAV_STATE_CALIBRATING; +// } else { +// *mavlink_state = MAV_STATE_UNINIT; +// } +// break; +// +// case SYSTEM_STATE_STANDBY: +// *mavlink_state = MAV_STATE_STANDBY; +// break; +// +// case SYSTEM_STATE_GROUND_READY: +// *mavlink_state = MAV_STATE_ACTIVE; +// break; +// +// case SYSTEM_STATE_MANUAL: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; +// break; +// +// case SYSTEM_STATE_STABILIZED: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; +// break; +// +// case SYSTEM_STATE_AUTO: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; +// break; +// +// case SYSTEM_STATE_MISSION_ABORT: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_LANDING: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_CUTOFF: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_GROUND_ERROR: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_REBOOT: +// *mavlink_state = MAV_STATE_POWEROFF; +// break; +// } + } /** @@ -361,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; struct actuator_armed_s armed; /* work around some stupidity in task_create's argv handling */ @@ -430,19 +438,19 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.load * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 68d49c24b..0236e6126 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -327,4 +327,4 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); return thread; -}
\ No newline at end of file +} diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f56243..1b49c9ce4 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,9 +52,11 @@ #include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> +#include <uORB/topics/actuator_armed.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> #include <drivers/drv_rc_input.h> @@ -69,7 +71,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372..c84b6fd26 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 99f25cfe9..04582f2a4 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Lorenz Meier <lm@inf.ethz.ch> + * Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +39,7 @@ * Implementation of multirotor attitude control main loop. * * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -57,12 +59,13 @@ #include <drivers/drv_hrt.h> #include <uORB/uORB.h> #include <drivers/drv_gyro.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_rates_setpoint.h> +#include <uORB/topics/vehicle_status.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/parameter_update.h> @@ -74,23 +77,20 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. - __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; static int mc_task; static bool motor_test_mode = false; - -static orb_advert_t actuator_pub; - -static struct vehicle_status_s state; +static const float min_takeoff_throttle = 0.3f; +static const float yaw_deadzone = 0.01f; static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -103,7 +103,8 @@ mc_thread_main(int argc, char *argv[]) memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -112,9 +113,11 @@ mc_thread_main(int argc, char *argv[]) int param_sub = orb_subscribe(ORB_ID(parameter_update)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* * Do not rate-limit the loop to prevent aliasing @@ -134,10 +137,9 @@ mc_thread_main(int argc, char *argv[]) actuators.control[i] = 0.0f; } - actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); @@ -145,23 +147,11 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); /* welcome user */ - printf("[multirotor_att_control] starting\n"); + warnx("starting"); /* store last control mode to detect mode switches */ - bool flag_control_manual_enabled = false; - bool flag_control_attitude_enabled = false; - bool flag_system_armed = false; - - /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; - - /* store if we stopped a yaw movement */ - bool first_time_after_yaw_speed_control = true; - - /* prepare the handle for the failsafe throttle */ - param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); - float failsafe_throttle = 0.0f; - + bool reset_yaw_sp = true; while (!thread_should_exit) { @@ -183,7 +173,6 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - // XXX no params here yet } /* only run controller if attitude changed */ @@ -193,10 +182,10 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of system state */ bool updated; - orb_check(state_sub, &updated); + orb_check(control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } /* get a local copy of manual setpoint */ @@ -212,19 +201,32 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); } + /* get a local copy of status */ + orb_check(status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), status_sub, &status); + } + /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + /* set flag to safe value */ + control_yaw_position = true; + + /* reset yaw setpoint if not armed */ + if (!control_mode.flag_armed) { + reset_yaw_sp = true; + } - /** STEP 1: Define which input is the dominating control input */ - if (state.flag_control_offboard_enabled) { + /* define which input is the dominating control input */ + if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; -// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -233,108 +235,57 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; -// printf("thrust_att=%8.4f\n",offboard_sp.p4); att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + /* reset yaw setpoint after offboard control */ + reset_yaw_sp = true; + + } else if (control_mode.flag_control_manual_enabled) { + /* manual input */ + if (control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; + + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ + reset_yaw_sp = true; + } - } else if (state.flag_control_manual_enabled) { - - if (state.flag_control_attitude_enabled) { - - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; + } else { + /* only move yaw setpoint if manual input is != 0 */ + if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { + /* control yaw rate */ + control_yaw_position = false; + rates_sp.yaw = manual.yaw; + reset_yaw_sp = true; // has no effect on control, just for beautiful log } else { - att_sp.thrust = 0.0f; + control_yaw_position = true; } + } - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; - - } else { - rc_loss_first_time = true; - + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { - att_sp.yaw_body = att.yaw; + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + att_sp.thrust = manual.throttle; } - - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - - } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } - } - } - - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + att_sp.yaw_body = att.yaw; + reset_yaw_sp = false; + } if (motor_test_mode) { printf("testmode"); @@ -342,65 +293,89 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + att_sp.timestamp = hrt_absolute_time(); + + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + /* manual rate inputs (ACRO), from RC control or joystick */ + if (control_mode.flag_control_rates_enabled) { rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; rates_sp.yaw = manual.yaw; rates_sp.thrust = manual.throttle; rates_sp.timestamp = hrt_absolute_time(); } + + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; + } + + } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } } + /* reset yaw setpoint after non-manual control */ + reset_yaw_sp = true; } - /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (state.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + /* run rates controller if needed */ + if (control_mode.flag_control_rates_enabled) { + /* get current rate setpoint */ + bool rates_sp_updated = false; + orb_check(rates_sp_sub, &rates_sp_updated); - /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_updated) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } - if (rates_sp_valid) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + /* apply controller */ + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); + + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; } - /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - multirotor_control_rates(&rates_sp, gyro, &actuators); + actuators.timestamp = hrt_absolute_time(); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = state.flag_control_attitude_enabled; - flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_system_armed; - perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ } /* end of poll return value check */ } - printf("[multirotor att control] stopping, disarming motors.\n"); + warnx("stopping, disarming motors"); /* kill all outputs */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) @@ -410,7 +385,7 @@ mc_thread_main(int argc, char *argv[]) close(att_sub); - close(state_sub); + close(control_mode_sub); close(manual_sub); close(actuator_pub); close(att_sp_pub); @@ -467,11 +442,11 @@ int multirotor_att_control_main(int argc, char *argv[]) thread_should_exit = false; mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); exit(0); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8f19c6a4b..c78232f11 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } - /* reset integral if on ground */ - if (att_sp->thrust < 0.1f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); + //TODO pid_reset_integral(&yaw_controller); } - /* calculate current control outputs */ /* control pitch (forward) output */ @@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (control_yaw_position) { /* control yaw rate */ + // TODO use pid lib /* positive error: rotate to right, negative error, rotate to left (NED frame) */ // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); @@ -246,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s } rates_sp->thrust = att_sp->thrust; + //need to update the timestamp now that we've touched rates_sp + rates_sp->timestamp = hrt_absolute_time(); motor_skip_counter++; } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index e78f45c47..431a435f7 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -60,6 +60,6 @@ #include <uORB/topics/actuator_controls.h> void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e58d357d5..0a336be47 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, bool reset_integral) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* reset integral if on ground */ - if (rate_sp->thrust < 0.01f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); + // TODO pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 362b5ed86..ca7794c59 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -59,6 +59,6 @@ #include <uORB/topics/actuator_controls.h> void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, bool reset_integral); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk index d04847745..bc4b48fb4 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/multirotor_pos_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = multirotor_pos_control SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c + multirotor_pos_control_params.c \ + thrust_pid.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index f39d11438..a25448af2 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,13 +35,14 @@ /** * @file multirotor_pos_control.c * - * Skeleton for multirotor position controller + * Multirotor position controller */ #include <nuttx/config.h> #include <stdio.h> #include <stdlib.h> #include <string.h> +#include <math.h> #include <stdbool.h> #include <unistd.h> #include <fcntl.h> @@ -52,15 +53,22 @@ #include <sys/prctl.h> #include <drivers/drv_hrt.h> #include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> #include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> +#include <uORB/topics/vehicle_local_position.h> #include <uORB/topics/vehicle_local_position_setpoint.h> -#include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <systemlib/systemlib.h> +#include <systemlib/pid/pid.h> +#include <mavlink/mavlink_log.h> #include "multirotor_pos_control_params.h" +#include "thrust_pid.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -79,12 +87,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) +static float scale_control(float ctl, float end, float dz); + +static float norm(float x, float y); + +static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n"); + + fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); exit(1); } @@ -92,9 +104,9 @@ usage(const char *reason) * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call - * to task_spawn_cmd(). + * to task_spawn(). */ int multirotor_pos_control_main(int argc, char *argv[]) { @@ -104,32 +116,36 @@ int multirotor_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor pos control already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } + warnx("start"); thread_should_exit = false; - deamon_task = task_spawn_cmd("multirotor pos control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn_cmd("multirotor_pos_control", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } if (!strcmp(argv[1], "stop")) { + warnx("stop"); thread_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor pos control app is running\n"); + warnx("app is running"); + } else { - printf("\tmultirotor pos control app not started\n"); + warnx("app not started"); } + exit(0); } @@ -137,98 +153,509 @@ int multirotor_pos_control_main(int argc, char *argv[]) exit(1); } -static int -multirotor_pos_control_thread_main(int argc, char *argv[]) +static float scale_control(float ctl, float end, float dz) +{ + if (ctl > dz) { + return (ctl - dz) / (end - dz); + + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + + } else { + return 0.0f; + } +} + +static float norm(float x, float y) +{ + return sqrtf(x * x + y * y); +} + +static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos control] Control started, taking over position control\n"); + warnx("started"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[mpc] started"); /* structures */ - struct vehicle_status_s state; + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; - //struct vehicle_global_position_setpoint_s global_pos_sp; - struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_vicon_position_s local_pos; - struct manual_control_setpoint_s manual; + memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_position_setpoint_s global_pos_sp; + memset(&global_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ + int param_sub = orb_subscribe(ORB_ID(parameter_update)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); - /* publish attitude setpoint */ + /* publish setpoint */ + orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); + orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + bool global_pos_sp_reproject = false; + bool global_pos_sp_valid = false; + bool local_pos_sp_valid = false; + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + bool reset_integral = true; + bool reset_auto_pos = true; + + hrt_abstime t_prev = 0; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; + const float takeoff_alt_default = 10.0f; + float ref_alt = 0.0f; + hrt_abstime ref_alt_t = 0; + uint64_t local_ref_timestamp = 0; + + PID_t xy_pos_pids[2]; + PID_t xy_vel_pids[2]; + PID_t z_pos_pid; + thrust_pid_t z_vel_pid; + thread_running = true; - int loopcounter = 0; - - struct multirotor_position_control_params p; - struct multirotor_position_control_param_handles h; - parameters_init(&h); - parameters_update(&h, &p); - - - while (1) { - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - - if (loopcounter == 500) { - parameters_update(&h, &p); - loopcounter = 0; + struct multirotor_position_control_params params; + struct multirotor_position_control_param_handles params_h; + parameters_init(¶ms_h); + parameters_update(¶ms_h, ¶ms); + + for (int i = 0; i < 2; i++) { + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + } + + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); + thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + + while (!thread_should_exit) { + + bool param_updated; + orb_check(param_sub, ¶m_updated); + + if (param_updated) { + /* clear updated flag */ + struct parameter_update_s ps; + orb_copy(ORB_ID(parameter_update), param_sub, &ps); + /* update params */ + parameters_update(¶ms_h, ¶ms); + + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; + + if (params.xy_vel_i == 0.0f) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0f; + + } else { + i_limit = 1.0f; // not used really + } + + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); + } + + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); + } + + bool updated; + + orb_check(control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + } + + orb_check(global_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + global_pos_sp_valid = true; + global_pos_sp_reproject = true; } - // if (state.state_machine == SYSTEM_STATE_AUTO) { - - // XXX IMPLEMENT POSITION CONTROL HERE - - float dT = 1.0f / 50.0f; - - float x_setpoint = 0.0f; - - // XXX enable switching between Vicon and local position estimate - /* local pos is the Vicon position */ - - // XXX just an example, lacks rotation around world-body transformation - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; - att_sp.roll_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { - /* set setpoint to current position */ - // XXX select pos reset channel on remote - /* reset setpoint to current position (position hold) */ - // if (1 == 2) { - // local_pos_sp.x = local_pos.x; - // local_pos_sp.y = local_pos.y; - // local_pos_sp.z = local_pos.z; - // local_pos_sp.yaw = att.yaw; - // } - // } + hrt_abstime t = hrt_absolute_time(); + float dt; + + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + + } else { + dt = 0.0f; + } + + if (control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = control_mode.flag_armed; + + t_prev = t; + + if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + + float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; + float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; + float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + + if (control_mode.flag_control_manual_enabled) { + /* manual control */ + /* check for reference point updates and correct setpoint */ + if (local_pos.ref_timestamp != ref_alt_t) { + if (ref_alt_t != 0) { + /* home alt changed, don't follow large ground level changes in manual flight */ + local_pos_sp.z += local_pos.ref_alt - ref_alt; + } + + ref_alt_t = local_pos.ref_timestamp; + ref_alt = local_pos.ref_alt; + // TODO also correct XY setpoint + } + + /* reset setpoints to current position if needed */ + if (control_mode.flag_control_altitude_enabled) { + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z); + } + + /* move altitude setpoint with throttle stick */ + float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + + if (z_sp_ctl != 0.0f) { + sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; + local_pos_sp.z += sp_move_rate[2] * dt; + + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { + local_pos_sp.z = local_pos.z + z_sp_offs_max; + + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { + local_pos_sp.z = local_pos.z - z_sp_offs_max; + } + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); + } + + /* move position setpoint with roll/pitch stick */ + float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { + /* calculate direction and increment of control in NED frame */ + float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; + sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + local_pos_sp.x += sp_move_rate[0] * dt; + local_pos_sp.y += sp_move_rate[1] * dt; + /* limit maximum setpoint from position offset and preserve direction + * fail safe, should not happen in normal operation */ + float pos_vec_x = local_pos_sp.x - local_pos.x; + float pos_vec_y = local_pos_sp.y - local_pos.y; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; + + if (pos_vec_norm > 1.0f) { + local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; + local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; + } + } + } + + local_pos_sp.yaw = att_sp.yaw_body; + + /* local position setpoint is valid and can be used for loiter after position controlled mode */ + local_pos_sp_valid = control_mode.flag_control_position_enabled; + + reset_auto_pos = true; + + /* force reprojection of global setpoint after manual mode */ + global_pos_sp_reproject = true; + + } else if (control_mode.flag_control_auto_enabled) { + /* AUTO mode, use global setpoint */ + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { + reset_auto_pos = true; + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z); + } + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { + // TODO + reset_auto_pos = true; + + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); + } + + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + + local_pos_sp.yaw = global_pos_sp.yaw; + att_sp.yaw_body = global_pos_sp.yaw; + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); + } + } + + reset_auto_pos = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + global_pos_sp_reproject = true; + } + + /* reset setpoints after AUTO mode */ + reset_sp_xy = true; + reset_sp_z = true; + + } else { + /* no control, loiter or stay on ground */ + if (local_pos.landed) { + /* landed: move setpoint down */ + /* in air: hold altitude */ + if (local_pos_sp.z < 5.0f) { + /* set altitude setpoint to 5m under ground, + * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ + local_pos_sp.z = 5.0f; + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z); + } + + reset_sp_z = true; + + } else { + /* in air: hold altitude */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z); + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); + } + } + } + + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + + /* run position & altitude controllers, calculate velocity setpoint */ + if (control_mode.flag_control_altitude_enabled) { + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + + } else { + reset_sp_z = true; + global_vel_sp.vz = 0.0f; + } + + if (control_mode.flag_control_position_enabled) { + /* calculate velocity set point in NED frame */ + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; + + /* limit horizontal speed */ + float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + + if (xy_vel_sp_norm > 1.0f) { + global_vel_sp.vx /= xy_vel_sp_norm; + global_vel_sp.vy /= xy_vel_sp_norm; + } + + } else { + reset_sp_xy = true; + global_vel_sp.vx = 0.0f; + global_vel_sp.vy = 0.0f; + } + + /* publish new velocity setpoint */ + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); + // TODO subscribe to velocity setpoint if altitude/position control disabled + + if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { + /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ + float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + + if (control_mode.flag_control_climb_rate_enabled) { + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; + + if (reset_int_z_manual) { + i = manual.throttle; + + if (i < params.thr_min) { + i = params.thr_min; + + } else if (i > params.thr_max) { + i = params.thr_max; + } + } + + thrust_pid_set_integral(&z_vel_pid, -i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i); + } + + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); + att_sp.thrust = -thrust_sp[2]; + + } else { + /* reset thrust integral when altitude control enabled */ + reset_int_z = true; + } + + if (control_mode.flag_control_velocity_enabled) { + /* calculate thrust set point in NED frame */ + if (reset_int_xy) { + reset_int_xy = false; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); + } + + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); + + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ + /* limit horizontal part of thrust */ + float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); + /* assuming that vertical component of thrust is g, + * horizontal component = g * tan(alpha) */ + float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); + + if (tilt > params.tilt_max) { + tilt = params.tilt_max; + } + + /* convert direction to body frame */ + thrust_xy_dir -= att.yaw; + /* calculate roll and pitch */ + att_sp.roll_body = sinf(thrust_xy_dir) * tilt; + att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); + + } else { + reset_int_xy = true; + } + + att_sp.timestamp = hrt_absolute_time(); + + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + + } else { + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + global_pos_sp_reproject = true; + reset_auto_pos = true; + } + + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; /* run at approximately 50 Hz */ usleep(20000); - loopcounter++; - } - printf("[multirotor pos control] ending now...\n"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 6b73dc405..9c1ef2edb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,29 +33,76 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.c - * - * Parameters for EKF filter + * @file multirotor_pos_control_params.c + * + * Parameters for multirotor_pos_control */ #include "multirotor_pos_control_params.h" -/* Extended Kalman Filter covariances */ - /* controller parameters */ -PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { - /* PID parameters */ - h->p = param_find("MC_POS_P"); + h->thr_min = param_find("MPC_THR_MIN"); + h->thr_max = param_find("MPC_THR_MAX"); + h->z_p = param_find("MPC_Z_P"); + h->z_d = param_find("MPC_Z_D"); + h->z_vel_p = param_find("MPC_Z_VEL_P"); + h->z_vel_i = param_find("MPC_Z_VEL_I"); + h->z_vel_d = param_find("MPC_Z_VEL_D"); + h->z_vel_max = param_find("MPC_Z_VEL_MAX"); + h->xy_p = param_find("MPC_XY_P"); + h->xy_d = param_find("MPC_XY_D"); + h->xy_vel_p = param_find("MPC_XY_VEL_P"); + h->xy_vel_i = param_find("MPC_XY_VEL_I"); + h->xy_vel_d = param_find("MPC_XY_VEL_D"); + h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); + h->tilt_max = param_find("MPC_TILT_MAX"); + + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); return OK; } int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->p, &(p->p)); + param_get(h->thr_min, &(p->thr_min)); + param_get(h->thr_max, &(p->thr_max)); + param_get(h->z_p, &(p->z_p)); + param_get(h->z_d, &(p->z_d)); + param_get(h->z_vel_p, &(p->z_vel_p)); + param_get(h->z_vel_i, &(p->z_vel_i)); + param_get(h->z_vel_d, &(p->z_vel_d)); + param_get(h->z_vel_max, &(p->z_vel_max)); + param_get(h->xy_p, &(p->xy_p)); + param_get(h->xy_d, &(p->xy_d)); + param_get(h->xy_vel_p, &(p->xy_vel_p)); + param_get(h->xy_vel_i, &(p->xy_vel_i)); + param_get(h->xy_vel_d, &(p->xy_vel_d)); + param_get(h->xy_vel_max, &(p->xy_vel_max)); + param_get(h->tilt_max, &(p->tilt_max)); + + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 274f6c22a..3ec85a364 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,23 +33,55 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.h - * - * Parameters for position controller + * @file multirotor_pos_control_params.h + * + * Parameters for multirotor_pos_control */ #include <systemlib/param/param.h> struct multirotor_position_control_params { - float p; - float i; - float d; + float thr_min; + float thr_max; + float z_p; + float z_d; + float z_vel_p; + float z_vel_i; + float z_vel_d; + float z_vel_max; + float xy_p; + float xy_d; + float xy_vel_p; + float xy_vel_i; + float xy_vel_d; + float xy_vel_max; + float tilt_max; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; }; struct multirotor_position_control_param_handles { - param_t p; - param_t i; - param_t d; + param_t thr_min; + param_t thr_max; + param_t z_p; + param_t z_d; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t xy_p; + param_t xy_d; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; + param_t tilt_max; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; }; /** diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6..000000000 --- a/src/modules/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier <lm@inf.ethz.ch> -// * @author Laurens Mackay <mackayl@student.ethz.ch> -// * @author Tobias Naegeli <naegelit@student.ethz.ch> -// * @author Martin Rutschmann <rutmarti@student.ethz.ch> -// * -// * Redistribution and use in source and binary forms, with or without -// * modification, are permitted provided that the following conditions -// * are met: -// * -// * 1. Redistributions of source code must retain the above copyright -// * notice, this list of conditions and the following disclaimer. -// * 2. 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. -// * 3. Neither the name PX4 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 OWNER 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. -// * -// ****************************************************************************/ - -// /** -// * @file multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include <stdio.h> -// #include <stdlib.h> -// #include <stdio.h> -// #include <stdint.h> -// #include <math.h> -// #include <stdbool.h> -// #include <float.h> -// #include <systemlib/pid/pid.h> - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc34..000000000 --- a/src/modules/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Laurens Mackay <mackayl@student.ethz.ch> - * @author Tobias Naegeli <naegelit@student.ethz.ch> - * @author Martin Rutschmann <rutmarti@student.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file multirotor_position_control.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); - -// #endif /* POSITION_CONTROL_H_ */ diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c new file mode 100644 index 000000000..b985630ae --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file thrust_pid.c + * + * Implementation of thrust control PID. + * + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include "thrust_pid.h" +#include <math.h> + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->limit_min = limit_min; + pid->limit_max = limit_max; + pid->mode = mode; + pid->dt_min = dt_min; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} + +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(limit_min)) { + pid->limit_min = limit_min; + + } else { + ret = 1; + } + + if (isfinite(limit_max)) { + pid->limit_max = limit_max; + + } else { + ret = 1; + } + + return ret; +} + +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) +{ + /* Alternative integral component calculation + * + * start: + * error = setpoint - current_value + * integral = integral + (Ki * error * dt) + * derivative = (error - previous_error) / dt + * previous_error = error + * output = (Kp * error) + integral + (Kd * derivative) + * wait(dt) + * goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + /* calculate the error integral */ + i = pid->integral + (pid->ki * error * dt); + + /* attitude-thrust compensation + * r22 is (2, 2) componet of rotation matrix for current attitude */ + float att_comp; + + if (r22 > 0.8f) + att_comp = 1.0f / r22; + else if (r22 > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; + else + att_comp = 1.0f; + + /* calculate output */ + float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; + + /* check for saturation */ + if (output < pid->limit_min || output > pid->limit_max) { + /* saturated, recalculate output with old integral */ + output = (error * pid->kp) + pid->integral + (d * pid->kd); + + } else { + if (isfinite(i)) { + pid->integral = i; + } + } + + if (isfinite(output)) { + if (output > pid->limit_max) { + output = pid->limit_max; + + } else if (output < pid->limit_min) { + output = pid->limit_min; + } + + pid->last_output = output; + } + + return pid->last_output; +} + +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) +{ + pid->integral = i; +} diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/multirotor_pos_control/thrust_pid.h index fb778dd66..5e169c1ba 100644 --- a/src/modules/mathlib/math/Limits.hpp +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -1,6 +1,7 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin <anton.babushkin@me.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,56 +33,44 @@ ****************************************************************************/ /** - * @file Limits.hpp + * @file thrust_pid.h * - * Limiting / constrain helper functions + * Definition of thrust control PID interface. + * + * @author Anton Babushkin <anton.babushkin@me.com> */ -#pragma once +#ifndef THRUST_PID_H_ +#define THRUST_PID_H_ -#include <nuttx/config.h> #include <stdint.h> -namespace math { - - -float __EXPORT min(float val1, float val2); - -int __EXPORT min(int val1, int val2); - -unsigned __EXPORT min(unsigned val1, unsigned val2); - -uint64_t __EXPORT min(uint64_t val1, uint64_t val2); - -double __EXPORT min(double val1, double val2); - -float __EXPORT max(float val1, float val2); - -int __EXPORT max(int val1, int val2); - -unsigned __EXPORT max(unsigned val1, unsigned val2); - -uint64_t __EXPORT max(uint64_t val1, uint64_t val2); - -double __EXPORT max(double val1, double val2); - - -float __EXPORT constrain(float val, float min, float max); - -int __EXPORT constrain(int val, int min, int max); - -unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max); - -uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max); - -double __EXPORT constrain(double val, double min, double max); - -float __EXPORT radians(float degrees); - -double __EXPORT radians(double degrees); - -float __EXPORT degrees(float radians); - -double __EXPORT degrees(double radians); - -}
\ No newline at end of file +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ +#define THRUST_PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ +#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 + +typedef struct { + float kp; + float ki; + float kd; + float sp; + float integral; + float error_previous; + float last_output; + float limit_min; + float limit_max; + float dt_min; + uint8_t mode; +} thrust_pid_t; + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); + +__END_DECLS + +#endif /* THRUST_PID_H_ */ diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c new file mode 100644 index 000000000..13328edb4 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -0,0 +1,31 @@ +/* + * inertial_filter.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + */ + +#include "inertial_filter.h" + +void inertial_filter_predict(float dt, float x[3]) +{ + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void inertial_filter_correct(float e, float dt, float x[3], int i, float w) +{ + float ewdt = w * dt; + if (ewdt > 1.0f) + ewdt = 1.0f; // prevent over-correcting + ewdt *= e; + x[i] += ewdt; + + if (i == 0) { + x[1] += w * ewdt; + x[2] += w * w * ewdt / 3.0; + + } else if (i == 1) { + x[2] += w * ewdt; + } +} diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h new file mode 100644 index 000000000..761c17097 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -0,0 +1,13 @@ +/* + * inertial_filter.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + */ + +#include <stdbool.h> +#include <drivers/drv_hrt.h> + +void inertial_filter_predict(float dt, float x[3]); + +void inertial_filter_correct(float e, float dt, float x[3], int i, float w); diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/modules/position_estimator_inav/module.mk index 0cc1b559d..939d76849 100644 --- a/src/modules/mathlib/CMSIS/library.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -32,15 +32,10 @@ ############################################################################ # -# ARM CMSIS DSP library +# Makefile to build position_estimator_inav # -ifeq ($(CONFIG_ARCH),CORTEXM4F) -PREBUILT_LIB := libarm_cortexM4lf_math.a -else ifeq ($(CONFIG_ARCH),CORTEXM4) -PREBUILT_LIB := libarm_cortexM4l_math.a -else ifeq ($(CONFIG_ARCH),CORTEXM3) -PREBUILT_LIB := libarm_cortexM3l_math.a -else -$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library) -endif +MODULE_COMMAND = position_estimator_inav +SRCS = position_estimator_inav_main.c \ + position_estimator_inav_params.c \ + inertial_filter.c diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c new file mode 100644 index 000000000..88057b323 --- /dev/null +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -0,0 +1,653 @@ +/**************************************************************************** + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file position_estimator_inav_main.c + * Model-identification based position estimator for multirotors + */ + +#include <unistd.h> +#include <stdlib.h> +#include <stdio.h> +#include <stdbool.h> +#include <fcntl.h> +#include <float.h> +#include <string.h> +#include <nuttx/config.h> +#include <nuttx/sched.h> +#include <sys/prctl.h> +#include <termios.h> +#include <errno.h> +#include <limits.h> +#include <math.h> +#include <uORB/uORB.h> +#include <uORB/topics/parameter_update.h> +#include <uORB/topics/actuator_controls_effective.h> +#include <uORB/topics/actuator_armed.h> +#include <uORB/topics/sensor_combined.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_gps_position.h> +#include <uORB/topics/optical_flow.h> +#include <mavlink/mavlink_log.h> +#include <poll.h> +#include <systemlib/err.h> +#include <geo/geo.h> +#include <systemlib/systemlib.h> +#include <drivers/drv_hrt.h> + +#include "position_estimator_inav_params.h" +#include "inertial_filter.h" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int position_estimator_inav_task; /**< Handle of deamon task / thread */ +static bool verbose_mode = false; + +static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const uint32_t updates_counter_len = 1000000; +static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + +__EXPORT int position_estimator_inav_main(int argc, char *argv[]); + +int position_estimator_inav_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + +/** + * Print the correct usage. + */ +static void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + exit(1); +} + +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int position_estimator_inav_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + if (thread_running) { + printf("position_estimator_inav already running\n"); + /* this is not an error */ + exit(0); + } + + verbose_mode = false; + + if (argc > 1) + if (!strcmp(argv[2], "-v")) + verbose_mode = true; + + thread_should_exit = false; + position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tposition_estimator_inav is running\n"); + + } else { + printf("\tposition_estimator_inav not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/**************************************************************************** + * main + ****************************************************************************/ +int position_estimator_inav_thread_main(int argc, char *argv[]) +{ + warnx("started."); + int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[inav] started"); + + /* initialize values */ + float x_est[3] = { 0.0f, 0.0f, 0.0f }; + float y_est[3] = { 0.0f, 0.0f, 0.0f }; + float z_est[3] = { 0.0f, 0.0f, 0.0f }; + + int baro_init_cnt = 0; + int baro_init_num = 200; + float baro_alt0 = 0.0f; /* to determine while start up */ + float alt_avg = 0.0f; + bool landed = true; + hrt_abstime landed_time = 0; + bool flag_armed = false; + + uint32_t accel_counter = 0; + uint32_t baro_counter = 0; + + /* declare and safely initialize all structs */ + struct actuator_controls_effective_s actuator; + memset(&actuator, 0, sizeof(actuator)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + + /* subscribe */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); + int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* advertise */ + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + + struct position_estimator_inav_params params; + struct position_estimator_inav_param_handles pos_inav_param_handles; + /* initialize parameter handles */ + parameters_init(&pos_inav_param_handles); + + /* first parameters read at start up */ + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ + /* first parameters update */ + parameters_update(&pos_inav_param_handles, ¶ms); + + struct pollfd fds_init[1] = { + { .fd = sensor_combined_sub, .events = POLLIN }, + }; + + /* wait for initial baro value */ + bool wait_baro = true; + + thread_running = true; + + while (wait_baro && !thread_should_exit) { + int ret = poll(fds_init, 1, 1000); + + if (ret < 0) { + /* poll error */ + errx(1, "subscriptions poll error on init."); + + } else if (ret > 0) { + if (fds_init[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + + if (wait_baro && sensor.baro_counter > baro_counter) { + baro_counter = sensor.baro_counter; + + /* mean calculation over several measurements */ + if (baro_init_cnt < baro_init_num) { + baro_alt0 += sensor.baro_alt_meter; + baro_init_cnt++; + + } else { + wait_baro = false; + baro_alt0 /= (float) baro_init_cnt; + warnx("init baro: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.z_valid = true; + local_pos.v_z_valid = true; + local_pos.global_z = true; + } + } + } + } + } + + bool ref_xy_inited = false; + hrt_abstime ref_xy_init_start = 0; + const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix + + hrt_abstime t_prev = 0; + + uint16_t accel_updates = 0; + uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; + + hrt_abstime updates_counter_start = hrt_absolute_time(); + hrt_abstime pub_last = hrt_absolute_time(); + + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame + float baro_corr = 0.0f; // D + float gps_corr[2][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + }; + float sonar_corr = 0.0f; + float sonar_corr_filtered = 0.0f; + float flow_corr[] = { 0.0f, 0.0f }; // X, Y + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; + + /* main loop */ + struct pollfd fds[7] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = actuator_sub, .events = POLLIN }, + { .fd = armed_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = optical_flow_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; + + if (!thread_should_exit) { + warnx("main loop started."); + } + + while (!thread_should_exit) { + int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate + hrt_abstime t = hrt_absolute_time(); + + if (ret < 0) { + /* poll error */ + warnx("subscriptions poll error."); + thread_should_exit = true; + continue; + + } else if (ret > 0) { + /* parameter update */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, + &update); + /* update parameters */ + parameters_update(&pos_inav_param_handles, ¶ms); + } + + /* actuator */ + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator); + } + + /* armed */ + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + } + + /* vehicle attitude */ + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; + } + + /* sensor combined */ + if (fds[4].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + + if (sensor.accelerometer_counter > accel_counter) { + if (att.R_valid) { + /* correct accel bias, now only for Z */ + sensor.accelerometer_m_s2[2] -= accel_bias[2]; + + /* transform acceleration vector from body frame to NED frame */ + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + + accel_corr[0] = accel_NED[0] - x_est[2]; + accel_corr[1] = accel_NED[1] - y_est[2]; + accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; + + } else { + memset(accel_corr, 0, sizeof(accel_corr)); + } + + accel_counter = sensor.accelerometer_counter; + accel_updates++; + } + + if (sensor.baro_counter > baro_counter) { + baro_corr = - sensor.baro_alt_meter - z_est[0]; + baro_counter = sensor.baro_counter; + baro_updates++; + } + } + + /* optical flow */ + if (fds[5].revents & POLLIN) { + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { + if (flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + + if (fabsf(sonar_corr) > params.sonar_err) { + // correction is too large: spike or new ground level? + if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { + // spike detected, ignore + sonar_corr = 0.0f; + + } else { + // new ground level + baro_alt0 += sonar_corr; + mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + z_est[0] += sonar_corr; + sonar_corr = 0.0f; + sonar_corr_filtered = 0.0f; + } + } + } + + } else { + sonar_corr = 0.0f; + } + + flow_updates++; + } + + /* vehicle GPS position */ + if (fds[6].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + /* initialize reference position if needed */ + if (!ref_xy_inited) { + /* require EPH < 10m */ + if (gps.eph_m < 10.0f) { + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } + } else { + ref_xy_init_start = 0; + } + } + + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + } + + } else { + /* no GPS lock */ + memset(gps_corr, 0, sizeof(gps_corr)); + ref_xy_init_start = 0; + } + + gps_updates++; + } + } + + /* end of poll return value check */ + + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + t_prev = t; + + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + baro_alt0 -= z_est[0]; + z_est[0] = 0.0f; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + } + + /* accel bias correction, now only for Z + * not strictly correct, but stable and works */ + accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); + + /* inertial filter correction for altitude */ + baro_alt0 += sonar_corr * params.w_alt_sonar * dt; + inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); + inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); + + bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; + bool flow_valid = false; // TODO implement opt flow + + /* try to estimate xy even if no absolute position source available, + * if using optical flow velocity will be correct in this case */ + bool can_estimate_xy = gps_valid || flow_valid; + + if (can_estimate_xy) { + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); + + /* inertial filter correction for position */ + inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); + + if (gps_valid) { + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + } + } + } + + /* detect land */ + alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp = z_est[0] - alt_avg; + alt_disp = alt_disp * alt_disp; + float land_disp2 = params.land_disp * params.land_disp; + /* get actual thrust output */ + float thrust = armed.armed ? actuator.control_effective[3] : 0.0f; + + if (landed) { + if (alt_disp > land_disp2 && thrust > params.land_thr) { + landed = false; + landed_time = 0; + } + + } else { + if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (landed_time == 0) { + landed_time = t; // land detected first time + + } else { + if (t > landed_time + params.land_t * 1000000.0f) { + landed = true; + landed_time = 0; + } + } + + } else { + landed_time = 0; + } + } + + if (verbose_mode) { + /* print updates rate */ + if (t > updates_counter_start + updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + warnx( + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", + accel_updates / updates_dt, + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt, + flow_updates / updates_dt); + updates_counter_start = t; + accel_updates = 0; + baro_updates = 0; + gps_updates = 0; + attitude_updates = 0; + flow_updates = 0; + } + } + + if (t > pub_last + pub_interval) { + pub_last = t; + /* publish local position */ + local_pos.timestamp = t; + local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.v_xy_valid = can_estimate_xy; + local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.x = x_est[0]; + local_pos.vx = x_est[1]; + local_pos.y = y_est[0]; + local_pos.vy = y_est[1]; + local_pos.z = z_est[0]; + local_pos.vz = z_est[1]; + local_pos.landed = landed; + local_pos.yaw = att.yaw; + + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + + /* publish global position */ + global_pos.valid = local_pos.global_xy; + + if (local_pos.global_xy) { + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.time_gps_usec = gps.time_gps_usec; + } + + /* set valid values even if position is not valid */ + if (local_pos.v_xy_valid) { + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + } + + if (local_pos.z_valid) { + global_pos.relative_alt = -local_pos.z; + } + + if (local_pos.global_z) { + global_pos.alt = local_pos.ref_alt - local_pos.z; + } + + if (local_pos.v_z_valid) { + global_pos.vz = local_pos.vz; + } + global_pos.yaw = local_pos.yaw; + + global_pos.timestamp = t; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } + flag_armed = armed.armed; + } + + warnx("exiting."); + mavlink_log_info(mavlink_fd, "[inav] exiting"); + thread_running = false; + return 0; +} diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c new file mode 100644 index 000000000..4f9ddd009 --- /dev/null +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/* + * @file position_estimator_inav_params.c + * + * Parameters for position_estimator_inav + */ + +#include "position_estimator_inav_params.h" + +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); +PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); +PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); + +int parameters_init(struct position_estimator_inav_param_handles *h) +{ + h->w_alt_baro = param_find("INAV_W_ALT_BARO"); + h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); + h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); + h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); + h->w_pos_acc = param_find("INAV_W_POS_ACC"); + h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); + h->flow_k = param_find("INAV_FLOW_K"); + h->sonar_filt = param_find("INAV_SONAR_FILT"); + h->sonar_err = param_find("INAV_SONAR_ERR"); + h->land_t = param_find("INAV_LAND_T"); + h->land_disp = param_find("INAV_LAND_DISP"); + h->land_thr = param_find("INAV_LAND_THR"); + + return OK; +} + +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ + param_get(h->w_alt_baro, &(p->w_alt_baro)); + param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_alt_sonar, &(p->w_alt_sonar)); + param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); + param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); + param_get(h->w_pos_acc, &(p->w_pos_acc)); + param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_acc_bias, &(p->w_acc_bias)); + param_get(h->flow_k, &(p->flow_k)); + param_get(h->sonar_filt, &(p->sonar_filt)); + param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->land_t, &(p->land_t)); + param_get(h->land_disp, &(p->land_disp)); + param_get(h->land_thr, &(p->land_thr)); + + return OK; +} diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/position_estimator_inav/position_estimator_inav_params.h index df8970d3a..61570aea7 100644 --- a/src/modules/mathlib/math/Dcm.hpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -1,6 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -31,78 +32,56 @@ * ****************************************************************************/ -/** - * @file Dcm.hpp +/* + * @file position_estimator_inav_params.h * - * math direction cosine matrix + * Parameters for Position Estimator */ -//#pragma once - -#include "Vector.hpp" -#include "Matrix.hpp" - -namespace math -{ +#include <systemlib/param/param.h> + +struct position_estimator_inav_params { + float w_alt_baro; + float w_alt_acc; + float w_alt_sonar; + float w_pos_gps_p; + float w_pos_gps_v; + float w_pos_acc; + float w_pos_flow; + float w_acc_bias; + float flow_k; + float sonar_filt; + float sonar_err; + float land_t; + float land_disp; + float land_thr; +}; -class Quaternion; -class EulerAngles; +struct position_estimator_inav_param_handles { + param_t w_alt_baro; + param_t w_alt_acc; + param_t w_alt_sonar; + param_t w_pos_gps_p; + param_t w_pos_gps_v; + param_t w_pos_acc; + param_t w_pos_flow; + param_t w_acc_bias; + param_t flow_k; + param_t sonar_filt; + param_t sonar_err; + param_t land_t; + param_t land_disp; + param_t land_thr; +}; /** - * This is a Tait Bryan, Body 3-2-1 sequence. - * (yaw)-(pitch)-(roll) - * The Dcm transforms a vector in the body frame - * to the navigation frame, typically represented - * as C_nb. C_bn can be obtained through use - * of the transpose() method. + * Initialize all parameter handles and values + * */ -class __EXPORT Dcm : public Matrix -{ -public: - /** - * default ctor - */ - Dcm(); - - /** - * scalar ctor - */ - Dcm(float c00, float c01, float c02, - float c10, float c11, float c12, - float c20, float c21, float c22); - - /** - * data ctor - */ - Dcm(const float *data); - - /** - * array ctor - */ - Dcm(const float data[3][3]); - - /** - * quaternion ctor - */ - Dcm(const Quaternion &q); - - /** - * euler angles ctor - */ - Dcm(const EulerAngles &euler); - - /** - * copy ctor (deep) - */ - Dcm(const Dcm &right); - - /** - * dtor - */ - virtual ~Dcm(); -}; - -int __EXPORT dcmTest(); - -} // math +int parameters_init(struct position_estimator_inav_param_handles *h); +/** + * Update all parameters + * + */ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index fbd82a4c6..796c6cd9f 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -59,14 +59,14 @@ static perf_counter_t c_gather_ppm; void controls_init(void) { - /* DSM input */ + /* DSM input (USART1) */ dsm_init("/dev/ttyS0"); - /* S.bus input */ + /* S.bus input (USART3) */ sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0; @@ -124,7 +124,7 @@ controls_tick() { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS); /* * In some cases we may have received a frame, but input has still @@ -197,7 +197,7 @@ controls_tick() { /* and update the scaled/mapped version */ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < MAX_CONTROL_CHANNELS); + ASSERT(mapped < PX4IO_CONTROL_CHANNELS); /* invert channel if pitch - pulling the lever down means pitching up by convention */ if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */ @@ -209,7 +209,7 @@ controls_tick() { } /* set un-assigned controls to zero */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) { if (!(assigned_channels & (1 << i))) r_rc_values[i] = 0; } @@ -321,8 +321,8 @@ ppm_input(uint16_t *values, uint16_t *num_values) /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; - if (*num_values > MAX_CONTROL_CHANNELS) - *num_values = MAX_CONTROL_CHANNELS; + if (*num_values > PX4IO_CONTROL_CHANNELS) + *num_values = PX4IO_CONTROL_CHANNELS; for (unsigned i = 0; i < *num_values; i++) values[i] = ppm_buffer[i]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 745cdfa40..206e27db5 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -53,13 +53,13 @@ #define DSM_FRAME_SIZE 16 /**<DSM frame size in bytes*/ #define DSM_FRAME_CHANNELS 7 /**<Max supported DSM channels*/ -static int dsm_fd = -1; /**<File handle to the DSM UART*/ -static hrt_abstime dsm_last_rx_time; /**<Timestamp when we last received*/ -static hrt_abstime dsm_last_frame_time; /**<Timestamp for start of last dsm frame*/ -static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**<DSM dsm frame receive buffer*/ -static unsigned dsm_partial_frame_count; /**<Count of bytes received for current dsm frame*/ -static unsigned dsm_channel_shift; /**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/ -static unsigned dsm_frame_drops; /**<Count of incomplete DSM frames*/ +static int dsm_fd = -1; /**< File handle to the DSM UART */ +static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received */ +static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last dsm frame */ +static uint8_t dsm_frame[DSM_FRAME_SIZE]; /**< DSM dsm frame receive buffer */ +static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */ +static unsigned dsm_channel_shift; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ +static unsigned dsm_frame_drops; /**< Count of incomplete DSM frames */ /** * Attempt to decode a single channel raw channel datum @@ -249,6 +249,10 @@ dsm_bind(uint16_t cmd, int pulses) if (dsm_fd < 0) return; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + // XXX implement + #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 +#else switch (cmd) { case dsm_bind_power_down: @@ -288,6 +292,7 @@ dsm_bind(uint16_t cmd, int pulses) break; } +#endif } /** diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c index 4485daa5b..10aeb5c9f 100644 --- a/src/modules/px4iofirmware/i2c.c +++ b/src/modules/px4iofirmware/i2c.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -69,6 +69,7 @@ static void i2c_rx_setup(void); static void i2c_tx_setup(void); static void i2c_rx_complete(void); static void i2c_tx_complete(void); +static void i2c_dump(void); static DMA_HANDLE rx_dma; static DMA_HANDLE tx_dma; @@ -92,7 +93,7 @@ enum { } direction; void -i2c_init(void) +interface_init(void) { debug("i2c init"); @@ -148,12 +149,18 @@ i2c_init(void) #endif } +void +interface_tick() +{ +} + /* reset the I2C bus used to recover from lockups */ -void i2c_reset(void) +void +i2c_reset(void) { rCR1 |= I2C_CR1_SWRST; rCR1 = 0; @@ -330,7 +337,7 @@ i2c_tx_complete(void) i2c_tx_setup(); } -void +static void i2c_dump(void) { debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526..deed25836 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -59,6 +59,12 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 +/* + * Time that the ESCs need to initialize + */ + #define ESC_INIT_TIME_US 1000000 + #define ESC_RAMP_TIME_US 2000000 + /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -68,6 +74,17 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static bool should_arm = false; +static bool should_always_enable_pwm = false; +static uint64_t esc_init_time; + +enum esc_state_e { + ESC_OFF, + ESC_INIT, + ESC_RAMP, + ESC_ON +}; +static esc_state_e esc_state; /* selected control values and count for mixing */ enum mixer_source { @@ -98,7 +115,7 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { @@ -112,12 +129,11 @@ mixer_tick(void) * Decide which set of controls we're using. */ - /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ + /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values or - not a valid mixer. */ + /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { @@ -132,7 +148,8 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; @@ -154,7 +171,7 @@ mixer_tick(void) if (source == MIX_FAILSAFE) { /* copy failsafe values to the servo outputs */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) { + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { r_page_servos[i] = r_page_servo_failsafe[i]; /* safe actuators for FMU feedback */ @@ -164,11 +181,53 @@ mixer_tick(void) } else if (source != MIX_NONE) { - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; + uint16_t ramp_promille; + + /* update esc init state, but only if we are truely armed and not just PWM enabled */ + if (mixer_servos_armed && should_arm) { + + switch (esc_state) { + + /* after arming, some ESCs need an initalization period, count the time from here */ + case ESC_OFF: + esc_init_time = hrt_absolute_time(); + esc_state = ESC_INIT; + break; + + /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ + case ESC_INIT: + if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { + esc_state = ESC_RAMP; + } + break; + + /* then ramp until the min speed is reached */ + case ESC_RAMP: + if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { + esc_state = ESC_ON; + } + break; + + case ESC_ON: + default: + + break; + } + } else { + esc_state = ESC_OFF; + } + + /* do the calculations during the ramp for all at once */ + if(esc_state == ESC_RAMP) { + ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; + } + + /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -176,11 +235,29 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 600.0f) + 1500; - + switch (esc_state) { + case ESC_INIT: + r_page_servos[i] = (outputs[i] * 600 + 1500); + break; + + case ESC_RAMP: + r_page_servos[i] = (outputs[i] + * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 + + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); + break; + + case ESC_ON: + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + break; + + case ESC_OFF: + default: + break; + } } - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servos[i] = 0; } @@ -193,30 +270,46 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = ( - /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* FMU is available or FMU is not available but override is an option */ - ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) ); - if (should_arm && !mixer_servos_armed) { + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + + if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> PWM enabled"); - } else if (!should_arm && mixer_servos_armed) { + } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> PWM disabled"); + } - if (mixer_servos_armed) { + if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + + } else if (mixer_servos_armed && should_always_enable_pwm) { + /* set the idle servo outputs. */ + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servo_idle[i]); } } @@ -265,9 +358,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a mixer change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { return; } @@ -344,16 +436,17 @@ mixer_set_failsafe() * Check if a custom failsafe value has been written, * or if the mixer is not ok and bail out. */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) || !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) return; /* set failsafe defaults to the values for all inputs = 0 */ - float outputs[IO_SERVO_COUNT]; + float outputs[PX4IO_SERVO_COUNT]; unsigned mixed; /* mix */ - mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT); /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -364,7 +457,7 @@ mixer_set_failsafe() } /* disable the rest of the outputs */ - for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++) r_page_servo_failsafe[i] = 0; } diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk index 6379366f4..59f470a94 100644 --- a/src/modules/px4iofirmware/module.mk +++ b/src/modules/px4iofirmware/module.mk @@ -3,17 +3,22 @@ SRCS = adc.c \ controls.c \ dsm.c \ - i2c.c \ px4io.c \ registers.c \ safety.c \ sbus.c \ ../systemlib/up_cxxinitialize.c \ - ../systemlib/hx_stream.c \ ../systemlib/perf_counter.c \ mixer.cpp \ ../systemlib/mixer/mixer.cpp \ ../systemlib/mixer/mixer_group.cpp \ ../systemlib/mixer/mixer_multirotor.cpp \ ../systemlib/mixer/mixer_simple.cpp \ -
\ No newline at end of file + +ifeq ($(BOARD),px4io-v1) +SRCS += i2c.c +endif +ifeq ($(BOARD),px4io-v2) +SRCS += serial.c \ + ../systemlib/hx_stream.c +endif diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 88d8cc87c..f5fa0ba75 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -36,7 +36,7 @@ /** * @file protocol.h * - * PX4IO I2C interface protocol. + * PX4IO interface protocol. * * Communication is performed via writes to and reads from 16-bit virtual * registers organised into pages of 255 registers each. @@ -45,7 +45,7 @@ * respectively. Subsequent reads and writes increment the offset within * the page. * - * Most pages are readable or writable but not both. + * Some pages are read- or write-only. * * Note that some pages may permit offset values greater than 255, which * can only be achieved by long writes. The offset does not wrap. @@ -62,12 +62,11 @@ * Note that the implementation of readable pages prefers registers within * readable pages to be densely packed. Page numbers do not need to be * packed. + * + * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, + * [2] denotes definitions specific to the PX4IOv2 board. */ -#define PX4IO_CONTROL_CHANNELS 8 -#define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 4 - /* Per C, this is safe for all 2's complement systems */ #define REG_TO_SIGNED(_reg) ((int16_t)(_reg)) #define SIGNED_TO_REG(_signed) ((uint16_t)(_signed)) @@ -75,10 +74,12 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_PROTOCOL_VERSION 4 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* PX4IO_PROTOCOL_VERSION */ +#define PX4IO_P_CONFIG_HARDWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ @@ -93,7 +94,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -105,19 +106,24 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */ +#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ -#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ +#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ #define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */ -#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */ -#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* [1] servo current limit was exceeded */ +#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* [1] accessory current limit was exceeded */ #define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */ #define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */ #define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */ +#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT (1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */ -#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */ -#define PX4IO_P_STATUS_IBATT 5 /* battery current (raw ADC) */ +#define PX4IO_P_STATUS_VBATT 4 /* [1] battery voltage in mV */ +#define PX4IO_P_STATUS_IBATT 5 /* [1] battery current (raw ADC) */ +#define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ +#define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ +#define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -143,7 +149,7 @@ #define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */ /* setup page */ -#define PX4IO_PAGE_SETUP 100 +#define PX4IO_PAGE_SETUP 50 #define PX4IO_P_SETUP_FEATURES 0 #define PX4IO_P_SETUP_ARMING 1 /* arming controls */ @@ -152,37 +158,40 @@ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ -#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ -/* px4io relay bit definitions */ -#define PX4IO_RELAY1 (1<<0) -#define PX4IO_RELAY2 (1<<1) -#define PX4IO_ACC1 (1<<2) -#define PX4IO_ACC2 (1<<3) +#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ +#define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ +#define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ +#define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ +#define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ -#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ -#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ -enum { /* DSM bind states */ +#define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ dsm_bind_power_down = 0, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart }; + /* 8 */ #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ -#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */ +#define PX4IO_PAGE_CONTROLS 51 /* 0..CONFIG_CONTROL_COUNT */ /* raw text load to the mixer parser - ignores offset */ -#define PX4IO_PAGE_MIXERLOAD 102 +#define PX4IO_PAGE_MIXERLOAD 52 /* R/C channel config */ -#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */ +#define PX4IO_PAGE_RC_CONFIG 53 /* R/C input configuration */ #define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */ #define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */ #define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */ @@ -194,10 +203,23 @@ enum { /* DSM bind states */ #define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */ /* PWM output - overrides mixer */ -#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_DIRECT_PWM 54 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /* PWM failsafe values - zero disables the output */ -#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +#define PX4IO_PAGE_FAILSAFE_PWM 55 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* Debug and test page - not used in normal operation */ +#define PX4IO_PAGE_TEST 127 +#define PX4IO_P_TEST_LED 0 /* set the amber LED on/off */ + +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + +/* PWM idle values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ /** * As-needed mixer data upload. @@ -218,3 +240,81 @@ struct px4io_mixdata { }; #pragma pack(pop) +/** + * Serial protocol encapsulation. + */ + +#define PKT_MAX_REGS 32 // by agreement w/FMU + +#pragma pack(push, 1) +struct IOPacket { + uint8_t count_code; + uint8_t crc; + uint8_t page; + uint8_t offset; + uint16_t regs[PKT_MAX_REGS]; +}; +#pragma pack(pop) + +#define PKT_CODE_READ 0x00 /* FMU->IO read transaction */ +#define PKT_CODE_WRITE 0x40 /* FMU->IO write transaction */ +#define PKT_CODE_SUCCESS 0x00 /* IO->FMU success reply */ +#define PKT_CODE_CORRUPT 0x40 /* IO->FMU bad packet reply */ +#define PKT_CODE_ERROR 0x80 /* IO->FMU register op error reply */ + +#define PKT_CODE_MASK 0xc0 +#define PKT_COUNT_MASK 0x3f + +#define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) +#define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) +#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) + +static const uint8_t crc8_tab[256] __attribute__((unused)) = +{ + 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15, + 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D, + 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, + 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, + 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, + 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, + 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, + 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, + 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, + 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, + 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, + 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, + 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, + 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, + 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, + 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, + 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, + 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, + 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, + 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, + 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, + 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, + 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, + 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, + 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, + 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, + 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, + 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, + 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, + 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, + 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, + 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 +}; + +static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused)); +static uint8_t +crc_packet(struct IOPacket *pkt) +{ + uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]); + uint8_t *p = (uint8_t *)pkt; + uint8_t c = 0; + + while (p < end) + c = crc8_tab[c ^ *(p++)]; + + return c; +} diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index bc8dfc116..e70b3fe88 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -64,11 +64,6 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -#ifdef CONFIG_STM32_I2C1 -/* store i2c reset count XXX this should be a register, together with other error counters */ -volatile uint32_t i2c_loop_resets = 0; -#endif - /* * a set of debug buffers to allow us to send debug information from ISRs */ @@ -147,8 +142,10 @@ user_start(int argc, char *argv[]) LED_BLUE(false); LED_SAFETY(false); - /* turn on servo power */ + /* turn on servo power (if supported) */ +#ifdef POWER_SERVO POWER_SERVO(true); +#endif /* start the safety switch handler */ safety_init(); @@ -159,10 +156,11 @@ user_start(int argc, char *argv[]) /* initialise the control inputs */ controls_init(); -#ifdef CONFIG_STM32_I2C1 - /* start the i2c handler */ - i2c_init(); -#endif + /* start the FMU interface */ + interface_init(); + + /* add a performance counter for the interface */ + perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface"); /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); @@ -205,6 +203,11 @@ user_start(int argc, char *argv[]) /* track the rate at which the loop is running */ perf_count(loop_perf); + /* kick the interface */ + perf_begin(interface_perf); + interface_tick(); + perf_end(interface_perf); + /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); @@ -223,12 +226,11 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets, (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 57cffcc23..dea67043e 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -42,15 +42,16 @@ #include <stdbool.h> #include <stdint.h> -#include <drivers/boards/px4io/px4io_internal.h> +#include <board_config.h> #include "protocol.h" /* * Constants and limits. */ -#define MAX_CONTROL_CHANNELS 12 -#define IO_SERVO_COUNT 8 +#define PX4IO_SERVO_COUNT 8 +#define PX4IO_CONTROL_CHANNELS 8 +#define PX4IO_INPUT_CHANNELS 12 /* * Debug logging @@ -77,6 +78,9 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ /* * Register aliases. @@ -119,32 +123,43 @@ extern struct sys_state_s system_state; /* * GPIO handling. */ -#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) -#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) -#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 + +# define PX4IO_RELAY_CHANNELS 4 +# define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +# define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) +# define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) +# define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +# define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +# define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) +# define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VBATT 4 +# define ADC_IN5 5 -#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) -#ifdef GPIO_ACC1_PWR_EN - #define POWER_ACC1(_s) stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s)) -#endif -#ifdef GPIO_ACC2_PWR_EN - #define POWER_ACC2(_s) stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s)) -#endif -#ifdef GPIO_RELAY1_EN - #define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) #endif -#ifdef GPIO_RELAY2_EN - #define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) + +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + +# define PX4IO_RELAY_CHANNELS 0 +# define POWER_SPEKTRUM(_s) stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s)) + +# define VDD_SERVO_FAULT (!stm32_gpioread(GPIO_SERVO_FAULT_DETECT)) + +# define PX4IO_ADC_CHANNEL_COUNT 2 +# define ADC_VSERVO 4 +# define ADC_RSSI 5 + #endif -#define OVERCURRENT_ACC (!stm32_gpioread(GPIO_ACC_OC_DETECT)) -#define OVERCURRENT_SERVO (!stm32_gpioread(GPIO_SERVO_OC_DETECT)) #define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) -#define ADC_VBATT 4 -#define ADC_IN5 5 -#define ADC_CHANNEL_COUNT 2 - /* * Mixer */ @@ -156,17 +171,16 @@ extern void mixer_handle_text(const void *buffer, size_t length); */ extern void safety_init(void); -#ifdef CONFIG_STM32_I2C1 /** * FMU communications */ -extern void i2c_init(void); -#endif +extern void interface_init(void); +extern void interface_tick(void); /** * Register space */ -extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); +extern int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values); extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values); /** @@ -191,10 +205,5 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/* send a debug message to the console */ +/** send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); - -#ifdef CONFIG_STM32_I2C1 -void i2c_dump(void); -void i2c_reset(void); -#endif diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a922362b6..9c95fd1c5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -44,6 +44,7 @@ #include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_pwm_output.h> #include "px4io.h" #include "protocol.h" @@ -57,14 +58,18 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_PROTOCOL_VERSION, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 2, +#else + [PX4IO_P_CONFIG_HARDWARE_VERSION] = 1, +#endif [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT, - [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS, - [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT, + [PX4IO_P_CONFIG_ACTUATOR_COUNT] = PX4IO_SERVO_COUNT, + [PX4IO_P_CONFIG_RC_INPUT_COUNT] = PX4IO_CONTROL_CHANNELS, + [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = PX4IO_ADC_CHANNEL_COUNT, [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS, }; @@ -79,7 +84,10 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_FLAGS] = 0, [PX4IO_P_STATUS_ALARMS] = 0, [PX4IO_P_STATUS_VBATT] = 0, - [PX4IO_P_STATUS_IBATT] = 0 + [PX4IO_P_STATUS_IBATT] = 0, + [PX4IO_P_STATUS_VSERVO] = 0, + [PX4IO_P_STATUS_VRSSI] = 0, + [PX4IO_P_STATUS_PRSSI] = 0 }; /** @@ -87,14 +95,14 @@ uint16_t r_page_status[] = { * * Post-mixed actuator values. */ -uint16_t r_page_actuators[IO_SERVO_COUNT]; +uint16_t r_page_actuators[PX4IO_SERVO_COUNT]; /** * PAGE 3 * * Servo PWM values */ -uint16_t r_page_servos[IO_SERVO_COUNT]; +uint16_t r_page_servos[PX4IO_SERVO_COUNT]; /** * PAGE 4 @@ -104,7 +112,7 @@ uint16_t r_page_servos[IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, - [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -114,7 +122,7 @@ uint16_t r_page_raw_rc_input[] = */ uint16_t r_page_rc_input[] = { [PX4IO_P_RC_VALID] = 0, - [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0 + [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0 }; /** @@ -138,7 +146,11 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, [PX4IO_P_SETUP_RELAYS] = 0, +#ifdef ADC_VSERVO + [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, +#else [PX4IO_P_SETUP_VBATT_SCALE] = 10000, +#endif [PX4IO_P_SETUP_SET_DEBUG] = 0, }; @@ -146,8 +158,11 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK) -#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) +#define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) /** @@ -166,7 +181,7 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; * * R/C channel input configuration. */ -uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; +uint16_t r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; /* valid options */ #define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) @@ -182,9 +197,33 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE * * Disable pulses as default. */ -uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +uint16_t r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 }; -void +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; + +/** + * PAGE 108 + * + * idle PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +int registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -233,7 +272,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ r_page_servo_failsafe[offset] = *values; @@ -247,6 +286,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_control_min[offset] = 900; + + else if (*values > 1200) + r_page_servo_control_min[offset] = 1200; + else if (*values < 900) + r_page_servo_control_min[offset] = 900; + else + r_page_servo_control_min[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_control_max[offset] = 2100; + + else if (*values > 2100) + r_page_servo_control_max[offset] = 2100; + else if (*values < 1800) + r_page_servo_control_max[offset] = 1800; + else + r_page_servo_control_max[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_IDLE_PWM: + + /* copy channel data */ + while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_idle[offset] = 0; + + else if (*values < 900) + r_page_servo_idle[offset] = 900; + else if (*values > 2100) + r_page_servo_idle[offset] = 2100; + else + r_page_servo_idle[offset] = *values; + + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -260,11 +368,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* iterate individual registers, set each in turn */ while (num_values--) { if (registers_set_one(page, offset, *values)) - break; + return -1; offset++; values++; } + break; } + return 0; } static int @@ -317,8 +427,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } + + if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } r_setup_arming = value; @@ -349,10 +465,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; - POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0); - POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0); - POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0); - POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0); +#ifdef POWER_RELAY1 + POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); +#endif +#ifdef POWER_RELAY2 + POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); +#endif +#ifdef POWER_ACC1 + POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); +#endif +#ifdef POWER_ACC2 + POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); +#endif + break; + + case PX4IO_P_SETUP_VBATT_SCALE: + r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; break; case PX4IO_P_SETUP_SET_DEBUG: @@ -371,9 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /** + * do not allow a RC config change while outputs armed + */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) || + (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) || + (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { break; } @@ -381,7 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; - if (channel >= MAX_CONTROL_CHANNELS) + if (channel >= PX4IO_CONTROL_CHANNELS) return -1; /* disable the channel until we have a chance to sanity-check it */ @@ -401,6 +532,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; @@ -426,7 +560,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { count++; } - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) { count++; } @@ -446,6 +580,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* case PX4IO_RC_PAGE_CONFIG */ } + case PX4IO_PAGE_TEST: + switch (offset) { + case PX4IO_P_TEST_LED: + LED_AMBER(value & 1); + break; + } + break; + default: return -1; } @@ -482,6 +624,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val /* PX4IO_P_STATUS_ALARMS maintained externally */ +#ifdef ADC_VBATT /* PX4IO_P_STATUS_VBATT */ { /* @@ -515,7 +658,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val r_page_status[PX4IO_P_STATUS_VBATT] = corrected; } } - +#endif +#ifdef ADC_IBATT /* PX4IO_P_STATUS_IBATT */ { /* @@ -525,26 +669,62 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val FMU sort it out, with user selectable configuration for their sensor */ - unsigned counts = adc_measure(ADC_IN5); + unsigned counts = adc_measure(ADC_IBATT); if (counts != 0xffff) { r_page_status[PX4IO_P_STATUS_IBATT] = counts; } } +#endif +#ifdef ADC_VSERVO + /* PX4IO_P_STATUS_VSERVO */ + { + /* + * Coefficients here derived by measurement of the 5-16V + * range on one unit: + * + * XXX pending measurements + * + * slope = xxx + * intercept = xxx + * + * Intercept corrected for best results @ 5.0V. + */ + unsigned counts = adc_measure(ADC_VSERVO); + if (counts != 0xffff) { + unsigned mV = (4150 + (counts * 46)) / 10 - 200; + unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; + + r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + } + } +#endif + /* XXX PX4IO_P_STATUS_VRSSI */ + /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); break; case PX4IO_PAGE_RAW_ADC_INPUT: memset(r_page_scratch, 0, sizeof(r_page_scratch)); +#ifdef ADC_VBATT r_page_scratch[0] = adc_measure(ADC_VBATT); - r_page_scratch[1] = adc_measure(ADC_IN5); - +#endif +#ifdef ADC_IBATT + r_page_scratch[1] = adc_measure(ADC_IBATT); +#endif + +#ifdef ADC_VSERVO + r_page_scratch[0] = adc_measure(ADC_VSERVO); +#endif +#ifdef ADC_RSSI + r_page_scratch[1] = adc_measure(ADC_RSSI); +#endif SELECT_PAGE(r_page_scratch); break; case PX4IO_PAGE_PWM_INFO: memset(r_page_scratch, 0, sizeof(r_page_scratch)); - for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i); SELECT_PAGE(r_page_scratch); @@ -587,6 +767,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; + case PX4IO_PAGE_IDLE_PWM: + SELECT_PAGE(r_page_servo_idle); + break; default: return -1; @@ -616,7 +805,7 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate) { for (unsigned pass = 0; pass < 2; pass++) { - for (unsigned group = 0; group < IO_SERVO_COUNT; group++) { + for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) { /* get the channel mask for this rate group */ uint32_t mask = up_pwm_servo_get_rate_group(group); diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc274..95335f038 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +118,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +140,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c new file mode 100644 index 000000000..94d7407df --- /dev/null +++ b/src/modules/px4iofirmware/serial.c @@ -0,0 +1,352 @@ +/**************************************************************************** + * + * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file serial.c + * + * Serial communication for the PX4IO module. + */ + +#include <stdint.h> +#include <unistd.h> +#include <termios.h> +#include <fcntl.h> +#include <string.h> + +#include <nuttx/arch.h> +#include <arch/board/board.h> + +/* XXX might be able to prune these */ +#include <chip.h> +#include <up_internal.h> +#include <up_arch.h> +#include <stm32.h> +#include <systemlib/perf_counter.h> + +//#define DEBUG +#include "px4io.h" + +static perf_counter_t pc_txns; +static perf_counter_t pc_errors; +static perf_counter_t pc_ore; +static perf_counter_t pc_fe; +static perf_counter_t pc_ne; +static perf_counter_t pc_idle; +static perf_counter_t pc_badidle; +static perf_counter_t pc_regerr; +static perf_counter_t pc_crcerr; + +static void rx_handle_packet(void); +static void rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg); +static DMA_HANDLE tx_dma; +static DMA_HANDLE rx_dma; + +static int serial_interrupt(int irq, void *context); +static void dma_reset(void); + +/* if we spend this many ticks idle, reset the DMA */ +static unsigned idle_ticks; + +static struct IOPacket dma_packet; + +/* serial register accessors */ +#define REG(_x) (*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x)) +#define rSR REG(STM32_USART_SR_OFFSET) +#define rDR REG(STM32_USART_DR_OFFSET) +#define rBRR REG(STM32_USART_BRR_OFFSET) +#define rCR1 REG(STM32_USART_CR1_OFFSET) +#define rCR2 REG(STM32_USART_CR2_OFFSET) +#define rCR3 REG(STM32_USART_CR3_OFFSET) +#define rGTPR REG(STM32_USART_GTPR_OFFSET) + +void +interface_init(void) +{ + pc_txns = perf_alloc(PC_ELAPSED, "txns"); + pc_errors = perf_alloc(PC_COUNT, "errors"); + pc_ore = perf_alloc(PC_COUNT, "overrun"); + pc_fe = perf_alloc(PC_COUNT, "framing"); + pc_ne = perf_alloc(PC_COUNT, "noise"); + pc_idle = perf_alloc(PC_COUNT, "idle"); + pc_badidle = perf_alloc(PC_COUNT, "badidle"); + pc_regerr = perf_alloc(PC_COUNT, "regerr"); + pc_crcerr = perf_alloc(PC_COUNT, "crcerr"); + + /* allocate DMA */ + tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA); + rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA); + + /* configure pins for serial use */ + stm32_configgpio(PX4FMU_SERIAL_TX_GPIO); + stm32_configgpio(PX4FMU_SERIAL_RX_GPIO); + + /* reset and configure the UART */ + rCR1 = 0; + rCR2 = 0; + rCR3 = 0; + + /* clear status/errors */ + (void)rSR; + (void)rDR; + + /* configure line speed */ + uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2); + uint32_t mantissa = usartdiv32 >> 5; + uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1; + rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT); + + /* connect our interrupt */ + irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt); + up_enable_irq(PX4FMU_SERIAL_VECTOR); + + /* enable UART and error/idle interrupts */ + rCR3 = USART_CR3_EIE; + rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE; + +#if 0 /* keep this for signal integrity testing */ + for (;;) { + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xfa; + while (!(rSR & USART_SR_TXE)) + ; + rDR = 0xa0; + } +#endif + + /* configure RX DMA and return to listening state */ + dma_reset(); + + debug("serial init"); +} + +void +interface_tick() +{ + /* XXX look for stuck/damaged DMA and reset? */ + if (idle_ticks++ > 100) { + dma_reset(); + idle_ticks = 0; + } +} + +static void +rx_handle_packet(void) +{ + /* check packet CRC */ + uint8_t crc = dma_packet.crc; + dma_packet.crc = 0; + if (crc != crc_packet(&dma_packet)) { + perf_count(pc_crcerr); + + /* send a CRC error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xff; + + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) { + + /* it's a blind write - pass it on */ + if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + dma_packet.count_code = PKT_CODE_SUCCESS; + } + return; + } + + if (PKT_CODE(dma_packet) == PKT_CODE_READ) { + + /* it's a read - get register pointer for reply */ + unsigned count; + uint16_t *registers; + + if (registers_get(dma_packet.page, dma_packet.offset, ®isters, &count) < 0) { + perf_count(pc_regerr); + dma_packet.count_code = PKT_CODE_ERROR; + } else { + /* constrain reply to requested size */ + if (count > PKT_MAX_REGS) + count = PKT_MAX_REGS; + if (count > PKT_COUNT(dma_packet)) + count = PKT_COUNT(dma_packet); + + /* copy reply registers into DMA buffer */ + memcpy((void *)&dma_packet.regs[0], registers, count * 2); + dma_packet.count_code = count | PKT_CODE_SUCCESS; + } + return; + } + + /* send a bad-packet error reply */ + dma_packet.count_code = PKT_CODE_CORRUPT; + dma_packet.page = 0xff; + dma_packet.offset = 0xfe; +} + +static void +rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg) +{ + /* + * We are here because DMA completed, or UART reception stopped and + * we think we have a packet in the buffer. + */ + perf_begin(pc_txns); + + /* disable UART DMA */ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + + /* reset the idle counter */ + idle_ticks = 0; + + /* handle the received packet */ + rx_handle_packet(); + + /* re-set DMA for reception first, so we are ready to receive before we start sending */ + dma_reset(); + + /* send the reply to the just-processed request */ + dma_packet.crc = 0; + dma_packet.crc = crc_packet(&dma_packet); + stm32_dmasetup( + tx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + PKT_SIZE(dma_packet), + DMA_CCR_DIR | + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + stm32_dmastart(tx_dma, NULL, NULL, false); + rCR3 |= USART_CR3_DMAT; + + perf_end(pc_txns); +} + +static int +serial_interrupt(int irq, void *context) +{ + static bool abort_on_idle = false; + + uint32_t sr = rSR; /* get UART status register */ + (void)rDR; /* required to clear any of the interrupt status that brought us here */ + + if (sr & (USART_SR_ORE | /* overrun error - packet was too big for DMA or DMA was too slow */ + USART_SR_NE | /* noise error - we have lost a byte due to noise */ + USART_SR_FE)) { /* framing error - start/stop bit lost or line break */ + + perf_count(pc_errors); + if (sr & USART_SR_ORE) + perf_count(pc_ore); + if (sr & USART_SR_NE) + perf_count(pc_ne); + if (sr & USART_SR_FE) + perf_count(pc_fe); + + /* send a line break - this will abort transmission/reception on the other end */ + rCR1 |= USART_CR1_SBK; + + /* when the line goes idle, abort rather than look at the packet */ + abort_on_idle = true; + } + + if (sr & USART_SR_IDLE) { + + /* + * If we saw an error, don't bother looking at the packet - it should have + * been aborted by the sender and will definitely be bad. Get the DMA reconfigured + * ready for their retry. + */ + if (abort_on_idle) { + + abort_on_idle = false; + dma_reset(); + return 0; + } + + /* + * The sender has stopped sending - this is probably the end of a packet. + * Check the received length against the length in the header to see if + * we have something that looks like a packet. + */ + unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma); + if ((length < 1) || (length < PKT_SIZE(dma_packet))) { + + /* it was too short - possibly truncated */ + perf_count(pc_badidle); + return 0; + } + + /* + * Looks like we received a packet. Stop the DMA and go process the + * packet. + */ + perf_count(pc_idle); + stm32_dmastop(rx_dma); + rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL); + } + + return 0; +} + +static void +dma_reset(void) +{ + rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR); + (void)rSR; + (void)rDR; + (void)rDR; + + /* kill any pending DMA */ + stm32_dmastop(tx_dma); + stm32_dmastop(rx_dma); + + /* reset the RX side */ + stm32_dmasetup( + rx_dma, + (uint32_t)&rDR, + (uint32_t)&dma_packet, + sizeof(dma_packet), + DMA_CCR_MINC | + DMA_CCR_PSIZE_8BITS | + DMA_CCR_MSIZE_8BITS); + + /* start receive DMA ready for the next packet */ + stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); + rCR3 |= USART_CR3_DMAR; +} + diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ba7cdd91c..e83fb7dd3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -75,6 +75,7 @@ #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/differential_pressure.h> @@ -94,7 +95,6 @@ log_msgs_written++; \ } else { \ log_msgs_skipped++; \ - /*printf("skip\n");*/ \ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ @@ -102,9 +102,6 @@ fds[fdsc_count].events = POLLIN; \ fdsc_count++; - -//#define SDLOG2_DEBUG - static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -233,7 +230,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("sdlog2 already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -250,7 +247,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (!thread_running) { - printf("\tsdlog2 is not started\n"); + warnx("not started"); } main_thread_should_exit = true; @@ -262,7 +259,7 @@ int sdlog2_main(int argc, char *argv[]) sdlog2_status(); } else { - printf("\tsdlog2 not started\n"); + warnx("not started\n"); } exit(0); @@ -387,11 +384,6 @@ static void *logwriter_thread(void *arg) /* only get pointer to thread-safe data, do heavy I/O a few lines down */ int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); -#ifdef SDLOG2_DEBUG - int rp = logbuf->read_ptr; - int wp = logbuf->write_ptr; -#endif - /* continue */ pthread_mutex_unlock(&logbuffer_mutex); @@ -407,9 +399,6 @@ static void *logwriter_thread(void *arg) n = write(log_file, read_ptr, n); should_wait = (n == available) && !is_part; -#ifdef SDLOG2_DEBUG - printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); -#endif if (n < 0) { main_thread_should_exit = true; @@ -422,14 +411,8 @@ static void *logwriter_thread(void *arg) } else { n = 0; -#ifdef SDLOG2_DEBUG - printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); -#endif /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { -#ifdef SDLOG2_DEBUG - printf("break logwriter thread\n"); -#endif break; } should_wait = true; @@ -444,10 +427,6 @@ static void *logwriter_thread(void *arg) fsync(log_file); close(log_file); -#ifdef SDLOG2_DEBUG - printf("logwriter thread exit\n"); -#endif - return OK; } @@ -604,15 +583,6 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "unable to create logging folder, exiting."); } - const char *converter_in = "/etc/logging/conv.zip"; - char* converter_out = malloc(150); - sprintf(converter_out, "%s/conv.zip", folder_path); - - if (file_copy(converter_in, converter_out)) { - errx(1, "unable to copy conversion scripts, exiting."); - } - free(converter_out); - /* only print logging path, important to find log file later */ warnx("logging to directory: %s", folder_path); @@ -623,14 +593,6 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 19; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); @@ -655,6 +617,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; + struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; memset(&buf, 0, sizeof(buf)); @@ -678,6 +641,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rc_sub; int airspeed_sub; int esc_sub; + int global_vel_sp_sub; } subs; /* log message buffer: header + body */ @@ -703,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPOS_s log_GPOS; struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; + struct log_GVSP_s log_GVSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -710,6 +675,14 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 20; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; @@ -824,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL VELOCITY SETPOINT --- */ + subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + fds[fdsc_count].fd = subs.global_vel_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -911,15 +890,14 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; - log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; - log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; - log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; - log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; - log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; + log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; + log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; - log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; + log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; + log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; LOGBUFFER_WRITE_AND_COUNT(STAT); } @@ -1065,10 +1043,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; - log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; - log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; - log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; LOGBUFFER_WRITE_AND_COUNT(LPOS); } @@ -1175,14 +1152,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } } -#ifdef SDLOG2_DEBUG - printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { -#ifdef SDLOG2_DEBUG - printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } @@ -1202,6 +1183,8 @@ int sdlog2_thread_main(int argc, char *argv[]) pthread_mutex_destroy(&logbuffer_mutex); pthread_cond_destroy(&logbuffer_cond); + free(lb.data); + warnx("exiting."); thread_running = false; @@ -1265,7 +1248,7 @@ int file_copy(const char *file_old, const char *file_new) fclose(source); fclose(target); - return OK; + return ret; } void handle_command(struct vehicle_command_s *cmd) @@ -1297,8 +1280,10 @@ void handle_command(struct vehicle_command_s *cmd) void handle_status(struct vehicle_status_s *status) { - if (status->flag_system_armed != flag_system_armed) { - flag_system_armed = status->flag_system_armed; + // TODO use flag from actuator_armed here? + bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR; + if (armed != flag_system_armed) { + flag_system_armed = armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 934e4dec8..4eeb65a87 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -106,10 +106,9 @@ struct log_LPOS_s { float vx; float vy; float vz; - float hdg; - int32_t home_lat; - int32_t home_lon; - float home_alt; + int32_t ref_lat; + int32_t ref_lon; + float ref_alt; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -149,15 +148,14 @@ struct log_ATTC_s { /* --- STAT - VEHICLE STATE --- */ #define LOG_STAT_MSG 10 struct log_STAT_s { - uint8_t state; - uint8_t flight_mode; - uint8_t manual_control_mode; - uint8_t manual_sas_mode; - uint8_t armed; + uint8_t main_state; + uint8_t navigation_state; + uint8_t arming_state; float battery_voltage; float battery_current; float battery_remaining; uint8_t battery_warning; + uint8_t landed; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -244,6 +242,14 @@ struct log_ESC_s { uint16_t esc_setpoint_raw; }; +/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ +#define LOG_GVSP_MSG 19 +struct log_GVSP_s { + float vx; + float vy; + float vz; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -254,11 +260,11 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), - LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), @@ -266,7 +272,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), - LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 6c8e514b6..992abf2cc 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -68,7 +68,11 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); -PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667); +PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); +PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); + +PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); @@ -154,36 +158,42 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ +PARAM_DEFINE_FLOAT(RC15_MIN, 1000); +PARAM_DEFINE_FLOAT(RC15_TRIM, 1500); +PARAM_DEFINE_FLOAT(RC15_MAX, 2000); +PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); +PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); + + PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#else /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +#endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 42268b971..e98c4d548 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -50,6 +50,7 @@ #include <stdio.h> #include <errno.h> #include <math.h> +#include <mathlib/mathlib.h> #include <nuttx/analog/adc.h> @@ -74,7 +75,7 @@ #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/manual_control_setpoint.h> -#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/vehicle_control_mode.h> #include <uORB/topics/parameter_update.h> #include <uORB/topics/battery_status.h> #include <uORB/topics/differential_pressure.h> @@ -138,6 +139,77 @@ #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg) /** + * Enum for board and external compass rotations. + * This enum maps from board attitude to airframe attitude. + */ +enum Rotation { + ROTATION_NONE = 0, + ROTATION_YAW_45 = 1, + ROTATION_YAW_90 = 2, + ROTATION_YAW_135 = 3, + ROTATION_YAW_180 = 4, + ROTATION_YAW_225 = 5, + ROTATION_YAW_270 = 6, + ROTATION_YAW_315 = 7, + ROTATION_ROLL_180 = 8, + ROTATION_ROLL_180_YAW_45 = 9, + ROTATION_ROLL_180_YAW_90 = 10, + ROTATION_ROLL_180_YAW_135 = 11, + ROTATION_PITCH_180 = 12, + ROTATION_ROLL_180_YAW_225 = 13, + ROTATION_ROLL_180_YAW_270 = 14, + ROTATION_ROLL_180_YAW_315 = 15, + ROTATION_ROLL_90 = 16, + ROTATION_ROLL_90_YAW_45 = 17, + ROTATION_ROLL_90_YAW_90 = 18, + ROTATION_ROLL_90_YAW_135 = 19, + ROTATION_ROLL_270 = 20, + ROTATION_ROLL_270_YAW_45 = 21, + ROTATION_ROLL_270_YAW_90 = 22, + ROTATION_ROLL_270_YAW_135 = 23, + ROTATION_PITCH_90 = 24, + ROTATION_PITCH_270 = 25, + ROTATION_MAX +}; + +typedef struct +{ + uint16_t roll; + uint16_t pitch; + uint16_t yaw; +} rot_lookup_t; + +const rot_lookup_t rot_lookup[] = +{ + { 0, 0, 0 }, + { 0, 0, 45 }, + { 0, 0, 90 }, + { 0, 0, 135 }, + { 0, 0, 180 }, + { 0, 0, 225 }, + { 0, 0, 270 }, + { 0, 0, 315 }, + {180, 0, 0 }, + {180, 0, 45 }, + {180, 0, 90 }, + {180, 0, 135 }, + { 0, 180, 0 }, + {180, 0, 225 }, + {180, 0, 270 }, + {180, 0, 315 }, + { 90, 0, 0 }, + { 90, 0, 45 }, + { 90, 0, 90 }, + { 90, 0, 135 }, + {270, 0, 0 }, + {270, 0, 45 }, + {270, 0, 90 }, + {270, 0, 135 }, + { 0, 90, 0 }, + { 0, 270, 0 } +}; + +/** * Sensor app start / stop handling function * * @ingroup apps @@ -189,9 +261,9 @@ private: int _mag_sub; /**< raw mag data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ - int _airspeed_sub; /**< airspeed subscription */ - int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _airspeed_sub; /**< airspeed subscription */ + int _diff_pres_sub; /**< raw differential pressure subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -210,13 +282,16 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + bool _mag_is_external; /**< true if the active mag is on an external board */ + struct { float min[_rc_max_chan_count]; float trim[_rc_max_chan_count]; float max[_rc_max_chan_count]; float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; - // float ex[_rc_max_chan_count]; float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; @@ -226,21 +301,22 @@ private: float accel_offset[3]; float accel_scale[3]; float diff_pres_offset_pa; + float diff_pres_analog_enabled; - int rc_type; + int board_rotation; + int external_mag_rotation; int rc_map_roll; int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; - int rc_map_manual_override_sw; - int rc_map_auto_mode_sw; + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_assisted_sw; + int rc_map_mission_sw; - int rc_map_manual_mode_sw; - int rc_map_sas_mode_sw; - int rc_map_rtl_sw; - int rc_map_offboard_ctrl_mode_sw; +// int rc_map_offboard_ctrl_mode_sw; int rc_map_flaps; @@ -265,10 +341,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - // param_t ex[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -277,19 +349,19 @@ private: param_t mag_offset[3]; param_t mag_scale[3]; param_t diff_pres_offset_pa; + param_t diff_pres_analog_enabled; param_t rc_map_roll; param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_manual_override_sw; - param_t rc_map_auto_mode_sw; + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_assisted_sw; + param_t rc_map_mission_sw; - param_t rc_map_manual_mode_sw; - param_t rc_map_sas_mode_sw; - param_t rc_map_rtl_sw; - param_t rc_map_offboard_ctrl_mode_sw; +// param_t rc_map_offboard_ctrl_mode_sw; param_t rc_map_flaps; @@ -306,6 +378,9 @@ private: param_t battery_voltage_scaling; + param_t board_rotation; + param_t external_mag_rotation; + } _parameter_handles; /**< handles for interesting parameters */ @@ -315,6 +390,11 @@ private: int parameters_update(); /** + * Get the rotation matrices + */ + void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); + + /** * Do accel-related initialisation. */ void accel_init(); @@ -380,9 +460,9 @@ private: void diff_pres_poll(struct sensor_combined_s &raw); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in parameters. @@ -437,7 +517,7 @@ Sensors::Sensors() : _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), - _vstatus_sub(-1), + _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), @@ -450,7 +530,11 @@ Sensors::Sensors() : _diff_pres_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")) + _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), + + _board_rotation(3,3), + _external_mag_rotation(3,3), + _mag_is_external(false) { /* basic r/c parameters */ @@ -479,8 +563,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -488,16 +570,16 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); - _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); - _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); - _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); - _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + +// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -537,9 +619,14 @@ Sensors::Sensors() : /* Differential pressure offset */ _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); + _parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA"); _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* rotations */ + _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + /* fetch initial parameter values */ parameters_update(); } @@ -573,7 +660,9 @@ int Sensors::parameters_update() { bool rc_valid = true; - + float tmpScaleFactor = 0.0f; + float tmpRevFactor = 0.0f; + /* rc values */ for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) { @@ -583,29 +672,27 @@ Sensors::parameters_update() param_get(_parameter_handles.rev[i], &(_parameters.rev[i])); param_get(_parameter_handles.dz[i], &(_parameters.dz[i])); - _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); - + tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + tmpRevFactor = tmpScaleFactor * _parameters.rev[i]; + /* handle blowup in the scaling factor calculation */ - if (!isfinite(_parameters.scaling_factor[i]) || - _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f || - _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) { - + if (!isfinite(tmpScaleFactor) || + (tmpRevFactor < 0.000001f) || + (tmpRevFactor > 0.2f) ) { + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ - _parameters.scaling_factor[i] = 0; + _parameters.scaling_factor[i] = 0.0f; rc_valid = false; } - + else { + _parameters.scaling_factor[i] = tmpScaleFactor; + } } /* handle wrong values */ if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -623,54 +710,35 @@ Sensors::parameters_update() warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { - warnx("Failed getting override sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { - warnx("Failed getting auto mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { - warnx("Failed getting flaps chan index"); - } - - if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { - warnx("Failed getting manual mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { - warnx("Failed getting rtl sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { - warnx("Failed getting sas mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { - warnx("Failed getting offboard control mode sw chan index"); + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("Failed getting mode sw chan index"); } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("Failed getting return sw chan index"); } - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); + if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + warnx("Failed getting assisted sw chan index"); } - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + warnx("Failed getting mission sw chan index"); } - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { + warnx("Failed getting flaps chan index"); } - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } +// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { +// warnx("Failed getting offboard control mode sw chan index"); +// } + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); @@ -682,15 +750,14 @@ Sensors::parameters_update() _rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1; - _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; - _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; + _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; - _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; - _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; - _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; +// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; @@ -725,16 +792,41 @@ Sensors::parameters_update() /* Airspeed offset */ param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); + param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled)); /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("Failed updating voltage scaling param"); } + param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); + get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + return OK; } void +Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix) +{ + /* first set to zero */ + rot_matrix->Matrix::zero(3,3); + + float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; + float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; + float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw; + + math::EulerAngles euler(roll, pitch, yaw); + + math::Dcm R(euler); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + (*rot_matrix)(i,j) = R(i, j); +} + +void Sensors::accel_init() { int fd; @@ -757,7 +849,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); - #else + #elif CONFIG_ARCH_BOARD_PX4FMU_V2 /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -765,6 +857,9 @@ Sensors::accel_init() /* set the driver to poll at 800Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 800); + #else + #error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 + #endif warnx("using system accel"); @@ -799,11 +894,11 @@ Sensors::gyro_init() #else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); #endif @@ -816,6 +911,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -824,11 +920,33 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); + + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } + + + + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) + errx(1, "FATAL: unknown if magnetometer is external or onboard"); + else if (ret == 1) + _mag_is_external = true; + else + _mag_is_external = false; close(fd); } @@ -842,7 +960,7 @@ Sensors::baro_init() if (fd < 0) { warn("%s", BARO_DEVICE_PATH); - warnx("No barometer found, ignoring"); + errx(1, "FATAL: No barometer found"); } /* set the driver to poll at 150Hz */ @@ -874,9 +992,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - raw.accelerometer_m_s2[0] = accel_report.x; - raw.accelerometer_m_s2[1] = accel_report.y; - raw.accelerometer_m_s2[2] = accel_report.z; + math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + vect = _board_rotation*vect; + + raw.accelerometer_m_s2[0] = vect(0); + raw.accelerometer_m_s2[1] = vect(1); + raw.accelerometer_m_s2[2] = vect(2); raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; @@ -897,9 +1018,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; - raw.gyro_rad_s[1] = gyro_report.y; - raw.gyro_rad_s[2] = gyro_report.z; + math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + vect = _board_rotation*vect; + + raw.gyro_rad_s[0] = vect(0); + raw.gyro_rad_s[1] = vect(1); + raw.gyro_rad_s[2] = vect(2); raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; @@ -920,9 +1044,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + + if (_mag_is_external) + vect = _external_mag_rotation*vect; + else + vect = _board_rotation*vect; + + raw.magnetometer_ga[0] = vect(0); + raw.magnetometer_ga[1] = vect(1); + raw.magnetometer_ga[2] = vect(2); raw.magnetometer_raw[0] = mag_report.x_raw; raw.magnetometer_raw[1] = mag_report.y_raw; @@ -977,21 +1108,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) } void -Sensors::vehicle_status_poll() +Sensors::vehicle_control_mode_poll() { - struct vehicle_status_s vstatus; - bool vstatus_updated; + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - if (vstatus_updated) { + if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !_hil_enabled) { + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; @@ -1144,9 +1275,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /** * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. */ - if (voltage > 0.4f) { + if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) { float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor @@ -1194,10 +1326,11 @@ Sensors::ppm_poll() manual_control.yaw = NAN; manual_control.throttle = NAN; - manual_control.manual_mode_switch = NAN; - manual_control.manual_sas_switch = NAN; - manual_control.return_to_launch_switch = NAN; - manual_control.auto_offboard_input_switch = NAN; + manual_control.mode_switch = NAN; + manual_control.return_switch = NAN; + manual_control.assisted_switch = NAN; + manual_control.mission_switch = NAN; +// manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1297,11 +1430,17 @@ Sensors::ppm_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* override switch input */ - manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); - /* mode switch input */ - manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + + /* land switch input */ + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + + /* assisted switch input */ + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + + /* mission switch input */ + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1313,21 +1452,17 @@ Sensors::ppm_poll() } } - if (_rc.function[MANUAL_MODE] >= 0) { - manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); - } - - if (_rc.function[SAS_MODE] >= 0) { - manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } - if (_rc.function[RTL] >= 0) { - manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } - if (_rc.function[OFFBOARD_MODE] >= 0) { - manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - } +// if (_rc.function[OFFBOARD_MODE] >= 0) { +// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); +// } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { @@ -1400,12 +1535,12 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_vcontrol_mode_sub, 200); /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ orb_set_interval(_gyro_sub, 4); @@ -1461,7 +1596,7 @@ Sensors::task_main() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index e01cc4dda..310fbf60f 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -42,7 +42,7 @@ #include <stdio.h> #include <math.h> -#include "conversions.h" +#include <geo/geo.h> #include "airspeed.h" @@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { - density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; -// printf("[airspeed] Invalid air density, using density at sea level\n"); + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; } float pressure_difference = total_pressure - static_pressure; - if(pressure_difference > 0) { + if (pressure_difference > 0) { return sqrtf((2.0f*(pressure_difference)) / density); - } else - { + } else { return -sqrtf((2.0f*fabsf(pressure_difference)) / density); } } + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); +} diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index def53f0c1..8dccaab9c 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -85,6 +85,14 @@ */ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + /** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + __END_DECLS #endif diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c index ac94252c5..9105d83cb 100644 --- a/src/modules/systemlib/conversions.c +++ b/src/modules/systemlib/conversions.c @@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } - -void rot2quat(const float R[9], float Q[4]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - int32_t idx; - - /* conversion of rotation matrix to quaternion - * choose the largest component to begin with */ - q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; - q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; - q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; - q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; - - idx = 0; - - if (q0_2 < q1_2) { - q0_2 = q1_2; - - idx = 1; - } - - if (q0_2 < q2_2) { - q0_2 = q2_2; - idx = 2; - } - - if (q0_2 < q3_2) { - q0_2 = q3_2; - idx = 3; - } - - q0_2 = sqrtf(q0_2); - - /* solve for the remaining three components */ - if (idx == 0) { - q1_2 = q0_2; - q2_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[6] - R[2]) / 4.0F / q0_2; - q0_2 = (R[1] - R[3]) / 4.0F / q0_2; - - } else if (idx == 1) { - q2_2 = q0_2; - q1_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[6] + R[2]) / 4.0F / q0_2; - - } else if (idx == 2) { - q3_2 = q0_2; - q1_2 = (R[6] - R[2]) / 4.0F / q0_2; - q2_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[7] + R[5]) / 4.0F / q0_2; - - } else { - q1_2 = (R[1] - R[3]) / 4.0F / q0_2; - q2_2 = (R[6] + R[2]) / 4.0F / q0_2; - q3_2 = (R[7] + R[5]) / 4.0F / q0_2; - } - - /* return values */ - Q[0] = q1_2; - Q[1] = q2_2; - Q[2] = q3_2; - Q[3] = q0_2; -} - -void quat2rot(const float Q[4], float R[9]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - - memset(&R[0], 0, 9U * sizeof(float)); - - q0_2 = Q[0] * Q[0]; - q1_2 = Q[1] * Q[1]; - q2_2 = Q[2] * Q[2]; - q3_2 = Q[3] * Q[3]; - - R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; - R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); - R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); - R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); - R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; - R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); - R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); - R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); - R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; -} - -float get_air_density(float static_pressure, float temperature_celsius) -{ - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h index 064426f21..dc383e770 100644 --- a/src/modules/systemlib/conversions.h +++ b/src/modules/systemlib/conversions.h @@ -43,7 +43,6 @@ #define CONVERSIONS_H_ #include <float.h> #include <stdint.h> -#include <systemlib/geo/geo.h> __BEGIN_DECLS @@ -57,34 +56,6 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); -/** - * Converts a 3 x 3 rotation matrix to an unit quaternion. - * - * All orientations are expressed in NED frame. - * - * @param R rotation matrix to convert - * @param Q quaternion to write back to - */ -__EXPORT void rot2quat(const float R[9], float Q[4]); - -/** - * Converts an unit quaternion to a 3 x 3 rotation matrix. - * - * All orientations are expressed in NED frame. - * - * @param Q quaternion to convert - * @param R rotation matrix to write back to - */ -__EXPORT void quat2rot(const float Q[4], float R[9]); - -/** - * Calculates air density. - * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius - */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); - __END_DECLS #endif /* CONVERSIONS_H_ */ diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c deleted file mode 100644 index 6463e6489..000000000 --- a/src/modules/systemlib/geo/geo.c +++ /dev/null @@ -1,438 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file geo.c - * - * Geo / math functions to perform geodesic calculations - * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - */ - -#include <systemlib/geo/geo.h> -#include <nuttx/config.h> -#include <unistd.h> -#include <pthread.h> -#include <stdio.h> -#include <math.h> -#include <stdbool.h> - - -/* values for map projection */ -static double phi_1; -static double sin_phi_1; -static double cos_phi_1; -static double lambda_0; -static double scale; - -__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567 -{ - /* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - phi_1 = lat_0 / 180.0 * M_PI; - lambda_0 = lon_0 / 180.0 * M_PI; - - sin_phi_1 = sin(phi_1); - cos_phi_1 = cos(phi_1); - - /* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale - - /* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */ - const double r_earth = 6371000; - - double lat1 = phi_1; - double lon1 = lambda_0; - - double lat2 = phi_1 + 0.5 / 180 * M_PI; - double lon2 = lambda_0 + 0.5 / 180 * M_PI; - double sin_lat_2 = sin(lat2); - double cos_lat_2 = cos(lat2); - double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth; - - /* 2) calculate distance rho on plane */ - double k_bar = 0; - double c = acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)); - - if (0 != c) - k_bar = c / sin(c); - - double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane - double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0))); - double rho = sqrt(pow(x2, 2) + pow(y2, 2)); - - scale = d / rho; - -} - -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - double phi = lat / 180.0 * M_PI; - double lambda = lon / 180.0 * M_PI; - - double sin_phi = sin(phi); - double cos_phi = cos(phi); - - double k_bar = 0; - /* using small angle approximation (formula in comment is without aproximation) */ - double c = acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c = acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) ); - - if (0 != c) - k_bar = c / sin(c); - - /* using small angle approximation (formula in comment is without aproximation) */ - *y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale; - *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; // *x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale; - -// printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0); -} - -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon) -{ - /* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */ - - double x_descaled = x / scale; - double y_descaled = y / scale; - - double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2)); - double sin_c = sin(c); - double cos_c = cos(c); - - double lat_sphere = 0; - - if (c != 0) - lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c); - else - lat_sphere = asin(cos_c * sin_phi_1); - -// printf("lat_sphere = %.10f\n",lat_sphere); - - double lon_sphere = 0; - - if (phi_1 == M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled)); - - } else if (phi_1 == -M_PI / 2) { - //using small angle approximation (formula in comment is without aproximation) - lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled)); - - } else { - - lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c)); - //using small angle approximation -// double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c); -// if(denominator != 0) -// { -// lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator); -// } -// else -// { -// ... -// } - } - -// printf("lon_sphere = %.10f\n",lon_sphere); - - *lat = lat_sphere * 180.0 / M_PI; - *lon = lon_sphere * 180.0 / M_PI; - -} - - -__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now / 180.0d * M_PI; - double lon_now_rad = lon_now / 180.0d * M_PI; - double lat_next_rad = lat_next / 180.0d * M_PI; - double lon_next_rad = lon_next / 180.0d * M_PI; - - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); - double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); - - const double radius_earth = 6371000.0d; - return radius_earth * c; -} - -__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) -{ - double lat_now_rad = lat_now * M_DEG_TO_RAD; - double lon_now_rad = lon_now * M_DEG_TO_RAD; - double lat_next_rad = lat_next * M_DEG_TO_RAD; - double lon_next_rad = lon_next * M_DEG_TO_RAD; - - double d_lat = lat_next_rad - lat_now_rad; - double d_lon = lon_next_rad - lon_now_rad; - - /* conscious mix of double and float trig function to maximize speed and efficiency */ - float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - - theta = _wrap_pi(theta); - - return theta; -} - -// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> - -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) -{ -// This function returns the distance to the nearest point on the track line. Distance is positive if current -// position is right of the track and negative if left of the track as seen from a point on the track line -// headed towards the end point. - - float dist_to_end; - float bearing_end; - float bearing_track; - float bearing_diff; - - int return_value = ERROR; // Set error flag, cleared when valid result calculated. - crosstrack_error->past_end = false; - crosstrack_error->distance = 0.0f; - crosstrack_error->bearing = 0.0f; - - // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value; - - bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); - bearing_diff = bearing_track - bearing_end; - bearing_diff = _wrap_pi(bearing_diff); - - // Return past_end = true if past end point of line - if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { - crosstrack_error->past_end = true; - return_value = OK; - return return_value; - } - - dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); - - if (sin(bearing_diff) >= 0) { - crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); - - } else { - crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F); - } - - return_value = OK; - - return return_value; - -} - - -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep) -{ - // This function returns the distance to the nearest point on the track arc. Distance is positive if current - // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and - // headed towards the end point. - - // Determine if the current position is inside or outside the sector between the line from the center - // to the arc start and the line from the center to the arc end - float bearing_sector_start; - float bearing_sector_end; - float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); - bool in_sector; - - int return_value = ERROR; // Set error flag, cleared when valid result calculated. - crosstrack_error->past_end = false; - crosstrack_error->distance = 0.0f; - crosstrack_error->bearing = 0.0f; - - // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value; - - - if (arc_sweep >= 0) { - bearing_sector_start = arc_start_bearing; - bearing_sector_end = arc_start_bearing + arc_sweep; - - if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F; - - } else { - bearing_sector_end = arc_start_bearing; - bearing_sector_start = arc_start_bearing - arc_sweep; - - if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F; - } - - in_sector = false; - - // Case where sector does not span zero - if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; - - // Case where sector does span zero - if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true; - - // If in the sector then calculate distance and bearing to closest point - if (in_sector) { - crosstrack_error->past_end = false; - float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); - - if (dist_to_center <= radius) { - crosstrack_error->distance = radius - dist_to_center; - crosstrack_error->bearing = bearing_now + M_PI_F; - - } else { - crosstrack_error->distance = dist_to_center - radius; - crosstrack_error->bearing = bearing_now; - } - - // If out of the sector then calculate dist and bearing to start or end point - - } else { - - // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) - // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to - // calculate the position of the start and end points. We should not be doing this often - // as this function generally will not be called repeatedly when we are out of the sector. - - // TO DO - this is messed up and won't compile - float start_disp_x = radius * sin(arc_start_bearing); - float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0d; - float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; - float lon_end = lon_now + end_disp_x / 111111.0d; - float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; - float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - - - if (dist_to_start < dist_to_end) { - crosstrack_error->distance = dist_to_start; - crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - - } else { - crosstrack_error->past_end = true; - crosstrack_error->distance = dist_to_end; - crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - } - - } - - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); - return_value = OK; - return return_value; -} - -__EXPORT float _wrap_pi(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing) || bearing == 0) { - return bearing; - } - - int c = 0; - - while (bearing > M_PI_F && c < 30) { - bearing -= M_TWOPI_F; - c++; - } - - c = 0; - - while (bearing <= -M_PI_F && c < 30) { - bearing += M_TWOPI_F; - c++; - } - - return bearing; -} - -__EXPORT float _wrap_2pi(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing)) { - return bearing; - } - - while (bearing >= M_TWOPI_F) { - bearing = bearing - M_TWOPI_F; - } - - while (bearing < 0.0f) { - bearing = bearing + M_TWOPI_F; - } - - return bearing; -} - -__EXPORT float _wrap_180(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing)) { - return bearing; - } - - while (bearing > 180.0f) { - bearing = bearing - 360.0f; - } - - while (bearing <= -180.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - -__EXPORT float _wrap_360(float bearing) -{ - /* value is inf or NaN */ - if (!isfinite(bearing)) { - return bearing; - } - - while (bearing >= 360.0f) { - bearing = bearing - 360.0f; - } - - while (bearing < 0.0f) { - bearing = bearing + 360.0f; - } - - return bearing; -} - - diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h deleted file mode 100644 index dadec51ec..000000000 --- a/src/modules/systemlib/geo/geo.h +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. Neither the name PX4 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file geo.h - * - * Definition of geo / math functions to perform geodesic calculations - * - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> - * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> - */ - -#pragma once - -__BEGIN_DECLS - -#include <stdbool.h> - -#define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f /* kg/m^3 */ -#define CONSTANTS_AIR_GAS_CONST 287.1f /* J/(kg * K) */ -#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f /* °C */ -#define CONSTANTS_RADIUS_OF_EARTH 6371000 /* meters (m) */ - -/* compatibility aliases */ -#define RADIUS_OF_EARTH CONSTANTS_RADIUS_OF_EARTH -#define GRAVITY_MSS CONSTANTS_ONE_G - -// XXX remove -struct crosstrack_error_s { - bool past_end; // Flag indicating we are past the end of the line/arc segment - float distance; // Distance in meters to closest point on line/arc - float bearing; // Bearing in radians to closest point on line/arc -} ; - -/** - * Initializes the map transformation. - * - * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT void map_projection_init(double lat_0, double lon_0); - -/** - * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT void map_projection_project(double lat, double lon, float *x, float *y); - -/** - * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system - * - * @param x north - * @param y east - * @param lat in degrees (47.1234567°, not 471234567°) - * @param lon in degrees (8.1234567°, not 81234567°) - */ -__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); - -/** - * Returns the distance to the next waypoint in meters. - * - * @param lat_now current position in degrees (47.1234567°, not 471234567°) - * @param lon_now current position in degrees (8.1234567°, not 81234567°) - * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) - * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) - */ -__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); - -/** - * Returns the bearing to the next waypoint in radians. - * - * @param lat_now current position in degrees (47.1234567°, not 471234567°) - * @param lon_now current position in degrees (8.1234567°, not 81234567°) - * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) - * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) - */ -__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); - -__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); - -__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center, - float radius, float arc_start_bearing, float arc_sweep); - -__EXPORT float _wrap_180(float bearing); -__EXPORT float _wrap_360(float bearing); -__EXPORT float _wrap_pi(float bearing); -__EXPORT float _wrap_2pi(float bearing); - -__END_DECLS diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 192195856..03ca71375 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -46,28 +46,33 @@ #include <mavlink/mavlink_log.h> -static FILE* text_recorder_fd = NULL; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); - text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb) +{ + lb->size = 0; + lb->start = 0; + lb->count = 0; + free(lb->elems); +} + +__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; } -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { return lb->count == 0; } -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { int end = (lb->start + lb->count) % lb->size; memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); @@ -80,19 +85,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ } } -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { if (!mavlink_logbuffer_is_empty(lb)) { memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; - if (text_recorder_fd) { - fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); - fputc("\n", text_recorder_fd); - fsync(text_recorder_fd); - } - return 0; } else { @@ -100,7 +99,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa } } -void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) { va_list ap; va_start(ap, fmt); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index b470c1227..843cda722 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -45,7 +45,8 @@ SRCS = err.c \ getopt_long.c \ up_cxxinitialize.c \ pid/pid.c \ - geo/geo.c \ systemlib.c \ airspeed.c \ - system_params.c + system_params.c \ + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 000000000..9d47d8000 --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file rc_check.c + * + * RC calibration check + */ + +#include <nuttx/config.h> + +#include <stdio.h> +#include <fcntl.h> + +#include <systemlib/rc_check.h> +#include <systemlib/param/param.h> +#include <mavlink/mavlink_log.h> +#include <uORB/topics/rc_channels.h> + +int rc_calibration_check(void) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + } +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 000000000..e2238d151 --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(void); + +__END_DECLS diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 3283aad8a..57a751e1c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -43,13 +43,29 @@ #include <fcntl.h> #include <sched.h> #include <signal.h> -#include <sys/stat.h> #include <unistd.h> #include <float.h> #include <string.h> +#include <sys/stat.h> +#include <sys/types.h> + +#include <stm32_pwr.h> + #include "systemlib.h" +void +systemreset(bool to_bootloader) +{ + if (to_bootloader) { + stm32_pwr_enablebkp(); + + /* XXX wow, this is evil - write a magic number into backup register zero */ + *(uint32_t *)0x40002850 = 0xb007b007; + } + up_systemreset(); +} + static void kill_task(FAR struct tcb_s *tcb, FAR void *arg); void killall() diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e52..3728f2067 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,11 @@ #include <float.h> #include <stdint.h> -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +__EXPORT void systemreset(bool to_bootloader) noreturn_function; + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c deleted file mode 100644 index ff6af031f..000000000 --- a/src/modules/test/foo.c +++ /dev/null @@ -1,4 +0,0 @@ -int test_main(void) -{ - return 0; -}
\ No newline at end of file diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk deleted file mode 100644 index abf80eedf..000000000 --- a/src/modules/test/module.mk +++ /dev/null @@ -1,4 +0,0 @@ - -MODULE_NAME = test -SRCS = foo.c - diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 301cfa255..1eb63a799 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,6 +81,9 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); + #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); @@ -102,6 +105,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s); #include "topics/vehicle_command.h" ORB_DEFINE(vehicle_command, struct vehicle_command_s); +#include "topics/vehicle_control_mode.h" +ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); + #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); @@ -114,6 +120,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp #include "topics/vehicle_global_position_set_triplet.h" ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/vehicle_global_velocity_setpoint.h" +ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); + #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); @@ -123,6 +132,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); @@ -150,6 +162,8 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); + +#include "topics/actuator_armed.h" ORB_DEFINE(actuator_armed, struct actuator_armed_s); /* actuator controls, as set by actuators / mixers after limiting */ diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h new file mode 100644 index 000000000..6e944ffee --- /dev/null +++ b/src/modules/uORB/topics/actuator_armed.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file actuator_armed.h + * + * Actuator armed topic + * + */ + +#ifndef TOPIC_ACTUATOR_ARMED_H +#define TOPIC_ACTUATOR_ARMED_H + +#include <stdint.h> +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_armed_s { + + uint64_t timestamp; + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_armed); + +#endif
\ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index a27095be5..e768ab2f6 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,6 +52,9 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + /** * @addtogroup topics * @{ @@ -72,16 +75,4 @@ ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ -}; - -ORB_DECLARE(actuator_armed); - -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 261a8a4ad..eac6d6e98 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -56,17 +56,18 @@ struct manual_control_setpoint_s { float yaw; /**< rudder / yaw rate / yaw */ float throttle; /**< throttle / collective thrust / altitude */ - float manual_override_switch; /**< manual override mode (mandatory) */ - float auto_mode_switch; /**< auto mode switch (mandatory) */ + float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ - float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ - float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ - float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ + + // XXX needed or parameter? + //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ float flaps; /**< flap position */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index e69335b3d..5a8580143 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -48,9 +48,12 @@ /** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, - * leaving at a sane value of 14. + * leaving at a sane value of 15. + * This number can be greater then number of RC channels, + * because single RC channel can be mapped to multiple + * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 14 +#define RC_CHANNELS_MAX 15 /** * This defines the mapping of the RC functions. @@ -63,18 +66,17 @@ enum RC_CHANNELS_FUNCTION ROLL = 1, PITCH = 2, YAW = 3, - OVERRIDE = 4, - AUTO_MODE = 5, - MANUAL_MODE = 6, - SAS_MODE = 7, - RTL = 8, - OFFBOARD_MODE = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, + MODE = 4, + RETURN = 5, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/mathlib/mathlib.h b/src/modules/uORB/topics/safety.h index 40ffb22bc..a5d21cd4a 100644 --- a/src/modules/mathlib/mathlib.h +++ b/src/modules/uORB/topics/safety.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,28 +32,26 @@ ****************************************************************************/ /** - * @file mathlib.h + * @file safety.h * - * Common header for mathlib exports. + * Safety topic to pass safety state from px4io driver to commander + * This concerns only the safety button of the px4io but has nothing to do + * with arming/disarming. */ -#ifdef __cplusplus +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H -#pragma once +#include <stdint.h> +#include "../uORB.h" -#include "math/Dcm.hpp" -#include "math/EulerAngles.hpp" -#include "math/Matrix.hpp" -#include "math/Quaternion.hpp" -#include "math/Vector.hpp" -#include "math/Vector3.hpp" -#include "math/Vector2f.hpp" -#include "math/Limits.hpp" +struct safety_s { -#endif + uint64_t timestamp; + bool safety_switch_available; /**< Set to true if a safety switch is connected */ + bool safety_off; /**< Set to true if safety is off */ +}; -#ifdef CONFIG_ARCH_ARM - -#include "CMSIS/Include/arm_math.h" +ORB_DECLARE(safety); #endif
\ No newline at end of file diff --git a/src/modules/mathlib/math/Vector.cpp b/src/modules/uORB/topics/vehicle_control_debug.h index 35158a396..6184284a4 100644 --- a/src/modules/mathlib/math/Vector.cpp +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -1,6 +1,7 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -32,69 +33,55 @@ ****************************************************************************/ /** - * @file Vector.cpp - * - * math vector + * @file vehicle_control_debug.h + * For debugging purposes to log PID parts of controller */ -#include "test/test.hpp" +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ -#include "Vector.hpp" +#include <stdint.h> +#include "../uORB.h" -namespace math +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s { + uint64_t timestamp; /**< in microseconds since system start */ -static const float data_testA[] = {1, 3}; -static const float data_testB[] = {4, 1}; + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ -static Vector testA(2, data_testA); -static Vector testB(2, data_testB); + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ -int __EXPORT vectorTest() -{ - vectorAddTest(); - vectorSubTest(); - return 0; -} + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ -int vectorAddTest() -{ - printf("Test Vector Add\t\t: "); - Vector r = testA + testB; - float data_test[] = {5.0f, 4.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ -int vectorSubTest() -{ - printf("Test Vector Sub\t\t: "); - Vector r(2); - r = testA - testB; - float data_test[] = { -3.0f, 2.0f}; - ASSERT(vectorEqual(Vector(2, data_test), r)); - printf("PASS\n"); - return 0; -} + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ -bool vectorEqual(const Vector &a, const Vector &b, float eps) -{ - if (a.getRows() != b.getRows()) { - printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows()); - return false; - } + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ - bool ret = true; +}; /**< vehicle_control_debug */ - for (size_t i = 0; i < a.getRows(); i++) { - if (!equal(a(i), b(i), eps)) { - printf("element mismatch (%d)\n", i); - ret = false; - } - } + /** + * @} + */ - return ret; -} +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); -} // namespace math +#endif diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h new file mode 100644 index 000000000..093c6775d --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file vehicle_control_mode.h + * Definition of the vehicle_control_mode uORB topic. + * + * All control apps should depend their actions based on the flags set here. + */ + +#ifndef VEHICLE_CONTROL_MODE +#define VEHICLE_CONTROL_MODE + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_control_mode_s +{ + uint16_t counter; /**< incremented by the writing thread every time new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + bool flag_armed; + + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + + // XXX needs yet to be set by state machine helper + bool flag_system_hil_enabled; + + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_rates_enabled; /**< true if rates are stabilized */ + bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_control_altitude_enabled; /**< true if altitude is controlled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + + bool flag_control_auto_enabled; // TEMP + uint8_t auto_state; // TEMP navigation state for AUTO modes +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_mode); + +#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index f036c7223..0fc0ed5c9 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,18 +62,17 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees LOGME */ - float alt; /**< Altitude in meters LOGME */ - float relative_alt; /**< Altitude above home position in meters, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + float alt; /**< Altitude in meters */ + float relative_alt; /**< Altitude above home position in meters, */ + float vx; /**< Ground X velocity, m/s in NED */ + float vy; /**< Ground Y velocity, m/s in NED */ + float vz; /**< Ground Z velocity, m/s in NED */ + float yaw; /**< Compass heading in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h new file mode 100644 index 000000000..73961cdfe --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin <anton.babushkin@me.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file vehicle_global_velocity_setpoint.h + * Definition of the global velocity setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ +#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_global_velocity_setpoint_s +{ + float vx; /**< in m/s NED */ + float vy; /**< in m/s NED */ + float vz; /**< in m/s NED */ +}; /**< Velocity setpoint in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_velocity_setpoint); + +#endif diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 76eddeacd..31a0e632b 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -54,27 +54,29 @@ */ struct vehicle_local_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - float x; /**< X positin in meters in NED earth-fixed frame */ - float y; /**< X positin in meters in NED earth-fixed frame */ - float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - - // TODO Add covariances here - + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool xy_valid; /**< true if x and y are valid */ + bool z_valid; /**< true if z is valid */ + bool v_xy_valid; /**< true if vy and vy are valid */ + bool v_z_valid; /**< true if vz is valid */ + /* Position in local NED frame */ + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< X position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + /* Velocity in NED frame */ + float vx; /**< Ground X Speed (Latitude), m/s in NED */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED */ + /* Heading */ + float yaw; /* Reference position in GPS / WGS84 frame */ - uint64_t home_timestamp;/**< Time when home position was set */ - int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ - float home_alt; /**< Altitude in meters LOGME */ - float home_hdg; /**< Compass heading in radians -PI..+PI. */ - + bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ + uint64_t ref_timestamp; /**< Time when reference position was set */ + int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ + int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ + bool landed; /**< true if vehicle is landed */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 94068a9ac..1c184d3a7 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,22 +54,67 @@ #include <stdbool.h> #include "../uORB.h" -/* State Machine */ -typedef enum -{ - SYSTEM_STATE_PREFLIGHT = 0, - SYSTEM_STATE_STANDBY = 1, - SYSTEM_STATE_GROUND_READY = 2, - SYSTEM_STATE_MANUAL = 3, - SYSTEM_STATE_STABILIZED = 4, - SYSTEM_STATE_AUTO = 5, - SYSTEM_STATE_MISSION_ABORT = 6, - SYSTEM_STATE_EMCY_LANDING = 7, - SYSTEM_STATE_EMCY_CUTOFF = 8, - SYSTEM_STATE_GROUND_ERROR = 9, - SYSTEM_STATE_REBOOT= 10, - -} commander_state_machine_t; +/** + * @addtogroup topics @{ + */ + +/* main state machine */ +typedef enum { + MAIN_STATE_MANUAL = 0, + MAIN_STATE_SEATBELT, + MAIN_STATE_EASY, + MAIN_STATE_AUTO, +} main_state_t; + +/* navigation state machine */ +typedef enum { + NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization + NAVIGATION_STATE_STABILIZE, // attitude stabilization + NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization + NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization + NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff + NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode + NAVIGATION_STATE_AUTO_LOITER, // pause mission + NAVIGATION_STATE_AUTO_MISSION, // fly mission + NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND + NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) +} navigation_state_t; + +typedef enum { + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; + +typedef enum { + HIL_STATE_OFF = 0, + HIL_STATE_ON +} hil_state_t; + +typedef enum { + MODE_SWITCH_MANUAL = 0, + MODE_SWITCH_ASSISTED, + MODE_SWITCH_AUTO +} mode_switch_pos_t; + +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_EASY +} assisted_switch_pos_t; + +typedef enum { + RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_RETURN +} return_switch_pos_t; + +typedef enum { + MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_MISSION +} mission_switch_pos_t; enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, @@ -82,26 +127,6 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ -enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ - VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -}; - -enum VEHICLE_MANUAL_CONTROL_MODE { - VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ - VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -}; - -enum VEHICLE_MANUAL_SAS_MODE { - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ - VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ - VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -128,9 +153,9 @@ enum VEHICLE_TYPE { }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** @@ -149,55 +174,54 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ - //uint64_t failsave_highlevel_begin; TO BE COMPLETED - commander_state_machine_t state_machine; /**< current flight state, main state machine */ - enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ + main_state_t main_state; /**< main state machine */ + navigation_state_t navigation_state; /**< navigation state machine */ + arming_state_t arming_state; /**< current arming state */ + hil_state_t hil_state; /**< current hil state */ + int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - /* system flags - these represent the state predicates */ - - bool flag_system_armed; /**< true is motors / actuators are armed */ - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ - - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - bool flag_preflight_accel_calibration; - bool flag_preflight_airspeed_calibration; + bool is_rotary_wing; + + mode_switch_pos_t mode_switch; + return_switch_pos_t return_switch; + assisted_switch_pos_t assisted_switch; + mission_switch_pos_t mission_switch; + + bool condition_battery_voltage_valid; + bool condition_system_in_air_restore; /**< true if we can restore in mid air */ + bool condition_system_sensors_initialized; + bool condition_system_returned_to_home; + bool condition_auto_mission_available; + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_launch_position_valid; /**< indicates a valid launch position */ + bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool condition_local_position_valid; + bool condition_local_altitude_valid; + bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception is terminally lost */ - bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ - uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool rc_signal_lost; /**< true if RC reception lost */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - //bool failsave_highlevel; - /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - float load; - float voltage_battery; - float current_battery; + + float load; /**< processor load from 0 to 1 */ + float battery_voltage; + float battery_current; float battery_remaining; + enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ uint16_t drop_rate_comm; uint16_t errors_comm; @@ -205,15 +229,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - bool flag_local_position_valid; - bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_valid_launch_position; /**< indicates a valid launch position */ - bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ - bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ }; /** diff --git a/src/modules/mathlib/module.mk b/src/modules/unit_test/module.mk index 2146a1413..f00b0f592 100644 --- a/src/modules/mathlib/module.mk +++ b/src/modules/unit_test/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -32,31 +32,8 @@ ############################################################################ # -# Math library +# Makefile to build the unit test library. # -SRCS = math/test/test.cpp \ - math/Vector.cpp \ - math/Vector2f.cpp \ - math/Vector3.cpp \ - math/EulerAngles.cpp \ - math/Quaternion.cpp \ - math/Dcm.cpp \ - math/Matrix.cpp \ - math/Limits.cpp -# -# In order to include .config we first have to save off the -# current makefile name, since app.mk needs it. -# -APP_MAKEFILE := $(lastword $(MAKEFILE_LIST)) --include $(TOPDIR)/.config +SRCS = unit_test.cpp -ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy) -INCLUDE_DIRS += math/arm -SRCS += math/arm/Vector.cpp \ - math/arm/Matrix.cpp -else -#INCLUDE_DIRS += math/generic -#SRCS += math/generic/Vector.cpp \ -# math/generic/Matrix.cpp -endif diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp new file mode 100644 index 000000000..64ee544a2 --- /dev/null +++ b/src/modules/unit_test/unit_test.cpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file unit_test.cpp + * A unit test library. + * + */ + +#include "unit_test.h" + +#include <systemlib/err.h> + + +UnitTest::UnitTest() +{ +} + +UnitTest::~UnitTest() +{ +} + +void +UnitTest::print_results(const char* result) +{ + if (result != 0) { + warnx("Failed: %s:%d", mu_last_test(), mu_line()); + warnx("%s", result); + } else { + warnx("ALL TESTS PASSED"); + warnx(" Tests run : %d", mu_tests_run()); + warnx(" Assertion : %d", mu_assertion()); + } +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h new file mode 100644 index 000000000..3020734f4 --- /dev/null +++ b/src/modules/unit_test/unit_test.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks <sjwilks@gmail.com> + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * 3. Neither the name PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file unit_test.h + * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html). + * + */ + +#ifndef UNIT_TEST_H_ +#define UNIT_TEST_ + +#include <systemlib/err.h> + + +class __EXPORT UnitTest +{ + +public: +#define xstr(s) str(s) +#define str(s) #s +#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } + +INLINE_GLOBAL(int, mu_tests_run) +INLINE_GLOBAL(int, mu_assertion) +INLINE_GLOBAL(int, mu_line) +INLINE_GLOBAL(const char*, mu_last_test) + +#define mu_assert(message, test) \ + do \ + { \ + if (!(test)) \ + return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \ + else \ + mu_assertion()++; \ + } while (0) + + +#define mu_run_test(test) \ +do \ +{ \ + const char *message; \ + mu_last_test() = #test; \ + mu_line() = __LINE__; \ + message = test(); \ + mu_tests_run()++; \ + if (message) \ + return message; \ +} while (0) + + +public: + UnitTest(); + virtual ~UnitTest(); + + virtual const char* run_tests() = 0; + virtual void print_results(const char* result); +}; + + + +#endif /* UNIT_TEST_H_ */ |