From e8da98fd149282533b5b8fa344c1619ece5fbcc0 Mon Sep 17 00:00:00 2001 From: mcsauder Date: Fri, 22 Apr 2022 10:43:25 -0600 Subject: [PATCH 01/13] Add gyro clipping to mirror accel clipping monitoring instances. --- msg/sensor_combined.msg | 22 ++++---- msg/sensor_gyro.msg | 2 + msg/vehicle_attitude.msg | 10 ++-- msg/vehicle_imu.msg | 3 ++ msg/vehicle_imu_status.msg | 1 + src/lib/drivers/gyroscope/PX4Gyroscope.cpp | 32 ++++++++++- src/lib/drivers/gyroscope/PX4Gyroscope.hpp | 9 ++-- .../sensors/vehicle_imu/VehicleIMU.cpp | 54 +++++++++++++++++-- .../sensors/vehicle_imu/VehicleIMU.hpp | 11 ++-- src/modules/sensors/voted_sensors_update.cpp | 12 +++-- 10 files changed, 126 insertions(+), 30 deletions(-) diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 26ea318a83..837c7a1fac 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -2,22 +2,24 @@ # These fields are scaled and offset-compensated where possible and do not # change with board revisions and sensor updates. -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid # gyro timstamp is equal to the timestamp of the message -float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period -uint32 gyro_integral_dt # gyro measurement sampling period in microseconds +float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period +uint32 gyro_integral_dt # gyro measurement sampling period in microseconds -int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp -float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period -uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp +float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period +uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 uint8 CLIPPING_Z = 4 -uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the sampling period -uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. -uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. +uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame +uint8 gyro_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame + +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index b1025e1d12..b906127b58 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -11,6 +11,8 @@ float32 temperature # temperature in degrees Celsius uint32 error_count +uint8[3] clip_counter # clip count per axis in the sample period + uint8 samples # number of raw samples that went into this message uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index a57b7c5058..f704d71933 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -1,12 +1,12 @@ # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) -float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame -float32[4] delta_q_reset # Amount by which quaternion has changed during last reset -uint8 quat_reset_counter # Quaternion reset counter +float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame +float32[4] delta_q_reset # Amount by which quaternion has changed during last reset +uint8 quat_reset_counter # Quaternion reset counter # TOPICS vehicle_attitude vehicle_attitude_groundtruth vehicle_vision_attitude # TOPICS estimator_attitude diff --git a/msg/vehicle_imu.msg b/msg/vehicle_imu.msg index 0184039157..a71bb7a01f 100644 --- a/msg/vehicle_imu.msg +++ b/msg/vehicle_imu.msg @@ -8,12 +8,15 @@ uint32 gyro_device_id # Gyroscope unique device ID for the sensor that float32[3] delta_angle # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt) float32[3] delta_velocity # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt) + uint16 delta_angle_dt # integration period in microseconds uint16 delta_velocity_dt # integration period in microseconds uint8 CLIPPING_X = 1 uint8 CLIPPING_Y = 2 uint8 CLIPPING_Z = 4 + +uint8 delta_angle_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. diff --git a/msg/vehicle_imu_status.msg b/msg/vehicle_imu_status.msg index 26db7487a7..78fb44703e 100644 --- a/msg/vehicle_imu_status.msg +++ b/msg/vehicle_imu_status.msg @@ -4,6 +4,7 @@ uint32 accel_device_id # unique device ID for the sensor that does not uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles uint32[3] accel_clipping # total clipping per axis +uint32[3] gyro_clipping # total clipping per axis uint32 accel_error_count uint32 gyro_error_count diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index 432a8cb98d..fdce2004a1 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 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 @@ -50,6 +50,22 @@ static constexpr int32_t sum(const int16_t samples[], uint8_t len) return sum; } +static constexpr uint8_t clipping(const int16_t samples[], uint8_t len) +{ + unsigned clip_count = 0; + + for (int n = 0; n < len; n++) { + // - consider data clipped/saturated if it's INT16_MIN/INT16_MAX or within 1 + // - this accommodates rotated data (|INT16_MIN| = INT16_MAX + 1) + // and sensors that may re-use the lowest bit for other purposes (sync indicator, etc) + if ((samples[n] <= INT16_MIN + 1) || (samples[n] >= INT16_MAX - 1)) { + clip_count++; + } + } + + return clip_count; +} + PX4Gyroscope::PX4Gyroscope(uint32_t device_id, enum Rotation rotation) : _device_id{device_id}, _rotation{rotation} @@ -90,6 +106,8 @@ void PX4Gyroscope::set_scale(float scale) } _scale = scale; + + UpdateClipLimit(); } } @@ -107,6 +125,9 @@ void PX4Gyroscope::update(const hrt_abstime ×tamp_sample, float x, float y, report.x = x * _scale; report.y = y * _scale; report.z = z * _scale; + report.clip_counter[0] = (fabsf(x) >= _clip_limit); + report.clip_counter[1] = (fabsf(y) >= _clip_limit); + report.clip_counter[2] = (fabsf(z) >= _clip_limit); report.samples = 1; report.timestamp = hrt_absolute_time(); @@ -145,8 +166,17 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample) _last_sample[1] = sample.y[N - 1]; _last_sample[2] = sample.z[N - 1]; + report.clip_counter[0] = clipping(sample.x, N); + report.clip_counter[1] = clipping(sample.y, N); + report.clip_counter[2] = clipping(sample.z, N); report.samples = N; report.timestamp = hrt_absolute_time(); _sensor_pub.publish(report); } + +void PX4Gyroscope::UpdateClipLimit() +{ + // 99.9% of potential max + _clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX); +} diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 893020a490..f89514f7a1 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -1,7 +1,6 @@ - /**************************************************************************** * - * Copyright (c) 2018-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2018-2022 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 @@ -53,7 +52,7 @@ public: void set_device_id(uint32_t device_id) { _device_id = device_id; } void set_device_type(uint8_t devtype); void set_error_count(uint32_t error_count) { _error_count = error_count; } - void set_range(float range) { _range = range; } + void set_range(float range) { _range = range; UpdateClipLimit(); } void set_scale(float scale); void set_temperature(float temperature) { _temperature = temperature; } @@ -64,6 +63,8 @@ public: int get_instance() { return _sensor_pub.get_instance(); }; private: + void UpdateClipLimit(); + uORB::PublicationMulti _sensor_pub{ORB_ID(sensor_gyro)}; uORB::PublicationMulti _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)}; @@ -76,6 +77,8 @@ private: float _scale{1.f}; float _temperature{NAN}; + float _clip_limit{_range / _scale}; + uint32_t _error_count{0}; int16_t _last_sample[3] {}; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 12d1f1a637..e5bdddb83f 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp @@ -407,11 +407,11 @@ bool VehicleIMU::UpdateAccel() _publish_status = true; - if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_clipping_notify_time) > 3_s)) { + if (_accel_calibration.enabled() && (hrt_elapsed_time(&_last_accel_clipping_notify_time) > 3_s)) { // start notifying the user periodically if there's significant continuous clipping const uint64_t clipping_total = _status.accel_clipping[0] + _status.accel_clipping[1] + _status.accel_clipping[2]; - if (clipping_total > _last_clipping_notify_total_count + 1000) { + if (clipping_total > _last_accel_clipping_notify_total_count + 1000) { mavlink_log_critical(&_mavlink_log_pub, "Accel %" PRIu8 " clipping, not safe to fly!\t", _instance); /* EVENT * @description Land now, and check the vehicle setup. @@ -419,8 +419,8 @@ bool VehicleIMU::UpdateAccel() */ events::send(events::ID("vehicle_imu_accel_clipping"), events::Log::Critical, "Accel {1} clipping, not safe to fly!", _instance); - _last_clipping_notify_time = accel.timestamp_sample; - _last_clipping_notify_total_count = clipping_total; + _last_accel_clipping_notify_time = accel.timestamp_sample; + _last_accel_clipping_notify_total_count = clipping_total; } } } @@ -534,6 +534,52 @@ bool VehicleIMU::UpdateGyro() _gyro_integrator.put(gyro_raw, dt); updated = true; + + if (gyro.clip_counter[0] > 0 || gyro.clip_counter[1] > 0 || gyro.clip_counter[2] > 0) { + // rotate sensor clip counts into vehicle body frame + const Vector3f clipping{_gyro_calibration.rotation() * + Vector3f{(float)gyro.clip_counter[0], (float)gyro.clip_counter[1], (float)gyro.clip_counter[2]}}; + + // round to get reasonble clip counts per axis (after board rotation) + const uint8_t clip_x = roundf(fabsf(clipping(0))); + const uint8_t clip_y = roundf(fabsf(clipping(1))); + const uint8_t clip_z = roundf(fabsf(clipping(2))); + + _status.gyro_clipping[0] += clip_x; + _status.gyro_clipping[1] += clip_y; + _status.gyro_clipping[2] += clip_z; + + if (clip_x > 0) { + _delta_angle_clipping |= vehicle_imu_s::CLIPPING_X; + } + + if (clip_y > 0) { + _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Y; + } + + if (clip_z > 0) { + _delta_angle_clipping |= vehicle_imu_s::CLIPPING_Z; + } + + _publish_status = true; + + if (_gyro_calibration.enabled() && (hrt_elapsed_time(&_last_gyro_clipping_notify_time) > 3_s)) { + // start notifying the user periodically if there's significant continuous clipping + const uint64_t clipping_total = _status.gyro_clipping[0] + _status.gyro_clipping[1] + _status.gyro_clipping[2]; + + if (clipping_total > _last_gyro_clipping_notify_total_count + 1000) { + mavlink_log_critical(&_mavlink_log_pub, "Gyro %" PRIu8 " clipping, not safe to fly!\t", _instance); + /* EVENT + * @description Land now, and check the vehicle setup. + * Clipping can lead to fly-aways. + */ + events::send(events::ID("vehicle_imu_gyro_clipping"), events::Log::Critical, + "Gyro {1} clipping, not safe to fly!", _instance); + _last_gyro_clipping_notify_time = gyro.timestamp_sample; + _last_gyro_clipping_notify_total_count = clipping_total; + } + } + } } return updated; diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 2caf491b21..f6bb04c784 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp @@ -148,10 +148,15 @@ private: float _coning_norm_accum{0}; float _coning_norm_accum_total_time_s{0}; - uint8_t _delta_velocity_clipping{0}; + uint8_t _delta_angle_clipping{0}; + uint8_t _delta_velocity_clipping{0}; + + hrt_abstime _last_accel_clipping_notify_time{0}; + hrt_abstime _last_gyro_clipping_notify_time{0}; + + uint64_t _last_accel_clipping_notify_total_count{0}; + uint64_t _last_gyro_clipping_notify_total_count{0}; - hrt_abstime _last_clipping_notify_time{0}; - uint64_t _last_clipping_notify_total_count{0}; orb_advert_t _mavlink_log_pub{nullptr}; uint32_t _backup_schedule_timeout_us{20000}; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index d363ba11de..3e89e18644 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -172,6 +172,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); _last_sensor_data[uorb_index].gyro_integral_dt = imu_report.delta_angle_dt; + _last_sensor_data[uorb_index].gyro_clipping = imu_report.delta_angle_clipping; _last_sensor_data[uorb_index].accel_calibration_count = imu_report.accel_calibration_count; _last_sensor_data[uorb_index].gyro_calibration_count = imu_report.gyro_calibration_count; @@ -229,11 +230,14 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, sizeof(raw.accelerometer_m_s2)); memcpy(&raw.gyro_rad, &_last_sensor_data[gyro_best_index].gyro_rad, sizeof(raw.gyro_rad)); + raw.accelerometer_integral_dt = _last_sensor_data[accel_best_index].accelerometer_integral_dt; - raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; - raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; - raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count; - raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count; + raw.accelerometer_clipping = _last_sensor_data[accel_best_index].accelerometer_clipping; + raw.accel_calibration_count = _last_sensor_data[accel_best_index].accel_calibration_count; + + raw.gyro_integral_dt = _last_sensor_data[gyro_best_index].gyro_integral_dt; + raw.gyro_clipping = _last_sensor_data[gyro_best_index].gyro_clipping; + raw.gyro_calibration_count = _last_sensor_data[gyro_best_index].gyro_calibration_count; if ((accel_best_index != _accel.last_best_vote) || (_selection.accel_device_id != _accel_device_id[accel_best_index])) { _accel.last_best_vote = (uint8_t)accel_best_index; From 9518b65f93614e9f06b7b26baa8a0e65c592e77c Mon Sep 17 00:00:00 2001 From: PX4 BuildBot Date: Thu, 19 May 2022 00:38:59 +0000 Subject: [PATCH 02/13] Update submodule mavlink to latest Thu May 19 00:38:59 UTC 2022 - mavlink in PX4/Firmware (87c73145b36a835b1635de0498a5613a7af5cafc): https://github.com/mavlink/mavlink/commit/a1cb2c0e71f191f04da3d92d92db8702a7e91ff6 - mavlink current upstream: https://github.com/mavlink/mavlink/commit/99e82cad70494903a23a67de08ff9cbb5918d8f3 - Changes: https://github.com/mavlink/mavlink/compare/a1cb2c0e71f191f04da3d92d92db8702a7e91ff6...99e82cad70494903a23a67de08ff9cbb5918d8f3 99e82cad 2022-05-19 Hamish Willee - Deprecated GPS_INJECT_DATA (#1842) bf3df03d 2022-05-16 Hamish Willee - WIND_COV - accuracy units are m/s (#1844) a73d4864 2022-05-11 Hamish Willee - development.xml FIGURE_EIGHT_EXECUTION_STATUS - add a note about it being set at lower rate (#1841) --- src/modules/mavlink/mavlink | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index a1cb2c0e71..99e82cad70 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit a1cb2c0e71f191f04da3d92d92db8702a7e91ff6 +Subproject commit 99e82cad70494903a23a67de08ff9cbb5918d8f3 From fd4b62032e674bc8b25c8e83b434b2564d5e7704 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 19 May 2022 12:47:50 -0400 Subject: [PATCH 03/13] commander: publish actuator_armed first on any change --- src/modules/commander/Commander.cpp | 127 ++++++++++++++++------------ 1 file changed, 74 insertions(+), 53 deletions(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 2ef3f756e8..5609b7d44e 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -91,6 +91,20 @@ typedef enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_ENUM_END = 129, /* | */ } VEHICLE_MODE_FLAG; +// TODO: generate +static constexpr bool operator ==(const actuator_armed_s &a, const actuator_armed_s &b) +{ + return (a.armed == b.armed && + a.prearmed == b.prearmed && + a.ready_to_arm == b.ready_to_arm && + a.lockdown == b.lockdown && + a.manual_lockdown == b.manual_lockdown && + a.force_failsafe == b.force_failsafe && + a.in_esc_calibration_mode == b.in_esc_calibration_mode && + a.soft_stop == b.soft_stop); +} +static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review"); + #if defined(BOARD_HAS_POWER_CONTROL) static orb_advert_t tune_control_pub = nullptr; static void play_power_button_down_tune() @@ -2126,8 +2140,6 @@ void Commander::updateParameters() void Commander::run() { - bool sensor_fail_tune_played = false; - /* initialize */ led_init(); buzzer_init(); @@ -2171,6 +2183,9 @@ Commander::run() perf_begin(_loop_perf); + const actuator_armed_s actuator_armed_prev{_armed}; + const vehicle_status_flags_s vehicle_status_flags_prev{_status_flags}; + /* update parameters */ const bool params_updated = _parameter_update_sub.updated(); @@ -3018,54 +3033,49 @@ Commander::run() _failsafe_old = _status.failsafe; } - /* publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed */ - if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed) { - update_control_mode(); + // prearm mode + switch ((PrearmedMode)_param_com_prearm_mode.get()) { + case PrearmedMode::DISABLED: + /* skip prearmed state */ + _armed.prearmed = false; + break; - _status.timestamp = hrt_absolute_time(); - _status_pub.publish(_status); + case PrearmedMode::ALWAYS: + /* safety is not present, go into prearmed + * (all output drivers should be started / unlocked last in the boot process + * when the rest of the system is fully initialized) + */ + _armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s); + break; - switch ((PrearmedMode)_param_com_prearm_mode.get()) { - case PrearmedMode::DISABLED: - /* skip prearmed state */ + case PrearmedMode::SAFETY_BUTTON: + if (_safety.safety_switch_available) { + /* safety switch is present, go into prearmed if safety is off */ + _armed.prearmed = _safety.safety_off; + + } else { + /* safety switch is not present, do not go into prearmed */ _armed.prearmed = false; - break; - - case PrearmedMode::ALWAYS: - /* safety is not present, go into prearmed - * (all output drivers should be started / unlocked last in the boot process - * when the rest of the system is fully initialized) - */ - _armed.prearmed = (hrt_elapsed_time(&_boot_timestamp) > 5_s); - break; - - case PrearmedMode::SAFETY_BUTTON: - if (_safety.safety_switch_available) { - /* safety switch is present, go into prearmed if safety is off */ - _armed.prearmed = _safety.safety_off; - - } else { - /* safety switch is not present, do not go into prearmed */ - _armed.prearmed = false; - } - - break; - - default: - _armed.prearmed = false; - break; } - _armed.timestamp = hrt_absolute_time(); - _armed_pub.publish(_armed); + break; - /* publish internal state for logging purposes */ - _internal_state.timestamp = hrt_absolute_time(); - _commander_state_pub.publish(_internal_state); + default: + _armed.prearmed = false; + break; + } + + + // publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed + if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed + || !(_armed == actuator_armed_prev)) { + + // Evaluate current prearm status (skip during arm -> disarm transition) + if (!actuator_armed_prev.armed && !_armed.armed && !_status_flags.calibration_enabled) { + + _status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); - // Evaluate current prearm status - if (!_armed.armed && !_status_flags.calibration_enabled) { perf_begin(_preflight_check_perf); bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode, false, true, hrt_elapsed_time(&_boot_timestamp)); @@ -3077,17 +3087,31 @@ Commander::run() bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req, _status, false); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res - && prearm_check_res), _status); + const bool prearm_check_ok = preflight_check_res && prearm_check_res; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, prearm_check_ok, _status); } - /* publish vehicle_status_flags */ + // publish actuator_armed first (used by output modules) + _armed.timestamp = hrt_absolute_time(); + _armed_pub.publish(_armed); + + // update and publish vehicle_control_mode + update_control_mode(); + + // vehicle_status publish (after prearm/preflight updates above) + _status.timestamp = hrt_absolute_time(); + _status_pub.publish(_status); + + // publish vehicle_status_flags (after prearm/preflight updates above) _status_flags.timestamp = hrt_absolute_time(); _vehicle_status_flags_pub.publish(_status_flags); - /* publish failure_detector data */ + // commander_state publish internal state for logging purposes + _internal_state.timestamp = hrt_absolute_time(); + _commander_state_pub.publish(_internal_state); + + // failure_detector_status publish failure_detector_status_s fd_status{}; - fd_status.timestamp = hrt_absolute_time(); fd_status.fd_roll = _failure_detector.getStatusFlags().roll; fd_status.fd_pitch = _failure_detector.getStatusFlags().pitch; fd_status.fd_alt = _failure_detector.getStatusFlags().alt; @@ -3098,6 +3122,7 @@ Commander::run() fd_status.fd_motor = _failure_detector.getStatusFlags().motor; fd_status.imbalanced_prop_metric = _failure_detector.getImbalancedPropMetric(); fd_status.motor_failure_mask = _failure_detector.getMotorFailures(); + fd_status.timestamp = hrt_absolute_time(); _failure_detector_status_pub.publish(fd_status); } @@ -3139,14 +3164,10 @@ Commander::run() } /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ - _status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); - - if (!sensor_fail_tune_played && (!_status_flags.system_sensors_initialized - && _status_flags.system_hotplug_timeout)) { + if (!_status_flags.system_sensors_initialized && + !vehicle_status_flags_prev.system_hotplug_timeout && _status_flags.system_hotplug_timeout) { set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING); - sensor_fail_tune_played = true; - _status_changed = true; } // check if the worker has finished From 887fe7dba2d2d0dbdf8a2b9141f1b0de781ebc4e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 19 May 2022 14:50:46 +0200 Subject: [PATCH 04/13] commander_params: shorten low battery action delay I got multiple times the feedback now that a consistent delay is helpful and makes sense but the default delay is too long for low battery action where you're trying to come back in time and possibly the emergency reaction kicks in while the critical action is executing which leads to a longer accumulated delay. --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index b16c5cf65d..f1bb34c84c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -296,7 +296,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); * @max 25.0 * @decimal 3 */ -PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 10.0f); +PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 5.f); /** * Imbalanced propeller failsafe mode From c5bbf4553b1ca880b105a54af9a06f981d8a4ab9 Mon Sep 17 00:00:00 2001 From: alessandro <3762382+potaito@users.noreply.github.com> Date: Thu, 19 May 2022 14:09:17 +0200 Subject: [PATCH 05/13] ubuntu.sh: fix gazebo and openjdk for 22.04 --- Tools/setup/ubuntu.sh | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index 852f38c94c..25c12cb170 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -200,13 +200,12 @@ if [[ $INSTALL_SIM == "true" ]]; then if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then java_version=11 - gazebo_version=9 elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then java_version=13 - gazebo_version=11 + elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then + java_version=11 else java_version=14 - gazebo_version=11 fi # Java (jmavsim or fastrtps) sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ @@ -220,20 +219,30 @@ if [[ $INSTALL_SIM == "true" ]]; then sudo update-alternatives --set java $(update-alternatives --list java | grep "java-$java_version") # Gazebo + if [[ "${UBUNTU_RELEASE}" == "18.04" ]]; then + gazebo_version=9 + gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev" + elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then + gazebo_packages="gazebo libgazebo-dev" + else + # default and Ubuntu 20.04 + gazebo_version=11 + gazebo_packages="gazebo$gazebo_version libgazebo$gazebo_version-dev" + fi + sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - # Update list, since new gazebo-stable.list has been added sudo apt-get update -y --quiet sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ dmidecode \ - gazebo$gazebo_version \ + $gazebo_packages \ gstreamer1.0-plugins-bad \ gstreamer1.0-plugins-base \ gstreamer1.0-plugins-good \ gstreamer1.0-plugins-ugly \ gstreamer1.0-libav \ libeigen3-dev \ - libgazebo$gazebo_version-dev \ libgstreamer-plugins-base1.0-dev \ libimage-exiftool-perl \ libopencv-dev \ From 47532ca07ba933a11ab21df6e3aa1a45f320b17b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 16 May 2022 18:25:26 +0200 Subject: [PATCH 06/13] ArmStateMachine: replace state name array with method --- msg/vehicle_status.msg | 2 -- .../ArmStateMachine/ArmStateMachine.cpp | 25 ++++++++++++++++++- .../ArmStateMachine/ArmStateMachine.hpp | 12 ++------- src/modules/commander/Commander.cpp | 2 +- src/modules/commander/state_machine_helper.h | 1 - 5 files changed, 27 insertions(+), 15 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index d3653ef54d..a53449ade0 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -1,5 +1,3 @@ -# If you change the order, add or remove arming_state_t states make sure to update the arrays -# in state_machine_helper.cpp as well. uint64 timestamp # time since system start (microseconds) uint8 ARMING_STATE_INIT = 0 diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index 593101fb57..2baaffdff9 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -60,6 +60,29 @@ events::px4::enums::arming_state_t ArmStateMachine::eventArmingState(uint8_t arm return events::px4::enums::arming_state_t::init; } +const char *ArmStateMachine::getArmingStateName(uint8_t arming_state) +{ + switch (arming_state) { + + case vehicle_status_s::ARMING_STATE_INIT: return "Init"; + + case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby"; + + case vehicle_status_s::ARMING_STATE_ARMED: return "Armed"; + + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error"; + + case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown"; + + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore"; + + default: return "Unknown"; + } + + static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, + "enum def mismatch"); +} + transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks, @@ -205,7 +228,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s if (!feedback_provided) { // FIXME: this catch-all does not provide helpful information to the user mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t", - arming_state_names[status.arming_state], arming_state_names[new_arming_state]); + getArmingStateName(status.arming_state), getArmingStateName(new_arming_state)); events::send( events::ID("commander_transition_denied"), events::Log::Critical, "Arming state transition denied: {1} to {2}", diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 55af4363de..43e59f25d0 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -52,6 +52,8 @@ public: ArmStateMachine() = default; ~ArmStateMachine() = default; + static const char *getArmingStateName(uint8_t arming_state); + transition_result_t arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, const arming_state_t new_arming_state, @@ -59,16 +61,6 @@ public: vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements, const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason); - // You can index into the array with an arming_state_t in order to get its textual representation - const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { - "INIT", - "STANDBY", - "ARMED", - "STANDBY_ERROR", - "SHUTDOWN", - "IN_AIR_RESTORE", - }; - private: static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5609b7d44e..f63ceff0f4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -479,7 +479,7 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::print_status() { - PX4_INFO("arming: %s", _arm_state_machine.arming_state_names[_status.arming_state]); + PX4_INFO("arming: %s", _arm_state_machine.getArmingStateName(_status.arming_state)); PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]); perf_print_counter(_loop_perf); perf_print_counter(_preflight_check_perf); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b0cf652e44..4f93b7f685 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -100,7 +100,6 @@ enum class position_nav_loss_actions_t { LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION. }; -extern const char *const arming_state_names[]; extern const char *const nav_state_names[]; enum RCLossExceptionBits { From 37c485ce89d4b4f6b401a67db80ced7d3bc5de79 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 May 2022 13:49:06 +0200 Subject: [PATCH 07/13] ArmStateMachine: move arm state into the class --- .../ArmStateMachine/ArmStateMachine.cpp | 111 +++++++++--------- .../ArmStateMachine/ArmStateMachine.hpp | 18 ++- .../ArmStateMachine/ArmStateMachineTest.cpp | 4 +- src/modules/commander/Commander.cpp | 40 +++---- 4 files changed, 90 insertions(+), 83 deletions(-) diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index 2baaffdff9..d7dfccc45e 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -38,51 +38,6 @@ constexpr bool ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]; -events::px4::enums::arming_state_t ArmStateMachine::eventArmingState(uint8_t arming_state) -{ - switch (arming_state) { - case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init; - - case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby; - - case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed; - - case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown; - - case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore; - } - - static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore, - "enum def mismatch"); - - return events::px4::enums::arming_state_t::init; -} - -const char *ArmStateMachine::getArmingStateName(uint8_t arming_state) -{ - switch (arming_state) { - - case vehicle_status_s::ARMING_STATE_INIT: return "Init"; - - case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby"; - - case vehicle_status_s::ARMING_STATE_ARMED: return "Armed"; - - case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error"; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown"; - - case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore"; - - default: return "Unknown"; - } - - static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, - "enum def mismatch"); -} - transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks, @@ -96,13 +51,12 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1"); transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status.arming_state; bool feedback_provided = false; const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON); /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_arming_state) { + if (new_arming_state == _arm_state) { ret = TRANSITION_NOT_CHANGED; } else { @@ -136,7 +90,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s if ((_last_preflight_check == 0) || (hrt_elapsed_time(&_last_preflight_check) > 1000 * 1000)) { status_flags.system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status, - status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, + status_flags, control_mode, false, !isArmed(), time_since_boot); _last_preflight_check = hrt_absolute_time(); @@ -144,7 +98,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s } // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status.arming_state]; + bool valid_transition = arming_transitions[new_arming_state][_arm_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. @@ -152,7 +106,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON - if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) { + if (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) { bool prearm_check_ret = true; @@ -177,8 +131,8 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s status_flags.system_sensors_initialized = true; /* recover from a prearm fail */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY; + if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + _arm_state = vehicle_status_s::ARMING_STATE_STANDBY; } // HIL can always go to standby @@ -189,7 +143,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s if (!hil_enabled && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { + (_arm_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { // Sensors need to be initialized for STANDBY state, except for HIL if (!status_flags.system_sensors_initialized) { @@ -205,7 +159,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY); ret = TRANSITION_CHANGED; - status.arming_state = new_arming_state; + _arm_state = new_arming_state; if (was_armed && !armed.armed) { // disarm transition status.latest_disarming_reason = (uint8_t)calling_reason; @@ -228,13 +182,58 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s if (!feedback_provided) { // FIXME: this catch-all does not provide helpful information to the user mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t", - getArmingStateName(status.arming_state), getArmingStateName(new_arming_state)); + getArmStateName(_arm_state), getArmStateName(new_arming_state)); events::send( events::ID("commander_transition_denied"), events::Log::Critical, "Arming state transition denied: {1} to {2}", - eventArmingState(status.arming_state), eventArmingState(new_arming_state)); + getArmStateEvent(_arm_state), getArmStateEvent(new_arming_state)); } } return ret; } + +const char *ArmStateMachine::getArmStateName(uint8_t arming_state) +{ + switch (arming_state) { + + case vehicle_status_s::ARMING_STATE_INIT: return "Init"; + + case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby"; + + case vehicle_status_s::ARMING_STATE_ARMED: return "Armed"; + + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error"; + + case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown"; + + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore"; + + default: return "Unknown"; + } + + static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, + "enum def mismatch"); +} + +events::px4::enums::arming_state_t ArmStateMachine::getArmStateEvent(uint8_t arming_state) +{ + switch (arming_state) { + case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init; + + case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby; + + case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed; + + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error; + + case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown; + + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore; + } + + static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore, + "enum def mismatch"); + + return events::px4::enums::arming_state_t::init; +} diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 43e59f25d0..999bcd864a 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -52,7 +52,7 @@ public: ArmStateMachine() = default; ~ArmStateMachine() = default; - static const char *getArmingStateName(uint8_t arming_state); + void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; } transition_result_t arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, @@ -61,9 +61,21 @@ public: vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements, const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason); -private: - static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state); + // Getters + uint8_t getArmState() const { return _arm_state; } + bool isInit() const { return (_arm_state == vehicle_status_s::ARMING_STATE_INIT); } + bool isStandby() const { return (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY); } + bool isArmed() const { return (_arm_state == vehicle_status_s::ARMING_STATE_ARMED); } + bool isShutdown() const { return (_arm_state == vehicle_status_s::ARMING_STATE_SHUTDOWN); } + + static const char *getArmStateName(uint8_t arming_state); + const char *getArmStateName() const { return getArmStateName(_arm_state); } + +private: + static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state); + + uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT}; hrt_abstime _last_preflight_check = 0; ///< initialize so it gets checked immediately // This array defines the arming state transitions. The rows are the new state, and the columns diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index a1251efe21..2527fec810 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -268,7 +268,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) PreFlightCheck::arm_requirements_t arm_req{}; // Setup initial machine state - status.arming_state = test->current_state.arming_state; + arm_state_machine.forceArmState(test->current_state.arming_state); status_flags.system_sensors_initialized = test->system_sensors_initialized; status.hil_state = test->hil_state; // The power status of the test unit is not relevant for the unit test @@ -297,7 +297,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) // Validate result of transition EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg; - EXPECT_EQ(status.arming_state, test->expected_state.arming_state) << test->assertMsg; + EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg; EXPECT_EQ(armed.armed, test->expected_state.armed) << test->assertMsg; EXPECT_EQ(armed.ready_to_arm, test->expected_state.ready_to_arm) << test->assertMsg; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f63ceff0f4..507eca31ca 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -479,7 +479,7 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::print_status() { - PX4_INFO("arming: %s", _arm_state_machine.getArmingStateName(_status.arming_state)); + PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]); perf_print_counter(_loop_perf); perf_print_counter(_preflight_check_perf); @@ -829,7 +829,6 @@ Commander::Commander() : // We want to accept RC inputs as default _status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; _status.nav_state_timestamp = hrt_absolute_time(); - _status.arming_state = vehicle_status_s::ARMING_STATE_INIT; /* mark all signals lost as long as they haven't been found */ _status.rc_signal_lost = true; @@ -1027,12 +1026,13 @@ Commander::handle_command(const vehicle_command_s &cmd) const bool forced = (static_cast(lroundf(cmd.param2)) == 21196); const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); - if (!forced) { - // Flick to in-air restore first if this comes from an onboard system and from IO - if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id - && cmd_from_io && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) { - _status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; - } + // Flick to in-air restore first if this comes from an onboard system and from IO + if (!forced && cmd_from_io + && (cmd.source_system == _status.system_id) + && (cmd.source_component == _status.component_id) + && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) { + // TODO: replace with a proper allowed transition + _arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE); } transition_result_t arming_res = TRANSITION_DENIED; @@ -1386,8 +1386,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) { + if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1506,9 +1505,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: { // Magnetometer quick calibration using world magnetic model and known heading - if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || (_status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) - || _worker_thread.isBusy()) { + if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1543,9 +1540,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { - if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN - || _worker_thread.isBusy()) { + if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -2455,7 +2450,7 @@ Commander::run() } /* If in INIT state, try to proceed to STANDBY state */ - if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) { _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, @@ -3099,6 +3094,7 @@ Commander::run() update_control_mode(); // vehicle_status publish (after prearm/preflight updates above) + _status.arming_state = _arm_state_machine.getArmState(); _status.timestamp = hrt_absolute_time(); _status_pub.publish(_status); @@ -3276,14 +3272,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) uint8_t led_color = led_control_s::COLOR_WHITE; bool set_normal_color = false; - uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms; + uint64_t overload_warn_delay = _arm_state_machine.isArmed() ? 1_ms : 250_ms; // set mode if (overload && (time_now_us >= _overload_start + overload_warn_delay)) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_PURPLE; - } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + } else if (_arm_state_machine.isArmed()) { led_mode = led_control_s::MODE_ON; set_normal_color = true; @@ -3291,7 +3287,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_RED; - } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + } else if (_arm_state_machine.isStandby()) { led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; @@ -3299,7 +3295,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + } else if (_arm_state_machine.isInit()) { // if in init status it should not be in the error state led_mode = led_control_s::MODE_OFF; @@ -4166,7 +4162,7 @@ void Commander::estimator_check() if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + if (!_arm_state_machine.isArmed()) { _nav_test_failed = false; _nav_test_passed = false; From 38e027ee459aacbf5a11c3737f536334b526356b Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 17 May 2022 16:09:58 +0200 Subject: [PATCH 08/13] ArmStateMachine: remove dependency on armed.armed To have the internal state as single source of truth for the arming state within Commander. --- .../ArmStateMachine/ArmStateMachine.cpp | 15 ++- .../ArmStateMachine/ArmStateMachineTest.cpp | 97 +++++++++---------- src/modules/commander/Commander.cpp | 87 +++++++++-------- 3 files changed, 99 insertions(+), 100 deletions(-) diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index d7dfccc45e..15400161d4 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -154,21 +154,20 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s // Finish up the state transition if (valid_transition) { - bool was_armed = armed.armed; - armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED); - armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY); ret = TRANSITION_CHANGED; - _arm_state = new_arming_state; - if (was_armed && !armed.armed) { // disarm transition + // Record arm/disarm reason + if (isArmed() && (new_arming_state != vehicle_status_s::ARMING_STATE_ARMED)) { // disarm transition status.latest_disarming_reason = (uint8_t)calling_reason; - } else if (!was_armed && armed.armed) { // arm transition + } else if (!isArmed() && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // arm transition status.latest_arming_reason = (uint8_t)calling_reason; } - if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // Switch state + _arm_state = new_arming_state; + + if (isArmed()) { status.armed_time = hrt_absolute_time(); } else { diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index 2527fec810..5da49a9a3a 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -44,7 +44,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) typedef struct { arming_state_t arming_state; // vehicle_status_s.arming_state bool armed; // actuator_armed_s.armed - bool ready_to_arm; // actuator_armed_s.ready_to_arm } ArmingTransitionVolatileState_t; // This structure represents a test case for arming_state_transition. It contains the machine @@ -66,8 +65,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) // We use these defines so that our test cases are more readable #define ATT_ARMED true #define ATT_DISARMED false -#define ATT_READY_TO_ARM true -#define ATT_NOT_READY_TO_ARM false #define ATT_SENSORS_INITIALIZED true #define ATT_SENSORS_NOT_INITIALIZED false #define ATT_SAFETY_AVAILABLE true @@ -81,9 +78,9 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "no transition: identical states", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED }, // TRANSITION_CHANGED tests @@ -92,97 +89,97 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "transition: init to standby", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: init to standby error", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: init to reboot", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby to init", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby to standby error", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: armed to standby", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby error to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: in air restore to armed", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED }, { "transition: in air restore to reboot", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, // hil on tests, standby error to standby not normally allowed { "transition: standby error to standby, hil on", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED }, // Safety switch arming tests { "transition: standby to armed, no safety switch", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED }, { "transition: standby to armed, safety switch off", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED }, // TRANSITION_DENIED tests @@ -191,51 +188,51 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "no transition: init to armed", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: armed to init", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED }, { "no transition: armed to reboot", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED }, { "no transition: standby error to armed", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: standby error to standby", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: reboot to armed", - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: in air restore to standby", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED }, // Sensor tests @@ -249,9 +246,9 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "no transition: init to armed, safety switch on", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED }, }; @@ -276,9 +273,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) safety.safety_switch_available = test->safety_switch_available; safety.safety_off = test->safety_off; - armed.armed = test->current_state.armed; - armed.ready_to_arm = test->current_state.ready_to_arm; - vehicle_control_mode_s control_mode{}; // Attempt transition @@ -298,7 +292,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) // Validate result of transition EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg; EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg; - EXPECT_EQ(armed.armed, test->expected_state.armed) << test->assertMsg; - EXPECT_EQ(armed.ready_to_arm, test->expected_state.ready_to_arm) << test->assertMsg; + EXPECT_EQ(arm_state_machine.isArmed(), test->expected_state.armed) << test->assertMsg; } } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 507eca31ca..7e73c56002 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -702,7 +702,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ run_preflight_checks = false; } - if (run_preflight_checks && !_armed.armed) { + if (run_preflight_checks && !_arm_state_machine.isArmed()) { if (_vehicle_control_mode.flag_control_manual_enabled) { if (_vehicle_control_mode.flag_control_climb_rate_enabled && !_status.rc_signal_lost && _is_throttle_above_center) { @@ -1630,7 +1630,7 @@ Commander::handle_command(const vehicle_command_s &cmd) unsigned Commander::handle_command_motor_test(const vehicle_command_s &cmd) { - if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { + if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; } @@ -1678,7 +1678,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd) unsigned Commander::handle_command_actuator_test(const vehicle_command_s &cmd) { - if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { + if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; } @@ -1758,7 +1758,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break; case action_request_s::ACTION_TOGGLE_ARMING: - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { disarm(arm_disarm_reason); } else { @@ -1803,7 +1803,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) // if there's never been a mode change force RC switch as initial state if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT - && !_armed.armed && (_internal_state.main_state_changes == 0) + && !_arm_state_machine.isArmed() && (_internal_state.main_state_changes == 0) && (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL || action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) { _internal_state.main_state = action_request.mode; @@ -2190,7 +2190,7 @@ Commander::run() _parameter_update_sub.copy(&update); /* update parameters */ - if (!_armed.armed) { + if (!_arm_state_machine.isArmed()) { updateParameters(); _status_changed = true; @@ -2244,7 +2244,7 @@ Commander::run() _vehicle_land_detected_sub.copy(&_vehicle_land_detected); // Only take actions if armed - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { if (!was_landed && _vehicle_land_detected.landed) { mavlink_log_info(&_mavlink_log_pub, "Landing detected\t"); events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected"); @@ -2288,7 +2288,7 @@ Commander::run() _safety.safety_switch_available, _status); // disarm if safety is now on and still armed - if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off + if (_arm_state_machine.isArmed() && _safety.safety_switch_available && !_safety.safety_off && (_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) { disarm(arm_disarm_reason_t::safety_button); } @@ -2387,7 +2387,7 @@ Commander::run() estimator_check(); // Auto disarm when landed or kill switch engaged - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { // Check for auto-disarm on landing or pre-flight if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { @@ -2505,7 +2505,7 @@ Commander::run() } // Transition main state to loiter or auto-mission after takeoff is completed. - if (_armed.armed && !_vehicle_land_detected.landed + if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || _status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) && (mission_result.timestamp >= _status.nav_state_timestamp) @@ -2529,7 +2529,7 @@ Commander::run() const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0; // Geofence actions - if (_armed.armed + if (_arm_state_machine.isArmed() && (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE) && !in_low_battery_failsafe_delay) { @@ -2629,7 +2629,7 @@ Commander::run() } /* Check for mission flight termination */ - if (_armed.armed && _mission_result_sub.get().flight_termination && + if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination && !_status_flags.circuit_breaker_flight_termination_disabled) { @@ -2679,7 +2679,8 @@ Commander::run() const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; - if (!_armed.armed && (is_mavlink || !_mode_switch_mapped) && (_internal_state.main_state_changes == 0)) { + if (!_arm_state_machine.isArmed() && (is_mavlink || !_mode_switch_mapped) + && (_internal_state.main_state_changes == 0)) { // if there's never been a mode change force position control as initial state _internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; _internal_state.main_state_changes++; @@ -2713,7 +2714,7 @@ Commander::run() // but only if actually in air. if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && !in_low_battery_failsafe_delay && !_geofence_warning_action_on - && _armed.armed + && _arm_state_machine.isArmed() && !_status_flags.rc_calibration_in_progress && manual_control_setpoint.valid && manual_control_setpoint.sticks_moving @@ -2750,7 +2751,7 @@ Commander::run() avoidance_check(); /* check if we are disarmed and there is a better mode to wait in */ - if (!_armed.armed) { + if (!_arm_state_machine.isArmed()) { /* if there is no radio control but GPS lock the user might want to fly using * just a tablet. Since the RC will force its mode switch setting on connecting * we can as well just wait in a hold mode which enables tablet control. @@ -2800,7 +2801,7 @@ Commander::run() auto fd_status_flags = _failure_detector.getStatusFlags(); _status_changed = true; - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { if (fd_status_flags.arm_escs) { // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { @@ -2952,7 +2953,7 @@ Commander::run() // automatically set or update home position if (_param_com_home_en.get() && !_home_pub.get().manual_home) { - if (!_armed.armed && _vehicle_land_detected.landed) { + if (!_arm_state_machine.isArmed() && _vehicle_land_detected.landed) { const bool can_set_home_lpos_first_time = (!_home_pub.get().valid_lpos && _status_flags.local_position_valid); const bool can_set_home_gpos_first_time = ((!_home_pub.get().valid_hpos || !_home_pub.get().valid_alt) && (_status_flags.global_position_valid || _status_flags.gps_position_valid)); @@ -2968,10 +2969,10 @@ Commander::run() } // check for arming state change - if (_was_armed != _armed.armed) { + if (_was_armed != _arm_state_machine.isArmed()) { _status_changed = true; - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { if (!_vehicle_land_detected.landed) { // check if takeoff already detected upon arming _have_taken_off_since_arming = true; } @@ -2985,7 +2986,7 @@ Commander::run() } } - if (!_armed.armed) { + if (!_arm_state_machine.isArmed()) { /* Reset the flag if disarmed. */ _have_taken_off_since_arming = false; _imbalanced_propeller_check_triggered = false; @@ -3067,7 +3068,7 @@ Commander::run() || !(_armed == actuator_armed_prev)) { // Evaluate current prearm status (skip during arm -> disarm transition) - if (!actuator_armed_prev.armed && !_armed.armed && !_status_flags.calibration_enabled) { + if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_status_flags.calibration_enabled) { _status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); @@ -3087,6 +3088,8 @@ Commander::run() } // publish actuator_armed first (used by output modules) + _armed.armed = _arm_state_machine.isArmed(); + _armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby(); _armed.timestamp = hrt_absolute_time(); _armed_pub.publish(_armed); @@ -3123,7 +3126,7 @@ Commander::run() } /* play arming and battery warning tunes */ - if (!_arm_tune_played && _armed.armed && + if (!_arm_tune_played && _arm_state_machine.isArmed() && (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) { /* play tune when armed */ @@ -3141,7 +3144,7 @@ Commander::run() /* play tune on battery warning */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); - } else if (_status.failsafe && _armed.armed) { + } else if (_status.failsafe && _arm_state_machine.isArmed()) { tune_failsafe(true); } else { @@ -3149,7 +3152,7 @@ Commander::run() } /* reset arm_tune_played when disarmed */ - if (!_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { + if (!_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { // Notify the user that it is safe to approach the vehicle if (_arm_tune_played) { @@ -3192,11 +3195,11 @@ Commander::run() _last_local_position_valid = _status_flags.local_position_valid; _last_global_position_valid = _status_flags.global_position_valid; - _was_armed = _armed.armed; + _was_armed = _arm_state_machine.isArmed(); arm_auth_update(now, params_updated); - px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed); + px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed()); perf_end(_loop_perf); @@ -3335,7 +3338,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS) - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { if (_status.failsafe) { BOARD_ARMED_LED_OFF(); @@ -3351,7 +3354,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) BOARD_ARMED_LED_ON(); } - } else if (_armed.ready_to_arm) { + } else if (_arm_state_machine.isStandby()) { BOARD_ARMED_LED_OFF(); // ready to arm, blink at 1Hz @@ -3426,7 +3429,7 @@ Commander::update_control_mode() _vehicle_control_mode = {}; /* set vehicle_control_mode according to set_navigation_state */ - _vehicle_control_mode.flag_armed = _armed.armed; + _vehicle_control_mode.flag_armed = _arm_state_machine.isArmed(); switch (_status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: @@ -3575,7 +3578,7 @@ Commander::print_reject_mode(uint8_t main_state) /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ - tune_negative(_armed.armed); + tune_negative(_arm_state_machine.isArmed()); _last_print_mode_reject_time = hrt_absolute_time(); } @@ -3707,7 +3710,7 @@ void Commander::data_link_check() events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained"); } - if (!_armed.armed && !_status_flags.calibration_enabled) { + if (!_arm_state_machine.isArmed() && !_status_flags.calibration_enabled) { // make sure to report preflight check failures to a connecting GCS PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode, true, false, hrt_elapsed_time(&_boot_timestamp)); @@ -3881,7 +3884,7 @@ void Commander::battery_status_check() battery_required_count++; } - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { if ((_last_connected_batteries & (1 << index)) && !battery.connected) { mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1); @@ -3927,9 +3930,11 @@ void Commander::battery_status_check() fault_index++) { if (battery.faults & (1 << fault_index)) { mavlink_log_emergency(&_mavlink_log_pub, "Battery %d: %s. %s \t", index + 1, - battery_fault_reason_str(static_cast(fault_index)), _armed.armed ? "Land now!" : ""); + battery_fault_reason_str(static_cast(fault_index)), + _arm_state_machine.isArmed() ? "Land now!" : ""); - events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land : + events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ? + events::px4::enums::suggested_action_t::land : events::px4::enums::suggested_action_t::none; /* EVENT @@ -3963,7 +3968,7 @@ void Commander::battery_status_check() if (_rtl_time_estimate_sub.copy(&rtl_time_estimate) && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s && rtl_time_estimate.valid - && _armed.armed + && _arm_state_machine.isArmed() && !_vehicle_land_detected.ground_contact // not in any landing stage && !_rtl_time_actions_done && PX4_ISFINITE(worst_battery_time_s) @@ -3989,7 +3994,7 @@ void Commander::battery_status_check() bool battery_warning_level_increased_while_armed = false; bool update_internal_battery_state = false; - if (_armed.armed) { + if (_arm_state_machine.isArmed()) { if (worst_warning > _battery_warning) { battery_warning_level_increased_while_armed = true; update_internal_battery_state = true; @@ -4226,7 +4231,7 @@ void Commander::estimator_check() bool xy_valid = lpos.xy_valid && !_nav_test_failed; bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; - if (!_armed.armed) { + if (!_arm_state_machine.isArmed()) { if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) { xy_valid = false; } @@ -4417,7 +4422,8 @@ void Commander::esc_status_check() if ((esc_status.esc_online_flags & (1 << index)) == 0) { snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1); esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; - events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land : + events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ? + events::px4::enums::suggested_action_t::land : events::px4::enums::suggested_action_t::none; // TODO: use esc_status.esc[index].actuator_function as index after SYS_CTRL_ALLOC becomes default events::send(events::ID("commander_esc_offline"), @@ -4425,7 +4431,8 @@ void Commander::esc_status_check() } } - mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : ""); + mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, + _arm_state_machine.isArmed() ? "Land now!" : ""); _last_esc_online_flags = esc_status.esc_online_flags; _status_flags.escs_error = true; @@ -4459,7 +4466,7 @@ void Commander::esc_status_check() } mavlink_log_emergency(&_mavlink_log_pub, "ESC%d: %s. %s \t", index + 1, - esc_fault_reason_str(fault_reason_index), _armed.armed ? user_action : ""); + esc_fault_reason_str(fault_reason_index), _arm_state_machine.isArmed() ? user_action : ""); events::send (events::ID("commander_esc_fault"), {events::Log::Emergency, events::LogInternal::Warning}, From 04071b9456ea6129717bc197b37c7fdd1224ac89 Mon Sep 17 00:00:00 2001 From: Nico van Duijn Date: Thu, 19 May 2022 14:53:45 +0200 Subject: [PATCH 09/13] Commander: ignore MAV_CMD_REQUEST_MSG This commit adds the MAV_CMD_REQUEST_MESSAGE to the list of vehicle commands which are ignored without generating a warning sound. --- src/modules/commander/Commander.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7e73c56002..9e3235ca93 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1609,6 +1609,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: + case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE: /* ignore commands that are handled by other parts of the system */ break; From a0cb7f62584cc7f20434020502a9a328ee379035 Mon Sep 17 00:00:00 2001 From: Nicolas MARTIN Date: Fri, 20 May 2022 12:02:42 +0200 Subject: [PATCH 10/13] HITL: undefined time_remaining_s should be NAN --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c689e7a0b3..5983c00170 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2323,6 +2323,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; hil_battery_status.remaining = 0.70; + hil_battery_status.time_remaining_s = NAN; _battery_pub.publish(hil_battery_status); } @@ -2752,6 +2753,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.timestamp = hrt_absolute_time(); + hil_battery_status.time_remaining_s = NAN; _battery_pub.publish(hil_battery_status); } } From 33a77c225c888848512bce798c51e81f3c930d5a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 20 May 2022 16:26:11 +1200 Subject: [PATCH 11/13] commander: lockdown is not termination We use lockdown to prevent outputs like motors and servos from being active in HITL simulation. This means that we can't treat the lockdown flag as a flight_terminated, otherwise we can't arm in HITL at all. --- src/modules/commander/Commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 9e3235ca93..4dacd0bdc3 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2929,7 +2929,7 @@ Commander::run() checkWindSpeedThresholds(); } - _status_flags.flight_terminated = _armed.force_failsafe || _armed.lockdown || _armed.manual_lockdown; + _status_flags.flight_terminated = _armed.force_failsafe || _armed.manual_lockdown; /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); From 9d0e57230a032a537123866f6eb71a1827649921 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 20 May 2022 12:49:06 -0700 Subject: [PATCH 12/13] boards: new px4_fmu-v6c board support (#19544) --- .ci/Jenkinsfile-compile | 1 + .github/workflows/compile_nuttx.yml | 1 + Makefile | 1 + boards/px4/fmu-v6c/bootloader.px4board | 3 + boards/px4/fmu-v6c/default.px4board | 91 ++++ .../fmu-v6c/extras/px4_fmu-v6c_bootloader.bin | Bin 0 -> 45684 bytes .../px4/fmu-v6c/extras/px4_io-v2_default.bin | Bin 0 -> 39952 bytes boards/px4/fmu-v6c/firmware.prototype | 13 + boards/px4/fmu-v6c/init/rc.board_defaults | 20 + boards/px4/fmu-v6c/init/rc.board_sensors | 21 + boards/px4/fmu-v6c/nuttx-config/Kconfig | 17 + .../fmu-v6c/nuttx-config/bootloader/defconfig | 96 ++++ .../px4/fmu-v6c/nuttx-config/include/board.h | 505 ++++++++++++++++++ .../nuttx-config/include/board_dma_map.h | 79 +++ boards/px4/fmu-v6c/nuttx-config/nsh/defconfig | 241 +++++++++ .../nuttx-config/scripts/bootloader_script.ld | 215 ++++++++ .../fmu-v6c/nuttx-config/scripts/script.ld | 223 ++++++++ boards/px4/fmu-v6c/src/CMakeLists.txt | 75 +++ boards/px4/fmu-v6c/src/board_config.h | 321 +++++++++++ boards/px4/fmu-v6c/src/bootloader_main.c | 62 +++ boards/px4/fmu-v6c/src/can.c | 128 +++++ boards/px4/fmu-v6c/src/hw_config.h | 128 +++++ boards/px4/fmu-v6c/src/i2c.cpp | 40 ++ boards/px4/fmu-v6c/src/init.c | 259 +++++++++ boards/px4/fmu-v6c/src/led.c | 235 ++++++++ boards/px4/fmu-v6c/src/manifest.c | 147 +++++ boards/px4/fmu-v6c/src/mtd.cpp | 104 ++++ boards/px4/fmu-v6c/src/sdio.c | 177 ++++++ boards/px4/fmu-v6c/src/spi.cpp | 61 +++ boards/px4/fmu-v6c/src/timer_config.cpp | 73 +++ boards/px4/fmu-v6c/src/usb.c | 105 ++++ .../include/px4_arch/hw_description.h | 18 +- .../stm32h7/include/px4_arch/hw_description.h | 2 + .../px4_arch/io_timer_hw_description.h | 24 + 34 files changed, 3485 insertions(+), 1 deletion(-) create mode 100644 boards/px4/fmu-v6c/bootloader.px4board create mode 100644 boards/px4/fmu-v6c/default.px4board create mode 100755 boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin create mode 100755 boards/px4/fmu-v6c/extras/px4_io-v2_default.bin create mode 100644 boards/px4/fmu-v6c/firmware.prototype create mode 100644 boards/px4/fmu-v6c/init/rc.board_defaults create mode 100644 boards/px4/fmu-v6c/init/rc.board_sensors create mode 100644 boards/px4/fmu-v6c/nuttx-config/Kconfig create mode 100644 boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig create mode 100644 boards/px4/fmu-v6c/nuttx-config/include/board.h create mode 100644 boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h create mode 100644 boards/px4/fmu-v6c/nuttx-config/nsh/defconfig create mode 100644 boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld create mode 100644 boards/px4/fmu-v6c/nuttx-config/scripts/script.ld create mode 100644 boards/px4/fmu-v6c/src/CMakeLists.txt create mode 100644 boards/px4/fmu-v6c/src/board_config.h create mode 100644 boards/px4/fmu-v6c/src/bootloader_main.c create mode 100644 boards/px4/fmu-v6c/src/can.c create mode 100644 boards/px4/fmu-v6c/src/hw_config.h create mode 100644 boards/px4/fmu-v6c/src/i2c.cpp create mode 100644 boards/px4/fmu-v6c/src/init.c create mode 100644 boards/px4/fmu-v6c/src/led.c create mode 100644 boards/px4/fmu-v6c/src/manifest.c create mode 100644 boards/px4/fmu-v6c/src/mtd.cpp create mode 100644 boards/px4/fmu-v6c/src/sdio.c create mode 100644 boards/px4/fmu-v6c/src/spi.cpp create mode 100644 boards/px4/fmu-v6c/src/timer_config.cpp create mode 100644 boards/px4/fmu-v6c/src/usb.c diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index a3e67d3eed..4e61920c58 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -105,6 +105,7 @@ pipeline { "px4_fmu-v5_stackcheck", "px4_fmu-v5_uavcanv0periph", "px4_fmu-v5x_default", + "px4_fmu-v6c_default", "px4_fmu-v6u_default", "px4_fmu-v6x_default", "px4_io-v2_default", diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index a54ac9337b..f871cd60a6 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -61,6 +61,7 @@ jobs: px4_fmu-v4pro, px4_fmu-v5, px4_fmu-v5x, + px4_fmu-v6c, px4_fmu-v6u, px4_fmu-v6x, raspberrypi_pico, diff --git a/Makefile b/Makefile index d43c96123e..0378c834a4 100644 --- a/Makefile +++ b/Makefile @@ -325,6 +325,7 @@ px4io_update: px4_io-v2_default cubepilot_io-v2_default cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5/extras/px4_io-v2_default.bin cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v5x/extras/px4_io-v2_default.bin cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6x/extras/px4_io-v2_default.bin + cp build/px4_io-v2_default/px4_io-v2_default.bin boards/px4/fmu-v6c/extras/px4_io-v2_default.bin # cubepilot_io-v2_default cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin diff --git a/boards/px4/fmu-v6c/bootloader.px4board b/boards/px4/fmu-v6c/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/px4/fmu-v6c/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board new file mode 100644 index 0000000000..6c491152a0 --- /dev/null +++ b/boards/px4/fmu-v6c/default.px4board @@ -0,0 +1,91 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI055=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PWM_OUT_SIM=y +CONFIG_DRIVERS_PX4IO=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL_L1=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MIXER=y +CONFIG_SYSTEMCMDS_MOTOR_TEST=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_PWM=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin b/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin new file mode 100755 index 0000000000000000000000000000000000000000..fa26776c8725f663d06c97a42cac24715dfde946 GIT binary patch literal 45684 zcmeFZdw5jU^)S5lIcH{0k|CL7f=(`wGjjq?F5o0V8$o|gX2Qv3&e?)$#( zol9YzZ#L#qn4U}Fgsnf29+H@l!dwccHqZOq^z6K$NpGFBR{EIK^Z-WVR;k5h?t#!+ z-GfFzigm~c*3V6e)*+nIHaO|sbzE%Qw#U$Xwalq>) z-Gs|}S6xQ9Bsbxb-c@h4K(3F4TmZ;59s^!sA$P$g7}#|Q2JZH3Ys*w@5ueQ!@fByv z->OD?&;iJY@GpGmAQU*m`+du*msgn;G15>g;uO}6dDbi^DPm-x){UKP(zSHNSDPf$ z@inkC-z@L_BE463d&;Zriw>-IS7+9`s}IyNqfnf=##Mc8b*AFNN0w#EnaUnC*_)}D z&}8p|YO}H*CVLlEn-w?Kvu61gPp0CEI9)L~ZnF30szhZMH`#krRifg;43j7?h@}vd zNL%72dy70aTc+&rWGXgnU|g7GXk7-EGM4=DU_oVJHLHjb=W=%hma}x+5$6VX1U8uE zxta9w;lrzNM)$QmheYYFPwQ5zUV{i&ACUmvdRB z%(OLBWGcJa+ehy^h7O>Gcd1a$tROe;21_}!BD<9=ro5c+g>J$Z zrdFqUQk5jPSx$}RuT)mb2b2Tya>5n530HVdIVba-&_(CMZ-u3FTn}EF9tO}*VL7v+ zubf$NzMNSxP|mE-5SRwbnH3Ec+C|5SOX1Y*Ao^^V!lvd&ezz^0+R_|0wNTof>A9{t zOG#~EBfdu){irC_m)7#A|202|wDi;V?cvmxqkc|syaD*WSF8N!md{nlztrQ7_%`UP z%`y^)RhXyth!NigrrIojqB`DasDLi0WLD6b&N}TYvIu z>#QOUBXprX(ap%TT}P*Oaa$YMva8xgAK#MH2R7H`{H1H`seNEev|IYX#=lM}2JJxy zcet334vjwEFr=b`s08gDRpYtnqcx>}znXU?mUnz~-4@iJSo+bwKz_RbeS}K?@&)8? zHz2siBlI4yWy*R=0TC!Mf5AH_zG^PPK>Ft}kYYCliat6NGI2SDiRamn>}E$-xszlh zvg2GNT?Qe0;PyhkFMEljG`rjw%Li` zFV-BU&eSElVtJpaCJ$#%lG&g);wv%Br&XPu33?+?LR5V$+;Gw;Lu0j|0 zHyGgm1_Q>w!GP&+Fp&E>444LBV3vorJ*&zT$WmCG!mi9_L#5)Xl7mo2xTGDfiQx5~ z>#HnFGGs6I6=kfl%KKL>M>DJAvs>eK5iV=5Ya*EH0b6QSl8`9_?ivSMN);;rer;S- zM7+iA#sh9q#v)zO)xR@!LKimx13o&Y&tU)n_-y+7DGX5B_;CX84<`U`4){d$Qk}#} zT6C!xTzzaFrSfMDuOT+exEOYuAlS-t*>O2>QcoV0x0 z^6>}y4<6fm#(3t#Z6Br;f-W&7J!7_LhpuB`@Ie5b0L&5e26V>XxqP`FFq^z8+?@Ql z=geR-ruo~*c38c{!Iy6Yo z44PsO4HA~8DZ`;bVxVU>NjWq~QaT554ue?+^7$_4yf#9#@ISxB=l%a>e)jyE`sp0$ z`~<=*@(!EbYr;8h?{Iqg{o$N)@9@mU4}^0Tdxx)+9t`J5-r?&jlyFXkci2){7tX2l z4rf$76wayg4rf-^hjXgE!&#oS;T#Vgzjru$i9ne5;yC-@9GwrB|I>bouWNnL-@_q} zc}=}GzyG(@;PSOAjmKzxx^CWKvv{qY_&RdT=PE!?0`LO~!1Dowe>{N%z^_gqfe!#a zF;N=ONsuc@Xh8xFk0ZQeoG*+9VIJ477di(^od`evMro&Zkq^*)t3_4qIxAb5fd6N_ zbC7oe*1ZZ{FaHL35E?2_CBpTuL05v20eY4!zY~d&yZnxl25;oVBg;Ix(H9z<153SD z$u8Y0Zm`dUC>hEw*W&Xw1#)M|S<2kX=Y4rw0J|r0D-nKs!eX^mBK*qaQRSe* z$~l#QYbK5=$d>tVo+sPKiX{yk{-5a^JV}1QPfX19B>53OI5x+V6CJ6t}n4fpE`^3I>_blTB7rKBc-d?Sm8OY67c^ff<$>8bgeoBe9e(K9EE|^(^p~s z`UT94WZ=Jf1o&Us8v;7XC}s9t3a38x)e}6sLyIn*vPp&|eC3sPC%MT9BkzDqekz>W z0zyOQdOQ;*ROtG9GVr$=Aoo{xK49!is9r`Kj(hyfNH?=g*;k?WWGZ@3CY5SzI?oj~ zQ=)Y!JjDf_FRlNdcy5UmRcg_uY5557I*%!ktrYqZ<|a%X7j~eM3(b_@(soJ%>7H!m zeLuoi#@jkBg;SsWD*rw)HroLO3Fkq)(464OQkHjo-cIES5K24IqJ!-hnxSAccu7S# zG9FT)i?&PGtRe7J8z*@qCmNEa4~Dp67T-Ccliu|k_dnIl>`vN`b|>!tXoxF@-BtaJKowG|U7AFKr-`3D+mjYu(C*anxE9!cj#D&$Ifc|;dlPG;Kgcg99A!*SMG_K9m znG-NP&pfAp{(~V{V3rak#;QfTVKfVpvQc4`0j{L3V|^AN6`;AR*1TdXu~ooMNx3o)pQTprKE%K6Tq_xH+x3E@j#hmmjJ&<==1OxCprCw zDmwF@Ou>!kduZ-2#wcZW4&tYw>n1bsl-}nJVegY|9G!g{yZ0*X{hQqSF)7{sF)7{p zC#i#g074&uHdI{UO>4hRIpUhpU#d_J@%Xr_N+*H+j^LGcN|ozz5;zA?{%y*uUC4%R zo9Xlpzhg`qc_;ZEa#Ib=yD9i*0?sS#eFPjY5fGj)f0=N?;jOeTbC4B&MoI;76?>4L zRzD-0sdiQC4(nqgKFO|nEKf4KxllRN+6?&ZF+uv(V+ijW)1oJ)ZB>HA;?yZlY<*0N zzBX-_baci)LdD3D*8XdCp#mY!=)IyfErW$J*shm&Wt%Id|85C{H19%%FJeYcKhid` zIwh%pg~VdR_`3vZwyBFraYG>~9zeyPsS6e4C>rD5M+=t{{#N5K!{|odHZCT`G`En> z;+#tP2E{Ms83BHjK$$8DSPqj zg%Y!%RML-ZM+m2n>m~2-_5`rIdtf9X@x>6Ia9rYU>n3o>DY5w66es!1%Otd*Mq=@M zQ+JhrxHvNiLN(!xe;{jCv9b9(FQbfal3W0Lu!qInQ^@f|y}tbCuIo;t1xpr(g6pv! zhExyXyx$~T4lkkIEdK43PeS_zi4P&3#Xp}aFAu8g@k;2WI|ojyjYohtUAq(f8_ghm zb}W9IF?E&(OrC5-kO2RA;w)KXMS}A(N?k?-ho_@;$NhCIW?80y52i{-DgYY^aPZE{xe@+4$ANaJL^;ji~0{DL>1PS496Ix~paMB9DPZ9ipe@PB0EY2Sj zq|QN;lf`p7=ooa8H}X^ii*Fv|q*DI^fyG5*hh%$)AZgJ7jm}6nfNWr8@lBH;yeyY? zwv8EuW(9;yAF{1{xS^snuwBtf3;ea4PyzQLT2%Wmh)3npPCB#u$GDIH9DaU+(%;P! zwAFz250#F~rw9%X@0f^>2k`G+|7Wi6i`=8HgQ&08S7u14;2npEX4zJvlO(5b3mMjjPMI!5?atS?O8Y)ZyEqYm_JavtW#fQc@*^9LW zf@JC>f$OCVk6toLvlK0gCsX|OYZu99=@^^A7Q2sbdn9%r-4-^rF!NJ;*!k%_XnuO| zp|w^e(G9r|t?7^5N&%-5E&9=RDj%I0CbjC)>2> zYm;l&QtozS@>4hGWS`6J(4ucn3X-nP(a9bG+(6vfNHX=HUF;DSZ<;{+DYThzlEaT? z(eyz=S8zUI4?@A;gIe?#lSugZYColcW+irl*?hvKFr9-hoEn=02=~@)^xHN)>(U3= zP&#sS@DhuUjdKHnl&geP79SsvpJ{?r+Gz@~Pc+lBaQnaSl;kQFZydk2SC|50@awe) zx~gFn6?`6~G|k}GyP#{Q5puU1>3OS@P%f1SS)4f`bf5)7VKf+DnbOYE04G6ciH?@a z<24;z=&o3L<#=g_7i-0*hEj#nPQdcy=PKa)Cb1f?VT?gljN3cX7f||ejaxb>T;s%L zRg3;qi>UL4aM=tgp^=mFj+Z-Fe106+4N?;wKb2tqjU%)+U1`@Bqy_UDxlkpr_?>at zU!Vxmh5PFj@c|H9*8qM^i^(%{j}Oq_7nHolt29n=-yFw1)uhzcnf;bcEiOKIx}9oQ zO8oiCO8Ft>fIOgPDOd&EF-`n;RkBze6FQbxviQOn*iZKqC~mo4X_miUCgDQq=<{|K z|8|T@BCo4oKablhue7uH^|9DU?Y%*hz~ISgu($Rgk=^itszo23$ZI^LP&rmFIgdLP z`*FZePk`Oivq5po?aDSetQ^#HXY(47xWj37H_HXeNx5EOSABhtTxbcReXff4IaiC` zIdO@~N?*ZoW)-F6XQo@5^aXtI-?5vdeP_Idw9uM2@;&I>dOTC_kU9tP*7Z0^e@1O> z(iN~l&Pv-D9cyhe6kG}xH1Y)(gEZ%@u|f5pG&e5;ctZf~2E2C6?nnFCw)+S-qD4pF zT#p%uy-iTrf|H(X^I~5Dgq9>i=q{e}1N4haVbjx({O*}>YRl7MQ_JBU=%*jIr$yiq zuIK4+YD@gYu94V1EH;d>2lNuyFZcX%2fL>%0uTSPxlCelKRHXv+X9MbSf{&x4Xw37 z(T9wPJ>Ym^Uj^-jIY8wZx?^cSF|MPIYrpB-Hk`!FbUrzpq`Q9& zi(e<~!8i}nqW?4HkO2Q>{MTwjMO@a^qEC)fJY28^Jg0+l2cgn~$D zsMvl4_|m|W%1S?onG_nfx-w$Wx==RUtGp58Azxj2TKZ`_ixbAE1`%@21GLS*231PM zUn76BSSA1DPS8dBoi~KtPbrXVy2Fc^gzTFSNl&&pBo<#J^~whSjg9CFEeqAlJ^zpS*f0oVxX^b8g=8$96O37=4c$o7m=N zs_mpWq`xMqp(57-SbWV8U?iV{_AvMk4McPRSuGNWcaNh3AnL71;P8%dnysL*kAWTv{-c=cp z^3|_jjpin95nGVd2iQE$i3Y16iQ55>jm1}}w9{Y(+%geW|C6)@c-GXl6Xfg1+uWop z&z8?HWoKLq9Uoxy7-byB4tLb9RoQM+WhsYqh* z`1CPXG}tXL_`XRhyFsX1$J}uJoi{uxNR|Z@VrcqTu4vFHFnGyT2&WEiFx`2>20=2# zAo^kuK7qlDu0r^9%nkgPApQ`8Af0QCkh{?cxtooUd+pq82-FA+o;}IM##W=_?i8f- zSi3Jzzw9ExhEC|B^-vXqTi=YQ=M53}Oz9idId+3|nt(4^>LdIv_CqQkbq}ME;cwF4Qo^q%2h^H?V>3FSRHrzO_dCuIX%#kQW=+PQr`S}=|^ArX*+{q znsPULhecqpYl8P9k$u$Ed2L-y0rvavJ(#&L|l?I%zp6GO;74*sM+c2DTUQEb{VWrJW4kKQ*qJV)~bDO{TzdQf%TR2EQ-` zV(Vj6=S0gG57pMvNbJ*B zFC(H*><63zNX+oF_SWl-UIL?OW5A(_H;bhjm)H^KktY}x*Ja(K93&Sj| zA7+!$I3IQo16v@k@8KfOlH;Sk+t>CSABEeEU5o(2hyvq0P9A1ii+fOhBLpd?ph){n z_)4GM3FB(b zo|4L5(SSipy94c>uW-1Qp#7XwDrwPgPbbs5ehz<^aPCjrm-v&G=tCR9sucO@y{|yo zSR%@78}7_gHWtOSBTm3Q9skrZQv1+W7vIB`qR?4(Dd7rsxSWK$aV$vB${A8~Jtw6~ zhMVSKgKkGhp2Q0bX2%ZI9;)BxAE?cf?ym*>gR#~fAh;vG<*m)7o$mUhu1h_DUm~sb zP%s?)RHc@H%c^!UVF@1)=?(L1Y+M{ZJ0>U_9e_8F(fNWcsfrIIc`{|6m{^S#Q9Is5 zFd?QJzh{cY2@`-Hout@8*xdzPm)`vCU+G+^FT~Otsa)AMW;D^;;2Pc14ztIiI4}Q0 zJaqXVLT&A-p=1DDJoc##tagdP4@_RDUtMo5uYFLgqaGNi2yu9^Z^J1v=~$1iV^5Li z9Uz_>swu%@>w~48ln*de5-zoL#pGBWta8I1E?7G zTvpKmRE+jSROq61+lD~W9fb;ucWagYggd#=MkrG9{95!DjjmaV+|Ze)bPhH+Hx>aV zug2Gl_7l^l)C>OB9sL~YW$--Gxb51bRXe7mWsO`U_u)qNVYrnKHyQ#JP;%;4)cR%&s$`>!zX&EC3VAFq{Ts@wJ3hG zhY+Fcfo%_LTmSY0J!Dc~9^|XL2@!m@Z4Y{}?~!gYDNtC-Bk9`FahA;mex}C}$X5PW z8-vlfPT@iu5jvtp-<`f@XRTc)Nb&p2S9b`5)iIsix(*$&?Y*#$*(XSv;|=nY0;as!g9Nmiw)eW ztltkMfAis5z-&yz)KGCUTf{88)Q`%on1$|4UA2H&u#`8um|#PNB8{qU!WBeSO20_* zVg&GULq(!Q(@ZR_UBm(Rx~ozv^>z4?Zs`o!WlB_R7y*Nkj+f7n?eZD2v*BSLjZ`52 z#FfyRys{5bku3twDqg?{&X8S#Nv6-Byq7eY=2W9RW*C^#pexe7dN0`m33jH3x8D@} z<#AWUw=C8Jg@?VwCznxQn5D`7vIs0KyPCUI-b!;Hp6fqDo|kEv@8PX-zWWT><@mHc zQ>Ic3?tClkVyrj?IV0dq;1Z0f?hcj7$u0+ak-pnY(B9U1X0Nzbx7XnLV2HgLaL+`ElrM2rarpl|L9Hjy<>XQ! zArE**AQkhvr`yscV^7-J*0t=&GPZRM7diqg-Z4$}MO%q=;643l@7}d(Yt7}W*S4;K z{D1VEjrDmp)~9X{+Iyj%*~`-&Zvy<&iQ2U`Nnb@vPYs!FDxkW#2}7IXOZ}PxNYM|VhoW|y%ir{FAkL+H7e4Iw763&d08RpAtQ)e*mgQ7ztmbC zCA|xYjJTaNIrb_h>15bv-cEduy-I(|cCyvM;KK1!WINa;E&7A$v+8!zWEzg0!!N3I z?$41Hn2IjZz(!N4F%9>0bfBX&uy~!mDqs3BX@Tiz-xw!ZI%s+-S{;+R^9tyF+`TSc z+Do=N3jFN4gLO7PSoc+kl0`CgBI{VJ{WxyP_%+;GGATc&ck(^tgd<%l@tY(qI;@%e zg2Z56J4<#6XX*~Nn{jf=3*>PJ>PZ&Ot10|xvdfe%y;paz{V;jl(XTi3n#82lr%4-_ z{8X0vi*|xM?%1a{_nsh)!ft&^FKzMTX_}j<*V9}_mY#{_{(PF+7HJ=sVic5Y%UnII zmIlC9zHU3&EKvP`Z;n~|SHGmQWLM1#q){kP#7N%;sv{_nY2MM8FX2)7y_mi^wv$X* zp8YN^=*7OW%*v_gvr}|l)VFU2YnIQF=Rk`#O`IkBK#M*;ah5y}ykCnxI&qe~>{#xP z51+hF00)uYtfZg=jz6=+>K{Z4@~g|(&#|qcCwky0*?5(&=DQr zs7@ZBwK`-W(oFZ+2_MRng zIGDYHsN0)zb;tjl!hqA3C-II`hjR&0WMz)g029}#(*?W{f)tEZFf$G^0g|b+E zL2h4vJF*#bv#PiPZi2bIpbO@3b6W1Tlv_OP2$QfyZGFd%E`tTSA6J1_;!TG z*>G!3vy$Y0TiI7ZZQZxqXY?EjRkBqysMaPo^=IH?~f=q$#AttR%;g|4=;BsQb7=ln> z=`xCZSGs6+C&{Kx`kpObQ2}w_R#qs!_J)diL%1S!3CvCPdT}=YOQ~H-7}1-z6F6j( z4mIA_h;G(lgN~ojjXsfaw=xy2*97T_yG;xBRiu+-<5aYN(lr0?!MIJE;>L#{w>!+( zjH0z_xf=)Gx4d`n{nU!E9p?IOxD+-$^~mq2w)m;AsU_piS>-=-G3UKlD=`e@pzDdg zo8IJdf@sd@H{&|4d$TipQkHNdtl5~$O)#lDm5u)U8oS${ZDdAznfsN~?Ym(yu>lBZ z)c43LrKZLr;Z9U?S_N@TjpK-p%MmDVG+NU<95yg$f+^mutn%+|yhC9}&a#l(-+llV z6Vb_x`ZiQ6&)0mt)eLO??My@5!;<3Obi&=3oy%2Ur8beun+Fw31;s(biSowmx!fHb zHgNh0?ygUj7o>(aCJ!i>+@`V)N>Vuikv||n)9+`9M^?2TtAmgJ#&YX^`Ev+Z=|wkwpj1B z`Q;KzIqocyhE#TR&jzBRn+)?l3&!algpg_qbo)E5S($U=XDh8uEpVQugr<{WFk8y0 z4ePDVN$!&yuW7yMJ+M1GgX#ggc%FB%n^%R)Sau_w&82Xv@2fW(L$z7C#V=xpHOnXC zR$!r_!i^dB3e}(Uyps<&u>g9HzS<&ZD9`9oFIqSrWQD2dpTLDx07(g0h>Ni!t3-`&)&*Y`q`4{6c&HL)(MqqI|t_G@EmoUA}Nfy7w23CZrMcUL!?^<7P0bnPda{&$Lt<~jojPHGQjQUuixF2MYtg&Of~)#NYNe0kmEuM3F;r*DOh|T}CW8x^ zD}y#S=G^9(ZqAg%J1xM=IG}41yx&D5ce5DWkYCdC`Wv$+NRYpTs3qCTQXvPEcU4p^+HS9ohxUjmfM>A6XDP@bi+zDdX#7f=VX~L<5lZW&0e2H8_3Ib*vP}!B?m_pgn8g+M ze*J!8nwFvEE5r!6-FTOKc}-q5+jKW#M%-qL;xkP4?pQ&&%Z?f$w(onUY*j)NvLz|o zoZ>LUCdqVX)7?KZ6(2X1)xN$bE&3acX);$O3jf;UWc2%**pyt&SnokemHTTBkCr_` zBN2SHX4%%nDon_PknP!6f2{obt8~OC@J@PuP=3$T^U6l=^Qn^T(4tw>wVS!v`qXZW z+o0i1TB=1K(3qw~A+d^%z>R%wcLdDKYbK+&YWivruJuaSnrCZcb^_Wy#i^@_l}_u? zSuJ|5i)qqVE<>p+XpB#44G1fnT$pE8K1T6RY5YG~AE}P+$5(BFrcN!|s}a={VCp!@ z*5QuPsGIg_)uMe9wN2||G<%O>^sH54Vpbn5`olN7O^~~w8U!j2nw63z@u)8?%1#X^ z7C)<4rG8~!#k4rj`!PA?gSlR;C3<`RPw7I9 ztZwxAM-8D6MphJqHN7%Z z_F|N_Qce=h)n?g6xLcQz^jpQqbghe|-|Do9&&K1!I9A-oh~gt!^e+=mdIzf1qCcDH z9K;n`w1~z`@Vt}GO-O}Y3(q^#dEWVHD&*32raq%Ey5~_&T8||}Z~P}FxsVL>4eg@7 z1kaJ%iXQkrl?Bkg>#&@%o!nOR9NFZcIoru5hxjbTlXsY1?j1HScSoGd#cjGCE;KVn zySGo2541KtN3sW=BiDD|(6p-QlcpuZh8xO;S?hD;x?Qf}WLwJk(w2FfjSjoCC(iLBkhZdh&WF+^osSm%>GU;tExLP> z-bBCci2JPSagH9%FY00D&**{qXM&Iy489)*^ZQ|^@t5CQGVC^PC$qX|4zPF&t1md& zcXH$;o2d6 zJ**0wO_)phc@N}2AEdoA^RElSjawq#WAQ$2zfygS_OcI>^h>&A2aN7%5X!Y^O3eGu zHvOau^S4AwkD(j4L~c69N~^=1S=@N7-PyC*uE;BM+>mD|m&9dOJJ!^K-n-l7BN49Mcw>7C@{gw!G ztm8B5X0Kt(=+rA%za=vM%8Fr@-4eO-3d-LS3BU5vXSU5t4O>S4@e1v4MPgi z_v10JZ;4!Z#rCWPYbm^k9cI|{E_a04K%?QrZM~nx`4aHFa{_Bx6l)swvfWX&(_>V! zlpWRVFbCOAs>@B30UzrZOG#5^I_DXwWid<9tlO)zj8Ndj2Yt0{N^g^A z%T)_sXhc{rf7w`rXROXu9@^{@&ht&n!iUQ}|ay`jl^;*F~^7Zu#7}HE#L* z{#I)FtPH*rPMitz%6q=;$)2m9;BVe9{e>)e7;bdp>a9N^xBl|A5TrP1x{SL)j6*~GK6NqkuIm0Gu}J_d+(OCXxjuF9L&_B?@s)L+*ZWc9tv+`pCY#v zJ+b*W%aiQ_ym+e?y zJ_G7CDXH>3yimMSg}EP-Zr{hG@6gBORNE!PH+sFpQpVkcZ#YN5k>ezWtX?Euhc&1o zyyGBAbrb|?uW~_3@PAy$PA<{c!}(P3bE8IjgoXh`a% zCD+EgLeiq&n&M++-{tioq<<@Ty&9+43qd*$Xrnt+i~1+~)sGF(8y|1Fnhr5J*_k-m zMktS_=Px~bDUbewe*^CXlMg8Graat>f#{vU=7z2B|&4p(M=*Yh#y-kqgfry$!( zlF-16^OuWVu!m|;d~A;MdW=}K_tqXb_?_{V$!zP~VOIYg&ukfq)g;03Z4a6-7Fnc~ zo8=<9le|F&W2PoQIjBYJ#{b*1muwQ!Fo*O(Evg$|?pe8cRoIZWa%0Bk7JXXp$7IP{ z>%)sv4{Sb902rK-ygtk_fE&O$oNRiZ0Ay@DuyLV*>E&a+P3R51u1Xk5LRxg;xG9!b zs!#4k2R$8da8V^4Td9YZ&z7}l-ncIY@m-#Jf+vNtlbVB@KVxtT!{nb0YSE1Gph{1# zpN*@Zl|D1wvPii((^CG5>qyspDQEmCQYLB94dX|$wCK+9Tb^kt{|(`N{h!%9_gtmd zEs@?=a$<41R5tqaXIXdfR$XW&XwenpKOURufP%#S{4FC*%ZC|J*Hgclay5gweq+aH z^_v+RQCqn!qq`zmyj#Sh{I*)ElKJ}OK`4^1E|l@9k2diIB@ zp2tLEQ{w(zzPTXAZ_Ct2l-`!L=Z zophWOj}*5g@LVh+9UgIEUreH3iY9w^RZd08^p7g1qNB0r<>?=Kwdii`haPVv@(8t8 z{+9$ecWOr!Be9YfXD55H)94~{7Fb>Kw9WUx+Z&lj8L68TqT~t0Wf%YV8q@t;s8RPE z+2kz8CT?xR`|YS`8@boH25;dEdD}>(b8X}LFyhuW946J>7lL5RzQH*s*nX1at~yB^ zD7&PY2&v7)m};0{5GvkHZYwk7(;8dJZDp+_dFfyfY!6mZJldzI{wjV4y}8S0tG%Pv zv&Ne#=PUmu)h=;iCzCI;!en&w*dhfAP(Qjg8bo4=e6OciIc-3_Xc5(2K<;F;er$!u z8}SvV3z_nC^a1kC>S8Ld<8Ib07bs#(C+~9?$kWjk#7iONY`Jx`yEap4y?lOyEz-U0 zBsqV>8FKyx8u3rl&dbP_DKn#cn3Zx>@kufu(C4SI=V0u4G4{l%ecf%E-rDC_m{ZIVBvylH~DmlMG;ECexl2Zc_#>NA{gDow8Tv;*?V0$Sm*(p zCKOaeliR5|3jiLk^?OoeV@G462TU~bi%%0wD5P@L<@lo>_z*zgvYEwY)hMK-%^ zSYyoYtu|Jk?J-u;NcCh7#mQ&WTJ-YRqttiBIT^js0bTUF0ZA|OV0@W}#8(nv=ZnC{ z_Qs3A|ME8Ajles3;GJLSdMNLv|H*Nh9H4rLB;Gj?*N-?U|K}ji|4pSvpq6H~lE!v^ zi!dHb>>B__UIhL~8sst_6pKCSeigdD{GQ%%?VZ-99>H}TUoL;|YB>qakgk)cf9p5N zOBuRQ3cE_-=g{v=*l-Ht#Zb|ciONl-r-qO@LnDa&P4DBNnM4@O~y(XG!en;m4>)Ix9g) zU|{sn!-qpAIIzl8Y%1wn^{guzq>&H?VwY@EQOuXnE;V#gdk^4OU!%9osc4GE`>BqL z!7mfAkM$!wI9(HqBiXKXySr0DD&U`tu?iG$AJOQN#TwX0`;qW>72%F98l}K1MhWmt z;>y;IfJryPq&3%x{tPA6FDP4Gx--wor-qnf)_-0u?aY)8Dp+kB^`#h2vk4%e0_ba~ zK#qI(F*QwSsEBJtUi53xHfdhNE?;_l-FNAm`VsMue z?$)~qf9sztH450x{qL+%rTJ#%QVAPjVa+EP~R;4^jjfF zEE-N^6NiyuG-$e!>!WsQdINr@iwUK$n}AZ}A1#Gl$Og8bxt@_e>4W@%pcdVvQQ!Xl zp3SqVH|+b^$o<+S_8iXo7U6GA$xkZ(Y1gM2FNAml{Xd-5msKjeFgP1! ziSUKhiVcIST3)G?UR@)nv{GKAw1)TuEqZGD7D*Sno}s&I_hRNlrvx~S(B1;mg7gd3 z96-CNw+hwNq63uLhhkBZrcQjqs3b{xX+}n!0(lwzmb-h0nCtE39dd&IbL9hhma^ZH zefMuO=am1frdrOF%awyGI1J_4?sC~7;WxDCQ*Tlo&rVmunf;a*GM|q@9b7?iT`&Jb z#c$BB8vwp}Jl(-}mIjcGlfY_s0ERIy)@}lk3l+kg+EYViC4h?v^@sY^gMbSO7kUFu zk?tBATMjt)RqC5+cT7edQ`EZ57o8>C((iU~p#?A*ojb*aHp67pHpPWb0j)6^&F-{I zHH2Suif~1f(V0_}iuO)jP)%3Iz&ilHNxWCI4YgaT)MM(Tk`2QCX`OUpEYZ>IO6cK2 zyxt)pynG7msW?{ND-bjt|;t`kI7PEyPr zyj&?aRCpANJgz1wjI_h=^EbQrV5X8NS1Six2|*A&azjOdpL)ygb&UrBC%xKG!72s* zOofq|4wp>7?qQJ{DiRe|S?2fnX+19nW@cB0QYj;uk>lmGPmki36P2{9{aWM+m5wM$ zVP$g%^>1ycp#HE86+J^xT-=c)mvmIiAX4wwh6=MBRaeSUHD!rTY7X#Hs$^5DJ7(ND z!}~EQ^?giAcfIN2f=CqQBNda;mnXhOxJB;8ku#pfmrs8%1ZZCcjiJA&zMKZR+%e$k zKmC3N{dM@;N5RXUPHShr6lcc={xzTP)S^Sv*NH4WJsnY%mx}!87A9{&*LwWl5Rc8v z$@@iD-1fsNN$vw(tPH|eOLqKA3B?S>U{2o8x>!XVM4W3=pT|puy0Ktn}32$<4!p-rl8`<|!kpUiw@m0k@))QY0w&ul}|e?806x zdVadFZqols-3gZ=NNH|l`sKR3x;2@U-;C5f@0uIDIjdN*N{WK;i7~LxIQL`Xa!@_( zlf>oFqRKRq^f_m)wM_w@K^>XEfxS`##~GQ$;lrKzO~95@hu%v+o+6G*HDoY00F&-@T>{2zud*_qwzZT5pIH> z@bt{CpP>8M)QRxG>tLnUEO?W_Z@g&^aFV!AkkIb_`h68>KiH!^^!=WCs+}`AEfw>I zhu8bYweOa^*q4NEI_m1{f;p`_v)Yfk5_)QP&TBvX5C~cA3}JouZ|@u= zKk5iU%7UIt;nZj2abY{I#)ZY}*n$@t&95V|wn1E%RiEUi{*mH3djC3DzpsJ|Spea_ zadDkq0_(wgTKb*wXUCrnr?y;ZtnGmLZ=G9@S@@F*bE&WW7X2l4Jx+wTX&SmzsxN9* z4AKk_jFQm7=Uw{K2M8-r+nKRrJ>J5_Yyn@1YduF~ z;q*g^{u*glMf|=;bC9)0+Ecfy;x#hg^cryp3GHd^yIqDLEt~n!EI-(4BpUln`~7@e zkm5QO3Q232+7sHhx~x6t$O028=I>2we{HPTw3}kp_7ZUj=Lj#PMFzI$dJ^r1U>bI& zSh{8iXn~HPq+hBt>|?>Sh;NIfD^ZB3PM}9{o z4(b;{$7PjjBpNOF)p3`^#(iy;|9O5)4~;aGRf@Ovj>J$VJWH51gqiVJ1fremk7M-q zi1%1LwI#Cb*m}%zHLV$rA6t4|lYj(183|7DDuZS5av zgo0aw^Bd94*I`ZnjUF`LLFf4Nzel4MY|`56I||m>C3?T-W*g2?uMpZ!;W+i0#9L|E z!B|ie1`QaZ&AmD9KM#1?wp9J6N}`g4Ubg@HaYU?yu^4d_C8%Mgz$bgcJjW z8g>6oIL}VMxM}q!H@2`PPP4oAVUHiZIG`Z>?iBsz9CMzg@z^ofV37Ky(-@;>C0Uv& z-CM_nvSYJ*aLh3K&VHEZ?cqWS1L88m-2?L$1T#yxkdRPLM420DW-FzVRS0h#tKE5G zY_5>ij!F(tIuQ@=Olv>jniH&)xKLpN!mo`{TKn$UCqEE>SSh8oXSE-2&F(7M#A3wG zawGiG*oDTu{>9j$i`&3|s?8WcC0xPDAc*U6ilJncZs4X(I?U;MRhV}%Nb$C7tW=`$ z1toP*aHVIKeABA1s$HbAB7_8H^g z-LAD$>vLuF8Z{+a?gcHX+uF1wavYe7W-C4 zMYH0QQl#so`|75mPZPmE(s?A&WtaJ~0u0F9hmW8Bbpa zT6Fyc*h(cXl&Mdp`mhPGIV75@y$X{JTJ*jNuu^UH#tE>}8%)E5DA^@%r2i47s&f#R z0cPJk@)GrbjCzP|s@Kc7!>GIP$H+njUe%=0|o@3V8?4_Bw8_2czFgYloGu&h3K=1J@0 z)-~z(r20k?d>%75FjM)C%AF!5RKqvaZ-5&+;`%cJ~=+ zwcA?N>t3zdOG`5DU`ODa_RrL`Mq#-3J=@yMmY8abR#sbmxRY4r0cA?8soMU-=l~@< z7U#3^#Bs?MvjdbnV>oux0F>Kfj5dlvxmEi&1wdIG<3kz1Sa4*4vcQkyMyIXiV-pZn z?i;dJGxmg2%p9u+k1q{rHa!+xc`P1$rr4$B3H%r3{SU4{Df6$QKW}+}=}{|g-IU&@ z?Zu>q;avw_wN=A}{vr06Oj{v`C)3GYJO9{nbrm!Pf4tXx(7FbO_fr>rxb~oJP4&B4 z{;P2%{65gMQzS3nlirfG=Hok-tlm}9q&;B}OWf#cs)MgyuVZHYw@i@-Y5s>#!ldSb zJO@vvw%kar?f|visv1fv4jrZruQ9xp?k`RYSl2Lbh-7(HLkZsd<1JvfckVC#K5*tq zX7*jH(_8#4fm`e?%&aqy;h2x_B~QsXy1?EYV6qMm2Ol8c4zazL?;uj?Q4njU*aRF*7TOzI(*X{ zmCvml?~CJ$j<(0KL&zD2`~qkJu~*V`h@#NWBe(4^$_{bvLhC|b@}`BGI7P>$7M86` z^_`GFIT6LQV2!VXY$+(-W8nYd00<`zfFO{l`cPZ!d_k*$j3Gj4P7x@xqHntRpso~@ z15vbh8BLkws45MhenY@Al^fe=Yi%`8jAVyPJh=^=5~Db20+D=wsP>9O>eeWsIpAqy zZfL50(WUEjkRkQ%=vrkgBz6Ci=D?78XH=1yS?w7%@{Z*d zT+#1;_*Y6!yFp2ax8mu~NtevbH023XP6v}iOcK@xvX>V3eU%(y8LY?B7@v8p3-c*I z_X)q0PIkOc!HdWEKVAaBl^z6;l3KjlaT+qvpc96MlP?`sAgto6$l5}BwYu2f< z(u&Mrplpli2q?cC1l#B?m@=a4pjJj==eMTS66U!S?Vo%3(M?i*8_ENA46aoK&^0UV zR$s`@;-FL+aY0 z9~p+wW?#PucLpLseT62U@`AO=8((J^9cfNMd*$RspxhBfNfqm;b@9QbtspY_P2M<9 z)1?U5Uhy;5Ro*7=Wf}ZqXx}wruONRF9om25HfSj+|iVop3(+cX`ViL`s8qcql~>@7FxN|nEH>{Rb4{k zK@g6lAif@VM`KBXFUwZ?EkEX%m1uLm#^^}+Gce!FP-k;ZYdz3 zTuZrXFDW3O`(oihE!ifBm(N=Yi{tZmX7y8q2w+>T|>^da+7wv&wWLX zse58zI}@1oLmJ3-xKIC(LY3DlCLTGy#_)fanQD%(gmKL9P0yvlRKnzqub!S84xrSC zwsMk=q*YI!ECePk9POC20F;MgtE<31KVWy~l-Z;ViL0J&zaa<{XOl&3S!AukaFbVj z*aQ5BJ?us^q3wKV98-w0k-w!nI&NT1lbJY7-sHc8T(uq?sre? zYG`W;ae#geEUjxg3n=b0IVPOK>mk4Y`#c5?K4S4R$51p$C^<}JumQt1k>jR zAou#fR@b!wyca6vGtFK364Y;yKI>s-gO#hFQ}wAYybZPE+~g6#uBv)p6>(@!D4rNP z@v8F_*{PKFK8braT}t&yi1?Ikgnu)p`eLg|8WCo3pSyqYZPYrX)TEzhI=LXKd0eM2JfiTZ5^Koy))N@gM<7OHy zglmYYr!^i!ln#cq8F%i78TY5bA7`A#X_&c{j3yJvvaXai#4quLoi_F~^9Js-k4JTx zd>6B~L2|UMRTv#izdiugIquor^9d-=Qk>zm`u?VMebn!@Zb%OojQN{3eVsXcH%Le8|b(?Y6Esv*K0i^nTEz zvl^^*)a7`|2RU=ywqahwk10R@Yq>O=k0k7#GV@z)9Xo{MK-VnaKzg+mO;1LWYY6jf zOznyCnJHb|0cH=rJroS_nTcK8UdZm;hS~z;8h^isU)5O?Q!hs2cMK)%i+wTmuUgBS zwlVolH8J(;DB1<+NJL|MJK>9|??k~C={h`MaFn&3+18bi{3Tw4Pc+;So;bqHNpAsr zQ;l*O&&1Ss+fD7C=sCiE$yPlpE%A*~SV#<{S9M8XAMHmut^tsP4S(^CT1U1vKT8@2 zjY~7Ip~h5n_~$LU^^QaQdbXrD`Wgcc}QI~&?T=`bbRmdnu7cC z$D2_P3JMps5RObH?0$F-EdcmTLl;>22B_EVJ>A2m#8lHDpUHNz`(vtgD9+(I^6PPsh91@x$1yh+G}JC_ythxn{)c>qt!PALKKWy72A!4^ckzW&b@={Wd1Qz9yzJ z(KtIB-dCp-Q^T=+QcP82@q0C->f221pd3ENE=-k}{J32`$^zK3bg4lCpXl(EAimpJ zhHTtCIW&+d!sMjWX^(P`Ho6i{CM}4l^M)8Jh{(^8us|dcb@mWz6@6URD;u+<`i+<5 zri~$ua|QWL3t9^$o-A+UBrc1|Y1}9he!-A>I09B1Y1&xO4mp>#v`TC00uWOdu@kvT zSyHl;-j?hxb1UsBQj4#invaip^vz;=>Q?D<3di5?ft(xK7;&llxs6u$OB>TjCHYRa zyO+9uzmdvIrPx2ff0YjxIX80&!5K^QSXcF z^Bvp~XK>U94oJ09ql*jRNY!`SNa1R$yLNS@`~B4*Hn|Jh*D4G|wE7pcjxEcGGwb~? zCWsE?9&7x&06;bQrN}a}$6j7r?hX+JJS&h?mQG4&})$)(a zvt#O~G47gh0N?Fn>W48tGof>(Ye+4OGZzk{gGZH_TE|Nr{|C|~UYEqv+*X{m$c88k zGbfpFw**uxl@9X-Vmbfw!-zEggdtJmys=F~Irq;uVQw-)9P|Y20!u|Uu`q8H4dpC4_uuc zD9J#s;jo%9`15ba_N{4r;urR<>HNqq<8QxVb@ZV2mOACqX=X0Yq=r~_%Q<<9_o6(z zmCqbO#&%7}9oG?7`yycFvqVxwo}{q&Ny<2$q^!u)M^u^eB&;gPkf|ZQZsP~E6#;Qy zQ+w1xBSHSyIRTe6p{&t0`zjOifA9!?O;_%c$d#5;6ud*~@59J%fV;;(Qd20|$diPi z#>|&JEJv?>!pFRUyV5I~jE`x!jx_ZO>@L@K|M4^sokgH@h4Bil?|AIH*vE}?aE|L+ zC7-YmEX|R=OKc<+puW8S!3`CK%r1$<)YoEho-pt`T}CKqH7?4LuwOEVMwF#X$-bC6 zbtqf15#ADKM`o=}8zZW5kg>E57yuMA<*ZE`&+K64oT)9)IOA{*>dMSgfv@&a&dvsr z;%h^oAcc-iZbzqSxr9-blE zy!{#WLA`{l+dsnv&O5_fRc2SSkJ%qqn}--8lCXOF&{Q8wmQa44-Mv&=?M~PfR_`A| z`K}|b@uF{R z3sIx*pdv}IMAR#TP7+abBfIGxCa_M(ZCaVqf$yMC4{Y`Argyku>PLZF=pFOy?n-I3 z`?#E7!5ViB;=NDOG(%j%d2Rie=Q0A`*HaP*@JZtw+^HMLSkj=4(dz;*d3FGDXZ;_N z>_REo7g5iKCoEuQu5pi)KBNY3W}A%+shjDPZU@ODV0}hfZf3q5CCzASa*h6Kl$6`{ z&$QG9=VcQ~B8t3K;)>$`MlM8891jkdx^cgj(*2C|oU5sq;a=_K_?LTm<{9^epl$(< z@Vf@ybjKy;nq5W>azA-mjf=9R+kN#CGY9PT?u4&$2u!p}slL4u*x@uXVh^de&^TvR zjmBA3C=vH@siCA-j`K&&ZcVvqiZsPHsuYw>1B{5X*H{Y5h5?kzRgv;52G3g*aGpDv z-!V$gAd|^=E^z5fL0K{Iot#jLK7B;;E!s(@K=#OkAj~)j!W{=e*o*t}R#rJUh|&s_ zcL)oOqEIF*NOMEh?e4I;ejuStpJf!&+{kP*pnm&qY>6I5cSHBBt3g1280-%=SL_kA zxk7mW+NT(ar421nXn9mEdN-vls-BISyV=k<&Z-!bZ!d!Li`i2+u5S)DxV*|^-fPl066|5$dyfhl-k1%1BK?&b z#0D4M(~?t3(#f>^q9nJ~jlO=sb(g!7@&F1r&4r&(9qrF|P!A4G zlwhheU?+Eyq~1hbXAhh5jC+)?lN!WN+?hptiYL15r{3u0xR=K_-JB&YbW0NE$Q3%t zYIjt1M4yncj&%XBZwc%v{_5nR-bBt6xQVMGx>$9-7Dog$z3wmP#4YB`tOM-uIlsp?|AztFOX zHG#Nn>2*00u7L<83i{ui1KPaU%p^?cm5$4&b+8N=A+s6>5QNJIQ z@sw-KCHU`JI@4`v_OzqrM_8_v*s6M${^Wa7@D{MtO7BTbeyuxr3-*VujJck3>&n;_ zg_`XOJ0sbHd$yW2Ae>$ULamn8Q^J*TZmtZ|)M_}<_ThMxDaJWlpKPutkn;?MeJDKx zWuW$GD90!S$}-q`t8_D33d}l18cL>o^9*J7?a07bwmuYqNl95Zk|X3uXr8gG<7mik zydf~U<7h}U&Iue1S&cUa4ul+BS4*P3yu;k@-JIfQBl_8;idSGG3D@eokA|iia|1_1 z4&%9Cslo{C-c-sQWb3gOUjz??iaFL^-H{X$lS=!c0`>fui{MX~bTqWeIMCu$wtJZw zrTyO0k6R#XM|Y{h3Z)7!bjqcQApDP9s+fc?nE!Ih&tU`Bb7y}0CmHOo`?0>6Mint&?=6#Z#{K1RA~g|k2Oik*W6OOQ#n<0itH;}M)`x-LxR7NV0{;p z(f;d5xe}WJ%IZNqdGVX0p}UQn`!~LwL`wUi)M!Z#=@X5)3Hx&ueWJ~8w6vXPlR>#I z@*+jf+C-L_>)Vp6JNFct-{tkHhn%2^M&44r00FuvByB?!UU z#43pvP2)3fAULZCCSpoHaCO8qlwkfsKUCabo~j7O4P|Ek1oDSoeNs}dUWdBQskauKsw0G4P=XM?off0Lv?PB)sEza&t4l#OW@7L+Z z@YYLU=j#9Dx1a0XptCahSaS$4`LFvP%t$RdJ0~%7p2b8)gSCl;+V!wu6oy8|0f~fbd7lR6u-mz z`5pIUY%k`Y&62V^xjnH$u7SDk3DKCHR7zv zX&t)Yi!?(_QA|d`Kf{_Ojo0S*u99nI{I{#*Xec&M7d)7O*YovOw2OGLO`c&z{PI5z zg7Ei)AaouC0glIFFVbsFBAJn!>BswRRZJYpHSrplZk290&CV)Um?X@pq2#t=t7Ppo z_rsldPe=QUEvqaIC3v6st~{)hqbp(Bm!F0pd8#FT*BzE)p+w`vui{5mDLLd|M8eN% zDCz!=X~dOjN%3~S;ef-kq$nxGByIJl(9{fED;i4vqTM4V$jtgs#`4)&4JF5AyCo z$onDS3jAOlQ6g~eP)-sJU=?x_?tv|rr*e4$ituzY#VkH|m8mcn> zHhAc=xOPPlXC%(Xf$v!cY_@*y@^vBL?20Klq0{Lq>-R2S(L0}+5Z)0<@Aq!z0~6X7 zpJO;s4tqz0%tDgYML@9+?CTW-wA#TmpHbcm%4=S`!XzCjo)F$1_V&HvE$TC|k(T6h zENhy~1?|eHB#cQ@Oh!;%^coJhic;wKDUQib{E4$?&z?PQ_G}yGpB&;7 zyC?t?0LwQrmJl8;!P|1YrC+uwW0RAtS9^*JjKiK{Cj_vcR?B6IDLGv+8JR#?zqia} zJs%$tfwKPNW##?+hJVU<$8sziAd?T> zY<^eET6un70-JV@HO}k5=j}_F2hOuu4JFCFIvJz3cT}0ge=(@Pnu;!qqM}$n3QjT|+jUEg=i#w0^i#7h-i*X|%t>QmxIOZ?R-9 z?Hp%39Y|AHqb|f~@pg-7N$dCCY4%UGoRj0F;X2V!@`)^jV)=#xB3Tly?YlEvORZ80 zHD@%Ge0a6q^BPL#oVAe=8nzGoxSvk#hfR92m5e1BerrE$Knt1-a(Y|=nNPBaov_LW z1BsK5lJ^OwO8m-^cd$5tGIqiLO7Z!V$TASr#P9uhC=A3A*TBkc;^mk{O#n=h6m|Do5QW9A6g9SX(f^#=2N zIp@x9VDi(7o^$Q@k0p6!T<=F*{RMc)aZ#WA(gphu`SV>Z7x{;nS>B8KWPL~{d`(~7 z_ceXB`R4PcWL9wvtW|h!8-ZP2=}bMdx3K8VCKZjs||(V?Vi2q{rQE>Z-VgsK@g-jK>+VZuRi)D2aeV!n$ePiA?$wUSx_cX zkwn$YG?y@qD3?7O))73z4cG{>bupIXQdEt$p|pGi`9{Kb)9WbX-~vq&laF%1sJex6 z5^q-m6gg1KICpYYrO!v54*kN^e(z>|ncz<)S9h_vV@3Je)J-7dY>Mw?S;7>VTDQAW z2@^D@nc6d|nLL2c)9+HX3ZQ`UmgIP(up?As97CLu5XK~c8XUdS~)k`Dz|7B%?V#b)d>{OVVt=; zwQ5cqVt+i+ka}x`&+O^!lT9S5uArz}nnA%fr4v&+NK{=zValn_=Z3ADAEz$IN%`+R zy1cz8$imK>OpsNd}%fTt>r{pn|`IMo| z-+JU6f-E)2^gXD45gV?R!_>}IUGiZ)yQ{LQcDYgmO;x*Tp(#C0Bp>u7WI@)lv^rWS zGFhOEponS1`#6Di%NV9p5Kqfy(lu;JGgkAB1vVDj;qYjFZ%h;sytuZ2Autpu|(C-5q!^JbD1D0laDc4xcyih3$Z8?_lS4mSV61t zCZ0^LPEn%9#Nc~+{gwlppBX@3vbs5quJNag zWqTzq1eM%n$ZvFwI>{>223Tc~#?);wR!JCOl~2$QoN~jkry^Dfqqe~hzzRID!e|b% z4GY0)Y8_3MP^-z}!(Zk)7yjn+_Woc(S$cJTTa#pMV?|bJAIP&dxlQfZo)0t~MTfgn zg2{tHPYIsGce}5tsrJcc$T|88J^7jnOmY9vztEL;N<_L9uTE5&e7WcIozly{`-7e?6evYnfxRf8W8pvV>X{@UMFW zgeM*W;a`t{pg#QLxAxKaf6HbNPHzTbk{5(pF9<;|2uMJ*^O44F?Cc-mwCKTt-&-FElsFK%H(a(ccJF%??K^`M~)f|yrrfzrjC!{ z)hze5%v&BRy(06LM@lcryyf9iMb5~v5q=h{?4i6xze~SArn0dqQ~QHwWz4yM2uuH< zhTn@?4$kgW-mPJ*ki&}GC^%T-llHrCk3cTmB!!v%ewn%2wS&Cqvm&DT`)Eh7h9 zqk|{JnEH&?iWN24Z7vPxnJJ~Ya}-XOQ}m{5Oc47<;BMBeVL$6ZPj{6=x?Q>kupRF! zsnIoXd)P*XGRaKd-{md}CX^%(#_%^5TTXfVE|*hDsNrXJXg#)EE6ZL3In&9sZkT#q za9zgifd0Acg*QuoZre?gi6C+cGjh4`ZU~I}K&^6LVQqf_SKH4O)+!>$6#Og5%vePE zIljx@&2mt`A1o%y%wc9cf_VGd2Upshtf6u91%c<%A z%;|kCr+P2Dw^8p@p7siM=-cVVT>tn(u5D3lmy_H49gfKZQLsH2V3b9%t0VReE#Awm z)#4kYV7n)ve`B=BD9)IU@Ro#K3HxK}b5WEj#nfjpb-#B*#dj;kn0j9nY)iX7Y{{Iu zxC3*4D2l#{%KHNzMlr@}NqPTCw2_WYEZbW=FF3KR!8JGdHQn8eDT>b}Codz6a(O78 zULC3^p4b2MALsX_q~e&4kLcT@r_aXc>`hU{w;^Lh5XZcJKWmA{FN|V6UXR9ioH*0c zxpaGCa0UU9QT{T7zn=^}?m81VDbMdq$^S9usZp@<^?y8j)t@h%fA3k;PZ}42P4HtM z{yscnf#_zG17SlM_{iN9#T(Hu<5#NZc&P3TT#iBeR3JBn`f&P!1i~mJ ztRn@7%nPtjoMFcyM#&rCNudSh`0FW`nc9_dQx&nh8Ra)&cCUp<#M*yk19BC!^@Hl^ z=sk!slo;h#VTY#K_c#aD-EHOl-^|qA)ForsA%kY4gzYzRkUfN9YrOr_f6{)8ODxm3 znkBF$cg5SsC{>iP@mlNR`Z4T@0U6Wcj*6Je`yYHDvHY-02%_zug%n6pwS9=O@d4%r zn0h(L%v5k4dLXP_!;$hU{Ca`ZyG*UAf$}Sc_4TflK=~DBeZ4CuNYzz?Xn~N~xzaUw zRsZim^wPRmpPZ~ct;_H?z}khtav$0>^~eh(Z#5_*hrW@^Bt&XIJp>|J*O>V(8LY`Q z0r~*>P|up9h{8p8cc+{Xc)e*`$&Zq36JN_sg7bz~ZDgt&bl zdTnG-y+CzDblckUB@3BC9GcB8+824HUG!l`stZ@(oPd{(*8=$-0 zhaH3J!;#WMyXhm2m$TSO%$z#Tw@l;l&n>Zx(o8W$I(|pog`HK{9W)3Bso$;YmA1# zjyA4_552<-s`X*CX1ptQ)kXms#qrKO>bNYU{xhbY98%<%db7VkWAUzW?50~xoWknr zXq_1HJ?Jae_%B6*dmOwYF*P!T9L%+j-Lw`8+UsboIIFEt;W&}R<9jvhW?Sr{hQax1 z%s+?)q`>oyQy zq=Ex+YQtV#_a1X~pL%`gEBDN6Ir%HsEw7+ex(B^gJ@0y@j``|Un%&II_(n7 zP5BBjA^87wukW&JGbkT=QLnnkHC5xOUxqyOx`(^h_AhobMO?1h!R21oIKJ3~QRWZm zE)jC>>xRjoob}#9AD@@%ZrWP!ZQ4}7u206Zo!;Hl>qvF?%G$WmJd(%M&)PBWuihpp z)qO@<>b6#W?%qvz{45Lpc9V&)xrSjD{5b19j^3Yg;Uf?JpIBklNl?X-%aZr5BczFksxF2kMpH6%2;&-XUw-UHTOIquZj5 zMD*8nvrmu&Uqn3~MeZO(ec&iFmmnqupCLRs=mU$FK5QCP>xPgio|6y_f?V-e|Gfa5 zgjR9OC36W{02#<*I6r@8gd_LRpz0pN-R$3DV7=0^y6#{dj&ZBByvmA`OJ{wtOz4yY`$)thGc?2=7mIznE8-N+?x%JoSSdYr{G`y3MqA9CoCN9*s@ zG%DAJoc1xbF7^>M)fldQ*L5j~-V^>kpBEfIUu!;eSone-_B_!Rw^;guHkfjMsKE!c z!2{OHx~4j`k47tz&e(hja?b|heEJJx+HBUIrCpjm5-rDedCAMaDU+O*7u;BHQ)*W$-HR7Yu z_a?4p4iZuGvHjwm;T@NlJX{-bjTNr_MR8})F5OX;;X}0Rz7n*mLP;)qDt}78Tnj?a zV<4P;0tDpx9zz6@Myd!{L75k}N|Q+yN!K)WC}~3*uS@geqje$<+6yG7Q*?A`5#ta!^lis>?^c6~t`#2Ez7 z=aG2t9F6HREnQ#G7ludI*R<68f*Z%zpQ*u%X>eDEoL-(bd`cUxW%A#3;gkA&5K-lI z+AR8Juvy0OTt4)sHW%NJ!H;c4Y_5?!OZnqBZ+dRiqI1lY;HCf-yw6f@7dgjF+*LiQ z+IcQ?c$DJijp3(;HV2nhPY6HNpD;&fhrZ9esMFv+Exdl!qP7rgoET^eamFcu zwh&`tJ_Yl@iWZFZF_43|sx`yIr-?t5>w6mJdK z5<9`Vx+bAp2z|iVB+wWXWaOx-&6Pm8Za|eG7qvQfhMx-I$SjCml)br58` zX&y%Wo6O`s9BiNuI~aR)u!Ej(yim*rnEV%t*9Bq9+8|!{QM7?RY{GxzVj%uWuF0*` zt^oU;|8~W#L71{MXqPfPCuMQU&A}zr;`Q9*n>xP!o>fM(Q*U@Vw`4edVX%TeK3y-` zr8M^u%8QNO2AUu?uG=bGa%Tng)ABK$xVw0Iu+Wz#ouH44ja&EA?c&}|Tj@?m2YQ*X zw*^3KDEX6Y$;}wf)f^nDa3e8xEA@7ReSd%vFUXc$!*IM1j9>XefE5wFErKMkRelYt zWVEHg{m;|Q*;%NU(Cr6HL_Ix(UQu~#xxyu*{<6G(;o{GzX;(u1XFbBv&w7&fMpPQ+ zNdY1<^a;M1?gC=eG$@6A_uIe%IW7`Wj}DCpnMU%V4-9p5v*+FKjqpLw)LTF3nSSe= z-x~psG5r1a9F+IF3Nsh({GNkX>EjN;`T^a#&t$2iJ00cyV>i@MZXe$(Bvn4nU;h1u zI$F~#k;+QfmF~(jb^PU38=CGim3`1-o@p%GPWfF$RB+58D0u>f592%Gpy~+gGx<)D zOd%0uVXT=ZF1Z z^91EVPk6z?Pt2ZFu5f}tVrnSnwK5hqt6WJOxzBTE9i)VuC1`>hYW{J^A*cWs>X+ZDfEE7ei)xUr~?T0s8o+NwHg^_DA1!f%w#`Z}89sG}3T zbu`Q4oK#0Ab=T2sPaUgctmIy(D!9W8+URc>W7)Y0ppj$SX;(K((vI@eK0Z!q16 z3{f?RxZz1M8AERLU0XHHcWqUsuek&nUOes0R9%3bniR;vw{y56-zB9Mft_|$y4@0T zb=(H$H6#o72J`Ho`Z&Jt|A1rpT*I-To%6+Eo9hVbUNnsi^6i@rb5fnurPqIXhtFD7 z=G*)KiIs_$aO=O7@Qf@|Ggf97 zvp->X9N)zjeb&Pl4XSrW_qp_4r+ZjerL@Gi)Q#TiARn<@5%p7wxC>J|-&pVN(=a+6 zG0E3FU~ta#y|>M!nArDfkTc!XKB(@Ci3Blw45)G(mtb>Twbd6|b!ExELG`6rR2>MT zb|s>YYX|>8B?yMYn_H8}I}DYu0O;&%XUp^}9huPSUvG z;K*!5743p&cO7E(gK~y4d!g}~>ZwzifPHF8kjZE2Ere+RC5KL(Y791qpgi$ZMBN_F z(llk@e7J2)H{_g`!vUv@%3#xVM%0J2*tH5c=lW*(tWqjvrW|lFL1qqmYINW(v+%Lz zkz_QMY+D?_l3%0D-U-!Hr!xWjbhB12B8fpERF|338BuQzLpCxdtvNK?$DkeAl%raX zsAJ7{XLZA*@6Zcl0c0PET&B8+FtGMqVj{DbJCIAv)u1e<6~3Zku7lfa3hpT$*%?u7 zVbtGYJEl(mJh;*Y(@q8v7u5c}Qp-nlGnu{E`qMH`K9eCg5;Ty1X*+m>w5!cp?XX*d zmDP7=aLU(Qh5ec!OH7u1)u3e1BR+PfNu1*Lw3}>+K}<=5mDTZdmxAofkX)N-wqHDj zlC<-3lGr29B!c*zoRpjN)pT3t>FG0STo6_x=uS|z%z&$^GqiRzS(ZvT`DyO5|%lVR&6Fs{&p7|fXOt7 zIOind>|;iO^4JkX5RD}x$aOySjLS~tf@tTC%x-x^B<1}z8mj;(o+F}77f3ANZ99i0 zbckn3BIW(t*MYL)2xK|?#*6~xjw4iCp+=B#5=@&P$ge7F`$J#y3_kZp?M>TogJ_!$ z%d5bCDR4cBGe2Am(00}$k~@8Kq%t2UN2pVAvWXULhwKEU`3PbXM8(PW%eb!u<#mdF zkp#ghW44=q9wJnXQZ^VAIEggCIx2^iO zm^W^v{A&2!pn7G1&rIl=OlAbQW9H0hGjd1a&hrQ-y?@DTMa1YRX@ZAU(iokt^Wpk(SslfgHKYHcpPkJS8v0%y?fw(eD zM2#V*-y4IfIl^Zqcj@->4e&cemkp}l&^WqmQ0)(kn)L#%=mru|r-fH!+RdXhM6Zpc zc?yt)%Z^x;huYS+LGHVL5YZE0Lbu=tTZVtF0`qZ(g3{IYR!A`1*Teren?CD*g-1U=~`1vKS%HBfGQxXqBx%q#k_%W{VCvjJ=TiUj; z4RYhPLMzaCt#Hp+Spb zWF#Yh{yA>G5HVcZ;nJVlWHy9i4)!GKReQX3F zU{CHVhQ3o?GkLXF;GO3TPQSdvm3RvMTl>5)@xvZq93)VeZQFPA1GMSld%hMnk&yD_tC2R;yD4c^e7?=>Qqa!2Zu?F)KBSLt4 zF&j?kgK)wn;nWIqVkqV)J-NM@oO&a~CR&PK?oDF+KlEP7Ii>_60zFc!KPBTGoH6a> zk>V+**n&w#&U1;**Lwy1z6iUmy!a!sfTVpsF7PYgq27_~Z+aQMK5Q85b1pi^va0jL z9+(6Z<9#}f*hsRRKzhD2ljwJa#b-|*>19V&_3{i;@U<+Eld_vUDc6uT`EK%n>>vx} zH$$x9x}sDW$4s^iejM9+5k06TioznQzCKW{K;<0K>Za<;1J{dG{rvz~xrJ7Ds;2+* z7O@o_MQnx8%8Q`13?!7Hjrm?>I4$`x>fvL;-vFdAA06zM3I`fVg0^NXLvlpY0zJ;szFmID031?V=~cBAi8 z2kN&9Ge_Fdo=D#T%6(x`V}(KN^20|>?GOStTm}ayUmUB|>w-TX8{WZd@p~073MRP2 zT9&;~(a^Euw!VKU8Gb3H8^%0%#AOdWzwL-i7i2_3jxGXOzmIKPi$>s?~N)yJ^) zuFQZ-JI)Bq*Nz4M?KyX4CX@R!lZxtH<4%sI%-V{KN86@kZ*gUKm{ScIzw$WKly(rm zl$Yi7$Q3!V{L7rPGDm%g=V}Ppn z9nqCi)jfQcEk#b1t9c{mXze9otEps2p1Bb30Buc`@_$j-@8MG2d1ZME(~g>y{3t)BG$47*T`Uw5@WNSAl;DNffE7AJCDCw%dGB z^{FW4__uIM*%V?U`R*&~$6>~jS{hZyM_EfsX;d8(6$xZV)lZ{Q^|w(f&+pqhlbQ6M zy!8TfFLpwO_xsQUoCjCit#8&h?_*fuRfZLQ^@vW>-M{`82yJhHz?G#-6@^@xsBCuP z2%XD8He$ZmOkJ6-)ZiB^jD=NrcD$RnTv1z&VmxBOkROC8y`sKx6#RG$S&Rj_ysxPH z!fY8b#tQ9t-i&z-r>*}XEvsF-WB3^TVvit@{iX}LJng@v*&z5-^@SKS^RleBjq4b( zfa!%)(_x;ecu?}CJ(yJbUhjMCVwwdGpRi2yEtlr_j<-1by;nVfBbJ&_x$ci3(6>O~ zZSlQEY6tf0KL@y}iDl&qfWe=w#2BzFr8$oAg}Sow?knn-1IWmOPx6TT&$mFp{$y-C z5n9@)~0Qwb-C~*8sjxqf|XCW2!&K zbfm&Js2T?1@7;*)MxDhkev+>FvUeV?U`Z$8vKO2TY8|*Luk_C}=q)mthWipz39Tbe zqU9f0ksF<3D`(T-(Qn0tbn9m^;`_XN#wT+Ih-^4XAZtWru0iHSJEj;|dT04tjnFZJyqwAX>G z(OMn$YZ~*nhhXabAnKN{rEzR0dIftMdqZ~-Wdc8>-!+sc-A2W^jjsIQdg(TrF}Kk* zE!ZO6M$K~@UAe(N={7ogZlfzZm{)ZhHO*~wO$e^9x{Vsv{3OYkFS|1}FmWddhv zy+wM#l@s{lo+ejzkg?jb`3Pq{erXGz+BWytn6kiK`!ecXg;X?62==$7N&3m@!Nz-- zJX3%R*?6t5W61cAa*&vMD26gNu%gw>SF)}&roJ2F3346177NK4ZpgkUW8FrZk=J5$ znW#))b<#5F8CP21)IHl>V}eYUE&KSTEs$j4BkW8j!m=;8*bvJurCEy;0(u)C#4_t+ zqVGbXrUlOWPSeV4jHxpCG4ILgkiBC4eH&*_y>I%8_4nN=tysVE?iCN*d;hdb$OZ$9 z0t4iL0Yor>1q{G~0eCRLB8G<<4FWhC1h}R_fH4gMj0YZ!z(58tP!0^Z7#>^zFqz@O z4ge{@g9&(8u=3tj;w>vStcKY6`1=n4V)q#UK%BnrCm~J1djo)c3>ZICA3x3>4y)Ew z-4F9U9@qM+HNUyl16AwSO|M$N?#>6+tg7~`s=R;I%Bl_Pr&Yq-HEY+bxNp^s5Ig@K z<})e{1a>&&hQkB~;PQA7^bijP2H?_o5R!)D6PfsR$-~En;c&!o2*6eV&sKoIR)B%6 z025mQquB~DvlWoRR)EM>OkcTb&_)*&s}s34&!N2$!z~LCpstS^&bW_pQ1Q`|4`l@&Ew)tqcIL-_`&C_S@qCzyrW@ z0D$-H1OO!9{bB&HA1heU3Tx&85|OlB4}ksYP6WVyJe~r8{pigBz<$I6Mq>CJfDyxQ z02mVhyvG2GVI2n?hJqe=6M*=#9&~v8i2w;0nnr@&1RD2%fXBN>L81x3eP%2qnEh|la{a0m_pcJ~msb6+OIK~Is$6xwnDdj8mOfBfxoX3P zyC1ms-c92A;bJkPpSgUvO}Mj*l610qn~B;GK_L0M4^7 zIG)92;g#KKzryhQjir+?6z|;EHy`_G)o1NmSoc3qXkj2_sTO+cGPMxy`QE8@@YX}| z(78Dt!h+#&w6k#!e%j_eH2RMv-#l11Z)#yi@3+^c&AWc5%i{q2_9=lVBnK3zWM0h}9C;-dy{LjzS z;dI725L^%3yBg*{fP-i48dzL0Q*~VHlQUrvFg8 zrH&~xX3WZS{9An;u=JLj^7H1!KkJnL80SJw=DH(^2F3?MX5ygsoRfN?5C zYz3|%knxL=_&fk$oklz%fb;tzwgT4?jGHzbCt{o-9;Z#7j9-kzf%3zrG@hrn0%iPS zB(6pOe?Ed)(7wR1>`Yv8%!2j>mgfrq;3l)6eSs5%xd8ABv!H$dU-Ra#5-|{j@h@wL zqsXy}1kS$~5scz_Xs3lq=bZ|QoRZonxYFXlO1OuxNULCfrIldq3-|yQ78W)VnaQdH z;%Vsz1M}N~&9XZ?0}Fv~Eg$@LOk$pkCoze6E{>SQjGNz!NzAzUC!dOQ*@wT$+*Znd zz#Dcs_UZqs)ho#n=_TJZROS!Y)l&{umWFYLwAmfOPM0e+aL9$;=|K6c{A*1|G9U+J z(r1H3A_Dx<=aP!H$zIJ5>-eX-uY=y|po=w7wE=21+ygbhZzf@1vuVQm>ieUuAl5vwYg5frdts(|VILjMEA61C zdE?mTnzv5EzGCU)WKzSql9}w~8U%hv0#N=>SrmN#KJ@4=$=wYo;e7Og2j_E0!Zpc1 Q$v&2hMLcFX3&Ah#@AAxnnE(I) literal 0 HcmV?d00001 diff --git a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin new file mode 100755 index 0000000000000000000000000000000000000000..25a00e2357d781ece9c0d41447c3f4a52c6d19f6 GIT binary patch literal 39952 zcmeFZ`+FP3xj6pL?n+wON-STJ_!eg+IYG8^QR0LeLW&~UE}Gp_xbz*pC7E9-Fau`op)y5`?!842rF`dKVI_x{{C-iz=wb=nF4Z|3Zymx3`ZP6ky7^;3zDerI;Ymc+PkL0Y|Jtgsklxour01%+_0pvO5{Y7m@h55#zPRBFD~@XlUBLrinFBoWaoYJ zsVz%Uo58ARtwvP|NRF;WEjP1DMt{4{>mRGoJfjK3m6l zuK!~doLT*Y<8M~vT=oYrOz2hHD9<0sVu^|Mj2hVkF3%{>{zV1n4<`O=d|7`TS8|-T z17YAx(t&k3$3Ic)2Z5NF-@_W!G^{5k{U2S6M-7I9uB$w5)#QTspgVZZ*L)_*=$i>* zn!@I#1xtD9Wf$Yg5>Lo&WEPx|UwImM9*_&jHgLrSm!|wG?AAArn)7o$;u~3FVArTN z0VMNPR*Ydo6;n$o118*>}-C0qPITk^#yDD3<#fMvM1hLs-#KL=v(h#0Fw0MXq4Z#o4wK7*DR{K&-Lrg1Ln{-+V?MD9CU7tz^l4SfwBTMG7sOnR3E$f=JfW^Pql(g~CqCHvgnAno?M#HXGf`u}6Zf^`wtXPxwmqS?LiR>m+||Md z=q$6>=e4!a_bn~!%>zsYANIt3t)Ivpn~U47lMbl23a(Mcoh5>GFxac!Dzw*`L%z)y z=hdgKb6yef5CLh*!{6d;i|@YMLJ*3}9h*zq3Z=num$|*JV{?bPGyN=SyGf+)nYux9 zdtG&i!|8q>%Fv7UdsDzeeCUy&rrfLv7?B<7*TILzbN$r=HsV7@&S=t+w>9FS(N{jyExKD3ZZ(y*7la+mW{1##^av&pi5Kcr)mkWv$fr_ z9)xdXTYS*viu+ueQk0rcc->hUDzAt_>*iXwqgrtD02WHIyzB*KZo8!LwtZn&HPw*-I3+k=TqT>hEb!NYQ z)^Py?IjpZcAsybiS-?A0(X1)!Q~Ng;G?uh= zxl1F=xpp^wK7a0Bcb8iiZH~jzne|`=^4Em5?U-bXLyL`=*khvDrYWyX?Q0wOvf8C7 zza40f`z&1jh}<0at!C^0DMJ|QA`|7zw=D9gRcahI&9;goO z+-z&Kwdrt|`^srduDleJWpnGn0_2ed9Xo4kUz?^FrmSsGs;%byjg%MBQwc5k>#c2e zOmTf<%Q4ZKK8qQC)UsYvP|Df{!ZGOrKRPBN+Y9PC*r|%nYB~!mTmJ!_g`27WvwZ0s zz7smK^`XrrfiK&f{cN9eKvRCBfnABHGLVGY9Dk?<$}8I5p{J(nxVlRaY}`^Hnrd%z zSuME0`Ht#$0jW$JSABqNm6&M&l9dqf2^qo<&jH>XGB$xlz?#xM1r|fSraU>Nucw@A z_f%&6H04KO2vZrq8$b{c2-`w=4SDrIu9z}6lYX(qHoS=hOv0OmU-l<%c*tBc?~jdQDk6m9>zG&JZ-^{mJ2=ru=zw zK-HVsdQB;u5-`(eo*vsRbxAgo!*+?x)-K4g?s0h}TYTFV8)4a-#rC@V#`X}{3h?z? z{ce2(RT&&i%*-g!W};PEySBL0MDQYZg z$`@P2B5zS+?|H7}h{`Q$Ccb4y)PhA;@rYWws6;AgKvhG4R?ayJ^YgrwIIkgFm@5Lg zU#rCC237!aw`P-UWShrBhCMcsq;<&K{!l%zTIC#{$q@E^3d5ye!!Z9f41*bX_#B3v z90-ri1^&Ldz~_g2wv8CQ%wbL0rd9W!9)YDf3;!ovEES25dWuDQD)K|{EzFPp516cJ z7)n2$u6;*F#y@O9^*|P@>z)DrgFDS38>6?~rV9Ukl|FMV{><2+o0*+)VV%^%fkH z(Xx4!Y>a2+=$q@C<7ajFx|SoTU0LD zPPrCt-7MgeP2F{1PefWbS76F}+g&|(7S=5qQJZT!`x0f&MM1QJ!+!iY-tRsUhWX9J zm(@&Ac5i1%a6df(LTjh>&XVAwAfvLr+dBrnRDaPQgB)w*oAKO-A05y2XCMost($kM zs1I3&?i6r$9T&?3AZJspoAsE=f}uOR>$Kp*)Y4cSIn_p3I4Q$|b8;KWf{)}yBT93J zq{qk90^bbG$I6ZM7(D^xXo@Q<8-nr+SfQS$-Nnbg8P9#lXWcQL>p!8^2y5=l3b^p@ zx{Uruzql~6Jh5?iT|EX8O9`0m8~Bp^;*e?=4yg;w=AbdS-EE1SP#2ofGJf5mM5Q?| z_&A-t^+nY#oKQI-FFw3Y7s;|4qItxXQy88h=v0)AmcJp-uqUE$oz*DcU=2cWsL99kQL)^`z@I zZDvo|@mi_7?kjoio!J39?ylRk8CmSOXJdCAZMV4nQ@M@I;r<~jp&SgcCOyQ~>Hb6I zygv)r9=Pp+m9BhG>)qbKu!LS>NLv#1fP?vXSOPL9^?Ubdl-kjYMpRFSG!w7eI16*J z+37jFJ7p;Q?hq{UM!?3!nwT{zw-y#vN4c>MCclas>tMN9q4Dn<>T%V2AVX8Fod*+T zLP-!+?V)RId%iO-xW{de)?+SKZUizsMfb^{r#{L4phZC!cnlbksL$S`k1`p0UhCVN9C%G;24d}6<4g6 z=?c_AtdQrh3(v$rHcsiv77o!}lA$?0aVU$6SBdi*I^*3Nv@0xolwEqfIEnbr~8DS_yB%)+*e5XMNxD)UOBv9nxmqU69)G9j0#<} z*r5A}S}HG2{YB2INJR5Vc@7(yVWHplPGwn$d?|cZwAhgKksH(6XQ#L_^N=y7T@jJ9 zD&pyuD4J#YMt0*&`~RBa%Fdq()S{!m88dQscJ801Ef>k`Qi>hL{L{%N)2HNvtvYPN zj=%>J^(TEG>TI1N)up&tA!rJj%#_PPQ%0xDX3&(+C&9|a@(j7S8f(hw^jm>G7dx@0 z{Cyg3HfTy@5-io2A^HihI59(*2^&tY)avH=&^@RD$gX|}Z{ADwR1PDHrre(D-MTVB zcX$Exwo^L|NayR+KfqzmqAA}?wNBUPLF;t=PhU3=w1+O7m)6ZC8@ubwv7#Io+3s_l zHB{|&TOxlr=iX*G^@nqhZu@@3b#_+%UiYR*`#@F1b=Fw5*PWo;V0&g~UfhpYp6czq zt+OPU7wjoUD|3b~!dM?Wvn#V_vpznv%lcRb=KUb$%?=TSsBC>~c-O&1r86%G!MtK3 zUu^q)XY2HuuphT}KHpgq9G0K&6zOSAL(B4Qo%=dzyY!dhLE84`8QfM7hULR&cIjf; z3O;Om2rc4Hv~~)(w?k9z(#{8eKA!6j%Ncp+K5M>PQ&zqK_D7@Ce&6fPiu~c+_H7-} z`vMNBRoo=?ieu6l@u%{NK&#{sd!pP^-c`rS4rDeQHOwA^{y=91tOcfPkyz`N^u)BF=v zQO?-gp^G8iiSTbbK9tpgMI+;@-`UldsGQa!G8zOdm^blrHI4Dc?mAchp%~;a5sKmC zDTYeo?Uy~4p2NnggXpF1x>wy_M$YW|?9P&4?yxEiCEf&4_}%8Wx$j(Sc#a%6Z2WNo?9Ydh z?RHUSQUi@-@#4&R@_Dq`_2e(?~E|_ zSFSI_>Hrs;gFIBf{#(9^mb@N=oSbM@U`geisHVKBv0GmYoOy&LIV|fah%CUp5|n2T zTLVifnaGp1x?`W6gmT)a6(5sZrOR+B{y=8%hcbh~_P*Rkvf01Td9d%vl;-}u8M-4v z`kBUj3038%{mWE-A{&%9UE2|=YHw<8@?ItWOr%i?uh)A`=4rw7+2|d#+S4V5#1pJ- z|NYoo*Z9r|SEaY@@2K@&f$jLn2$a9F{T+4CiO5e63pQdK)5FCw$coMArRM@!fWBsgJc)wKHTqvA~-h0J3CqzBfC7 z$SsL-Z+3v%ceeER77Y>G1ZT^uuWAQEk_>qx0i;{i+35S_Dv*6DB0oz^$~I{&{8nDA zE^9B6(AZn(Ih7kk%U+QoEa*UR>Od&hfpC=$geDGz8XX8`9S9Yb!2j1?;A`l))PYc{ z17V>KghCw%w^srG-YVc{RRaIc71aKOa@W}=_XZ2+g(tasAl-=zzL*bh0Y0)9_$GUM zh)VZ@6Bi{&QNN}4dRy zjp}r+4KQPa%o?e?PVX(2^xk56I;}eIdAS(VHWZ%Djp9z*UuxG@Sx~*Eyrmvb%X*!+ zR9YB7vo#~)9ZMtnJ*IOz*9YExjgs=aiB%rbo9LA5qcBaGVEsrR6pY`EZ|2b-`#XA!V%CnArj2NME-okU#jf2AevSh`Otb*apV zYV6#J2-SNu$gwva9TK)JZYW4Q_F$n!C2q#qNL(A%#^Mb3S7u_?4swK)} zixch!9R~X~(ag%Vk!)}cqsl{JWL+hpRkK{pqwkoq$KIK_R$Xx&ELsu$s|qe^`@F1z z@N~^9Dkq!|KEIuE5~k-FwOX*PXM3JeIdkj!-cE+FjEx*&^F6&CT&#w%+`s;QR-nGA zvk3wJ`hFlk*EHqqlz^=bh}@s#Y`ynuiaKS-Iy%c=&>f7Nzmcz$n$K$wu7N8>u+G5{ z<4nA?!<{p969yr#`AUgj%8VVor*G`Goa~_+>H4ehs&L&7xmI1)zEE8@jFx>NUnwn3 zaDTa8<*!0lnnoF>R$b=QjlE@Bn&1{9C!NPBnetvrQ_6cO4|aN~uLdd#!jIxT*oUvf zvoIo0q;Tnq*A`-CxqfUXqL%2G_FK?nu-8Oj%wQZ;VfS?ykq1*8z8=@(lyZ0K07lFC ztD9=LRo|(3t+q4@LW9bg-c<+1jK7BRbu{ODYB_-YXao^4bwXY4L{5FpUia*yX!+}I z5^W=TkY%sRHGfa#=J6Okj!08#2S!f`I4g#-Qp&2-4K;t0+sMsujP7=b$PoTI&3i9e zxj_29jZT_FNN}A+1v9)Q(lK>VSfdK&hW>RMd#3p!AI-)4>VTA}$hpt`O%w^Lj4+Kr zem0>_^IM&_P^u1eJjgnjXkWqv)SJjahTn3S4{_elgM|`UEf~n^B-JiArzjt#JnzPU zPZ9z^j;Z@3Ab-&^@e&|^4%4`j4vPYilNyJs1Iq;046TjK zhUx(thoSSzw2w{4e}H^_7yxWVSl6hpM^)E2-WmbK#CS%Hrj%d2o{^zcc7oj;nnV31 zJ2I9~mN&bhL^4WfCF??CN4SFt&b8BR3SX~Q0SLz~#0i8+8NwIz2I>tJ4$zh8qrQ#l zo?yXDZ%MkJ8TC4;R;?02g0UlP^Leek+n4F7cT;|VuFuT0g(YgV%%u7Fg}$8u)?dP* zkC2VM#BWTc#aWFj)168w_Ni%Um6jH2Xbzt3)nh7iH6@kI32fTDQhb5p)sc~2%;MY# zySN~N78gYC+-#AuJWzh;hN1M#faI#05=#nL9k57uZnlmh<{Q~6eVx|2znb2E-wDm} z(%%1;+`O6c$WJCd_i!%sWO=`)97|GvsB-|l2;>&kQ_5j;1Yz_dB7aL*Fon)jIqVFe zxwPz8>5MN^sc-I|QB66RM8YT2GQZ;zi4VfMjrRtu4?ODDN7+~ba`y8WktvlM7I3)~ zlMz9QjP$3mWOIo9=~}AOOy@sjH6U_8y%2e09D3@7n6Bd*h>RTL(TuHD^TtO85dLDI z_L>f(a*8n`9SNZyEf*H1BWTO%2z19ntVu^}53ASpbFmxJ<%4Q2gRy*-jER1W~L zC%=|yJe_+U^zVnPpgIf6s~&WtgI=$T0HG5nt_d5Ih+X8I1#B$AiDwI zK{1~8P4qr^KB#x(MJu*)v3fvcc|r_0BmtkfeUoIn18lt;fNY&g>zNCFIZU4~NEKTz z?#tGV`GLX*{BAyaH0?*J3RFrri+4(U#4qF$iO4{DC*KWt>Jhm{75d%vh%~5RKM`?A z9&wY@AzsuKxmW>0L_eJw@#jnYC(G**S*&K{8`wXMngt+xr@$VHARD{oq^v1{iHfb1 z1FfIXWAg!*qy~WeXaek4N47{FF(~a2$E6-U_YtyHZ0&UAdpbmibVS@FvFk5Vf{QgF zAb*Vnt}2712ySHF0HJ6bn8@hz^7(QJ0E>!HzX-R&EIv}!+?cyxls zN;G9@iu!)OO8h|vSTv$|axm33E|0jSQtux3vP&$KPt*;&C45o^A0ji=N(>gN4bDrL zW7#<_zYfJho`^UpN5 z_)ON`+q06|u;x%lCx=UPvp%}A-zV7vo(GVFiIzJN@l8_pH5PVuqM^p>4`&Y zZ%4)#2}Gw}kWrHYu z7k7WhgF1&kiZ*BasmCeXNA>ayFQ(G#qK?1gL0zRWs@q(d*6AyKU`0Eb4M09fX81_; z0Du~Ta=W+U+9M-SWA};zeO~7M(+Jei(gH921!A4l;ral?M8m@S?06|ITtVgWt8OJa zC;-Wyq%xVgtngZh4eBntPoNOXfvipa+O0%g0uUw*0hi@o3o)1OvMm(Cyqv~rD1=V{ z5=&@H-PZA0fYsOzj|-KVY-?zQ^VTLrK(cK})ST6-PcbC-K*Z1izl zN;&xSX*5W5b>G*jU1;|YCj3fY0KKRwO;c32(G-4aQGmf~RPJhr*dlEb^$%!D=@gCf zI5W|;jw-2l1NHM)MZj`W<(y^pCsi08Ec`_M+2HE2Gk1TLoQc3z+=$3Jjf<7@2;1N<1phItivC+JyuGUX3Esjd@Y(VrqWsOzqFh^^8laZuK$ z&;E}RU(i^W{u{ZCu==mkvgvAf4p3X^wJU*#_xG%<4$Z*ScF+{8el8URY(LxYW}}*N zjY?%jxpYnjFo%FB30Gi7J&@<7`ueH7$*u>|L-*#iEYjBl>6*&yFMEHX0EA2O0c2zE zGY?!?KXZuv&wrO~Kx(JknE!ue+usjg=!1IqOn5f#PV6MjnZJXEvzw z8txKjO7Ey9A6j14PtU%2YT>XGFT-GaTv}U*j^|2C`+MC5ALmLH{RIu57k?$2aTX@B zUt()Ar3yT=5ePSB9;P0Tyy3^CU%7k75c|Ry%RM*7p~uCqqjs!opmyjh;&cR;=8A5I z@W%$4caqa&m2M7nNoXZFxChBNsY znOp`$^(Hc3MZ~6Nc}pZmM#oG0^HVaALDe46c}t`c2^_HqKfrp^_QSN#i`oNpALLzQ zz<#_~Lgd!uYRpl;HyKD$&9x+e$gU*k`8?PZ$Zpcd?gfjqG(hz?hjee1O{8m7Aq{*< zR^Pi6$P@h+eL-}L?QTB8RijuFd%emzI^8anTS9Xb=vnF5WLUxBTs)r`be;Vbm=|b1 z(Z1Gwo4NzTtrht8Rv^s@5U3>Zwe)sW4|q21cYhP1{`lTa;24d5p3Yf0D+B18_>hhA znd!U^L$fpza0pqn?>|R(ZRRlC;qedxO zCAJSxdbYbLJx32^=tu$KiyP1jI3te}3rDJz=ZqEOq+3Vflvw1~|f9D4C z>zcA498nJmtMAxx$BwqQ?ue+b3(OAvgWha+cvE!xvjMjR}X+K zDFd{J^w^Z9?mZLEwBPFAY5d33|E6*EB{Xgib!?(is{q>qXd5Cd(iD%ju%?_Cxb-D= z{HKwb-@CgN2&<;OY;dHMX4_pK;A;fJ!eBA|1i^kc5KBU>-^(;&&_opM!G}o4=JbECd_%*O6hEmF3C$9@oo9V5@pl2{ACQm8*r&tLb+-U)) zlzCHNKR1Mgf66K4rnD!6_nR<~*{aEQqX%MUrm{v?vuUF(P~^`I2!6j?cj5-|?Gdz$ z4LmJY4;16I5|R7PL9^i~YXku~;6!^Ja<1hQa<&A38RI%&SFXpj+~b%T>-)HLLl8jb$9yEJ8Mm}aWwQ295FF<2w&KG#riP~9h_?Vp8p zQCQ-M7-IRl`6Ng8s(aN5nqRO|3!Q^z*HM}eu|+f4*d&NG81-(jL+3Zz;)km`}bK>Fk?L}KH6uM*6Qb$U)WWYKJ@DXtvv9u zMZlHwfc$cj`X1&>mUNCojkXZ~Ud|T6aw&dOZX@7%%1tv$e0UaSj-~a4hu@4r*3!`| zHft0a&PB~jxDx|klDb^1DLcZ<^e%k>-YZ-fMpm#s<9-Bxb^3FuD~zFIHN9>&(O2?crA01>Y@3n zi|+iCfS;DZr)g9!YD$I1;lcp5#ekS6srEnTMB8@;KN-_MKnVtbueDikN=Y>5@w{(6xY z+v`}&<6N940JPWLn8qkF0?U^5XNy(+jUw0vN1&>`E?-pSn?*$iXpaBnp54i_g))Op>Y9*Mm(c)HT+QA%w`ERi~Ji7Vqy_dBJ8dH!T|R z*U!OLNrz|RoBKaem-{|Zm+$^WUB2%Vb$Q>rwfZQs*~BZb6TeuOQszwEsB$Y?YR?{9 zTT701*ZoU|@RtSB{i8Xv^l1-R0<0f|k|5PPr?X4((m;I~V_zO_Bj1P2w|N!M zhUxt#6l0}q&z>+Vg7CjnZdmRZC;2F5H&w!a1Y27N-iYqYlA`q-{d(q|e-2ZZt!(ss8WmxwjkV3Trqdl~= zLcL1N?jr*<2X;d4keH2KZo|>Hr05M`>5$%%P*sN*y`jBM=S5etqRz{#_oP>jS(@EW z^>9Z6@DDWr-;suFuVcNe$a*PEM_MPpeY&3J;nG+Rhj(L`|8U>qD(pKPgB&3FI}XR@ z=NKX=hO*I$>#=}sQg_{_!;h`>6?SaYH%?%S3ZW|D^QFiHL84a8?O3?Ggy%2^mW9CPN3MjHq+ zmZ)ey9slpqEb?lQ%KJ_pjtT|%*LqPe-5_&@z~+mPjVJT4#)1zsf-+s-I-G! zt%&>Ko+ zT1!vRNpd<;GN#*D@vOgxe}rTb8Y-FF|i5tP+mJ8$CA ziMvkKoHe5&m$_mh$VP1>H;aBPCgz+?5&eE|O};wgDyQR?Xk~n`v67glYx$fl(PDyf z^pT+3cXpRASj>zW4QGREN{$BC5C1#1zTjZ6g6P@9u^V%Gg0{%Zu{I_eG*&D)VXQ1a z@yVAAsmQn4Y&M|oL1wc(!Th*y8x#5dC>Vbo$;{*_*+b0iDLp#wiTgZe>(_D- zG3TI&CqC$Dj&t|?Q=UJ)3e1WheZgB#%Z+#s-Xk~SZn-$_YerGD42GFyM}yDGtIz7$ z;!!qdPb4lwn0*%bFxckSP`}WEDD=#&$(Eb(CvtHdeozp;(X!QIlb97W^X>C* zG591>OqiU4u)`u)isQZ?6owaBY!X^ACSMpQE2drfp-gv{em`A7cFf4o{gS7eO1tac z{dVu6tB-$S$A)y&6v8wH#N)g;++7+*7Hb^tIhZ_qj!Dk)7#$1nLah7PSrv4HidLRI z**;(le5Xp0zh8AurtSIyuz+&C+c(Ycr~c4t)rs2&9FO!E49984GZ0?Ad{ofe!+N|( z`byU0f60`0v6!jpaznJej#W`fLS}I>&fSWtpkg9wE^e=5usMWk9(Ctq6fG<^50q|2 z%V6R8sG80>6!4U?Qv>_Nka=KA35Tg~g7X$ig#ni4?gYB)sK=t50fCOi<4NVE0UE0_ z)|>DwJf$qx=GVO9=Ayd#DdjGW=7$uhD0@=*)zr8QVLJoDlycAOYN%r?hdbRlSUBpE z9swX%P3o}}`1MwwpeZjTusR>w`+K*dRnNF}r>Ioek)V;itl|Gq=YyvFG(lvvV!$mQ zyF>hKI|yppPMB1_A67#gerPLNWjHEIn)1U0^%ASDlyb-6_64s(t90TfXN~#0K$a$E z$ILnBqx6fWv?Ts1)7uF&*T9eG;JH|jfgGIJj;EA!DX#3hL)T-V@#A`omTJm>Yg`$7svb`%@sxe|R18>p*Z8E$4Q3GocVl>+d!6bKnnybkGsfk0 zNdh|7|CS2AlPV{0V;^uWH?lQ95UB@q?Tu{B-fv5%McU{4=_sfAi(`05M$7qy^xoFT zQHn)&3}v;{HWT2;$MrZn#vo0(UgOI8hiG5TqwjMqYNhi?W+W#qXGWo=%qX;UT6~V? zaN4p(4(HIFHlfFApkwR7u_HNUN2io~Q@XNQLpBTx4~5Uny<57?-8_1RyOdRG(oM*1gx4wQ_!YG`fMiX;VjW&F)UovsV@Gn9o`NMy zhNcuxiYv<*+P}4VRObIUb|l9%WXF6AAx#-g(RijEPbqa5ZUacf+@jir7F7TPF(5rL z=$SE;3uAgSkhLm@DYP?HFj~%4hp)nCC=T4POhPm}1IT*`9iCFMQeZC_f`v1N_NV7* zJhe&s%-o_{fzF4{rB|xzr#)q-+6WK2J~Cj#cVRAOHY~%1(oGUy#lH2Ldp1_0W^3l& zp*uz4$ERq-1gZa&+e1H>bojx*@{Of|4U#Faua3jix4bq1ws%IL>YWj$O2F*{%W+aB zp&3|@AL{cRcv$-JfsF_J?urP_pR^{pvizYc3A(mOAC92!fQ{{1xp7nAti(Q1B6$N6 zz4cP2J4IoaBm_+Hq_)|c+9u4sQjFR6pLg5Ejbc95#dzKMD3pJdrpJi7*-wpO}A}Qrz?JGH@?9{%L+lY=!XKHb0WM}%G(ZhW}vXd5EiJ2EQ z{Ve-Jnbwih~M^k2}xH9Gxt-Vqcu%XX6aH(dhDMQ*t&2;9$IU(J&&Z;SY zO-Ioum4_~gqSJ_-*+sS3bcRxyAJ4+`aKlhuKalGaI?ODdb=ry1OPX?1iYw!WP*1mH z?{6cB`{4nwtrRPUS7S}7(aeE1V&dX5v-r5m@1B3!ETj~Nro+_-jIr{p>+qEF#3bb= zcAQcS+OT|5UFZC1V6J!_eotL@-IQ`<@-Fe$D)0O60FjG2CpA)(=Zm9PWRMoqEN;#FVbiei z?!7;jj>pg(&+3pz;_%vj){5q{(w_ST%r$6AZUSs}+R zTYvOs>9+pC5m-ho!TjM8>8woO%woL=f@wHYzEM==MhPloQED4f%aPt3Cm|4+1?*+H zO?eGgDO)P(&lbHR8OW1@fiAUhaMpvb)T$@HCmmB^@W=?vU)7I<7V%g2+r($@FUGC- z3)vyIhQALIoqE2HC8*~+rF{Q22p3fXKhs+z9l9i2 zh4s^wd@s!U>5Vxyrd5>$-wxB9lVibm!~BNK9n_4aNc*K3D5s~-j-1?($(KxLpimtl zr)j=KdDC)@<~h;a-XCh^k{zGvKIHi1`TDE2GWKjR@lv4Eutxe}f z4N$r;-}+3ML)2$V^#JNgnu#s_^nVqo%_&8CBd z&VymNY`JP|2gkuMShM_wF(aE_v$l4S?C|X$GqO;5bL6laorEPfMfbRUq|^6IaXyZY zpus_v8>AWNOvEq!Mde|Sn?}#BJ(=b22_7x>*7O9kd^R8N|3v+2&^2lT(RWy71Q%vT zO-#P$WtDNJl>4W+7?-^i-$=bt%%t-73BeDG+z|)96`vYGd0Ary)AeF*;FQV>kBZL( zD+`|pKI^vn7I-U8ZYv1~f4k;9jktME|C^Rmsq-an_FVuw67-}*F8j4OlBA^CNcXik9s+WWdRpa%PmeG*u8(?s){m`5;3yfQ_jE7TWWKL)w~l$(_PWdy$ddmB zak0*FN!^8ZE&YxCP?5ok+VmN$?z)H-(O=RkEwAhNy)^dxUl<%N(&2m5JqOpF3{LDA z_Xdu{1f-i_Q^Ev_J?mOMI&^=~BA$$1kGC-`LPXQrf)SWK%&8TW2?*2%+# zrkJ+WM+&#^cDv5b+Wtz6FZ;G;@~e*Zx6q0!MT7v0`Lt9?iRU-oy?yluS*| zOqPC+Pnw9;Ma7|Hip)%!zsDy*rQ_>r_SG7u z$7hc2tA(>CeP;Z^7|+qM2jxyTe-ai03HZSBrxDQc={t)DCOTqJ;f?NhGskwg;cUNe zXYuW0EQ^G_7#6=A`O^qo`&;>0*O8dOL@A}N4J1=!-o(S|%%nNDe1bXqj0=+dWTj`8 zsdg0AzEaCguyNzb|D<$S>Lb7Ky;AEsyUWK^ud4a=m`N8(reZUbx$97pPy8}iJ8G(Z zsg}+T#k6Jj9(}3yo|DWK6w;_$JqoM0%5TO5L#N9+VVGOS8lUI+3ticJ!9v$|DRyF$ybF(gVaD1Qp4u;t9vWk2ZX7e| zQ1!5E$H(Mcyi4AW`(++)k$K!8(_OWyCSRuerCg?RiuE)8p5PO~Y)_Xvo8J4L&Y)!d zJ;5Rmn$DGMuVek@5a;g+z7VvHBDUAvHj0=ue234neLUBX{6oP|!cN2Qf{VCKQ9 z&Odjt!N)tXTkOCFrGZ3Ua5(bk^1bwlrSIn$b-lL3Jk zKv>9w@RcDGgGHoXV~SSCfMpuq9041Zw`y9BkY-JIst=LxPEZTsS^B>`RE8W;!TLmdu-c#m(Sz(Wja^f@gMsjWJm}*slNfQa>}(1-4(-e>eg*hRCX3 z=xSie6d39soXaK7SH)kNGk0U$g=wUlaN$4O#qoc3B;J9`6fV`r%W#*vL}(#v$eS=s26hf z{J3GIFSmy!YbJr%CmHe&^+VO}W5_|3=JnBhRYcxO^^Nf?*R_5ui`8|3?eqwT?riLI z^Wh!tqN9ieQ$9>FeiSE_#&2=1oyM7uh+c|w(bnzs|DLrc6=9%Y_nt^9_HX;J(LNBWV56(kvt}n=^WUgrj<@=sGL(Kq?-(~;tHXaEfn`UE z^I;)5Jgnt+OLvY?iXR;C_;lmMh zAiJRoQ_rcUl%^2bHMJdQJ^scQ5T<9*PUlDQr1(AyJdMR%_bkoJBy~DWy*kYVjGw6O z`--onwkIMVslx%?D6rI1m=!$`=yn%G@*9u&dx9Rqu*ZCqll2A;tGrpjj|36frwW1b z>DqB0^~4NCXinIPjw@?lkP9+6q^GrnF`@0+*%A2OvYrS*606pI~r2JXhoBR{g2 zMJ~5XQLr_i*Y1730gct7FAE2Q20J>@UF(V>GH+_HcW>j75e9YnHG$&eV!M2ZJfLQx zo84a5`L_$$vDNI&3&0{xxAoU9i9X)gMKO%~wsiTvlz(v%?lQ1Ec5Mdo;}ibcQ;}tA zE|>#1+07^B`P%(xsW0dvJhc0msxf()nh*Qjg<*@D52&h7-sj#GF7*}q{=dq;1ip!C z?fW@1Ni$8-CT&{i227e3(x!#9MW|FIP1>PN3T0IklypJD9w=%-ybXfa;EGhdKt-$Q zdqG^9V!f0KLF8V=<(jZ)D`4w#9h$0lQVKI^O7s0rQWWp^`o7;czh65!`#H}!=b5uS z&;RkUI2F~NoL9Vm||ILEMH&$&AqyP;Wb2$Fu0cd{1~Sc5=e#G%=&o~;98IQ~UTfXp zIL`W=`P;X*?rg zm@aXy{fYz7qYz^_JhIX@peS|wq}5(naSG-ZewuS ze^UcB^SWOJz?FrR?c;?+!vZ!qGHQd4N)~b(24=qVg9(2>L>xkc{F+cnSaQny<0cAd z%qQ@;9io7ikBr~7F9X&5X9SY)IL0S=iA)+J>6B>Yo!ys#TOTib;33&9i>7Kfd7?V= zwd*`nv~?c5QZ@Ii)((1awOD&ZG;n{#+`MkLG0ko4f3s$xsaqIuq`6OAQRoi{auCzN zy)Y7EoZE2i2h&1RBrF#GE`Wp5sCv4E0}ewvsPVqwi6NZlBZCJ72&b(dJRlI^n6yS? z9b*}5A7dG3*VtY*wx0o7XCwP&kUFc{b~;s!<LH;-?f@0wKHV{ks6o=UKT$XFbNsEkRapQCX8UpR+QH$~szenw47otki;WL@~`a zQb5;JK)+7`{im0KN{a^i2`a(|FnuviF?|c$%>9Wl$bU4fnW?GBFc%&^yT45;8p>BS>vkiYAAlT*dfiX=Cv#TJn}{H;dG_AMMj zKaZq;DjMia zfv#&)Rj6iZY^s?W?gXi^##>Tt1-1fPoF&N?_i_E^@1^~D_%f-TYRx7O+dC#Mj7I!BO)(zmIbQqqP}9Z z#W$bUR4biBnC-#!ubh6P{=q4J8 zm*UITLy+$s9B=<8d!r`9nj>treMfps1?;44lv!RL%57pq9MNj#&QBmjP zEo16)T^MB`#Ak+YgcKFDEyx=K9)Af8c;uGUdR$+kpry~G_~|ST%MWBTgqpU6cw;Dx zg;)Xxc?lRUMgxue2iK>tEs75E7ll!l(G^KnF&*T;6cR0?W+qunuMsfF?-?@plQ78d z8iI7CI?j@ARTiaGWm{8i3dw#JqZ@w&hiI>=uRAdSvy>`x=H3@gTD{Ngyc zaXWFjxJ=xB<;WPJO2FWBnjZ?)bNs)W1S>-VGQ$&X| z#dwc5UNhN@8gD669D+u$#U8@aPl($ddXzchQmPBbg!m7IeZp;e&`tB?r`I@fU+xnM zm5^2F!K=a=M}c#Z1f5H_q&R*%g+v@9bPLa)xY!H%Eqx?a$pKl_M?|NYEv1y(SbBn^ zwxn26%Tk@G=6Idc9k0m~Dz}7fx)A4v}YCXwnID%|y zJ;hdXApHpI*%3hh5DoM%n;WLDjVFt)B#Pnk$oB>FZA|CsF(f=C1-e!W^a~WwyQLVr z1JXb11XP(O>NRDk==gQ^#m4f7{Lm=+X? zr5$P|+*;>~G`GnEKO@rEHjQ3lZw_vSHp8s&OJUmJY$# zFBN4P$1vX*ii=1GXl`e5o10o!u?Lw-u2!~B$kAI$6T4!Yo)#vxsDx;U*PzDf7Wo@k z_F@u>IfOVS5ba0NprzcU91{PFh1}@_>)zQX==Gp$7+C#IfpecQiT;h2ITlQl6vr`P zvOY6h{=9I@a4#=lr-sYz7GB(e%kD6a4ws$R(s^%;zknnf<25nGHb z&KzM1Fz&h(X#L3e#k`^?8fZM8G5^5$r0(;n}PC612<*9KllFV`=O@|XBCa*QC>>bV> z3{IWo?r^R$C%HeoVo<5<`=5G_X=;T$I>&(rGKvSI$A;TmV<0IXAS zwB)X`c+W7uW*9U}$`_jR1VV-iC|$Zh$OJEu8Q=3kFO=?MQRbWMFu!&fBRCBuLyNE(Nv&M^}@7My~OTQy`l#& zO|cAkiW9V=uQ5*;U7{-a7rUdh(7hv!LXcQ;jRo$@j>!Ez?m0maF?h!`Ai(%a4pAag zf{U6c_Cm&ZhQ`_m)c781Fp8gGjP@Y)(N>^=`(Y4EEQ|?c_#`4UQeG)N7>AS;(CPQwY7xRjU4lMwoB^l+rvd*-O8WUCV z{lWs5v0quY#~9VAd>^=0tR=0-x+H5kH4L)g{-GHt;0BIhdg2!RWuyg-f;cQmu|1e$ zq7C-2WdBpG>5NDX+>?Xp;XAiN15yLG?I@O0BDB$+X_|#5u_bjO=;UsEV~PDEH}^CC?HjbCFW{TRbXeFfD^EnIk4PxL8)eqoV&25@@AKBP0OOkrf5TF^aSS=Y{o@Lj0)^Xl0j>Cd$0ukS{2eY9EQx1*J01 zDKn`Yc-@HOVw{;YOX_x&52ErskdZba6XU6rbPHAo`8?JMO$ph<$P?O--WctX1V#O# zLvsa+Ci@;EO@cfJ-GVW>d$DHx4a5Z+9hxVIQuACD%aOi>Ln2aJQFV1h&pm=pgUiPB zip-S|&kloju~%==G6h1SHN)tOFIv;8U#z-VwYcWRnu|4QwQ1Ea)=E6YRj6(>k{;r!V{W4{ zjKyrcr#3B}2#W7dDDSsPa$AG`evDKK z;T-d*?em@RNux_m<1F{w zeRnd}1vp#%sclU8;h_HtDr&4ont4?;GrlL1hiE*E&lg5PB!^LuM&IcyH5HiVITV(A z?p~OT^GkAnXp1ec5 zMa+)&d~+5<^hGOo2vl>WFFJL0*s?*cS;TNIXi~M~n2p2x1kr3POK=10H`9#96MfO~ zFyV|Ka(v3dc-w>)=A&?LUi z<^^OPWXLzumscN-TcNSxxn5(FnN-yt+qX760$#Bplh>M-zQR;oK371^#r<*dDuc>{ zG?ko4_KA?77x@z?EZ?5w3h?g^l?kF3JN!s?)%&#o`U2A@7;E4bGFRl!5Pj;sp&vXh zysnZNd`MOF}f)(XZk{XDZl(rd zFcPD7^8+rFLwL|$B0IX1?E8=$`vzG;^lc@-5r8%c3v=XtB#-qgV{5`X!>_S7#xUmD zSf4DG3Gw805#wyfvr>BOvJ;VF8p#fmLFWpiA00! z4W1w3_r5PG7h9w@VxXs5Ws+IlS1O+!X5~Laj|c|Ekp5^(%hgQ{(VdW9tBvo3bg4GE z6Vj;;qQyP~?LCh!<;icdC zAsxDe3cvi-PuYfVCIh@Dh;Qx!{AVL?^N=WfXBps2hYY?FKw7yQ-`(LGzyRMKG>22Q z19`~I*`5(M~9gT>+ZyIa0+ z`!aHnftCWTlwwT#5dT%EqJMV&wj``Q6=NyxBgCn;I@j4=LZkHOM-}@)^0JX~zhIP} ziR_=?Oaddayw;lL5Lwc!!m-H=&n8KM9xnyDFb5fgwm3_wt;pr+C6=A-B}(5hQttmU zMxBwB5Aj!qu*7zV|Iqy+gZ0;c3%_;-g{8Tz3^EV#|9T(XI8Q9`J(?MN(`v=jOK4Ok z!Xqso;(LZJ^+st<^@=o96V2?eif%g9OFZ~}Rfs=K*>6P}Osu_g(IvGWWJS8acIB7lo1Zj_y&?Xq;k@vv-#%6^5TByFbbrc^s*j#nLo=5aRSALs&2x>-}FvO3rr=Tb*s zN>l6pt7Yq3ufDU^-}$OdmHo1NPTDKh#B#L|uO~GtS-hid7})SmtXZ`J6{xB=vLU{G z7~g*i&*Jf!fyXBvr#Z+lUkdc(9Av1F0zG?VoLzneD6JG|+_%jm=VACP44*mR^D%s; znPf?4lFD^qjFv%u{ID8pK36A}1^8>%qtpqO1ao3pLKs_hj}>WD<#H=&EwPd`WgSgF zRqeFm)6XmLvV8${bUOh}Cf8HzIq-8$urM4IMMUi+X9O9om4q(3aM*0nVZJ-B( z%nY0aRJISU$Pm8sRar~>7tF<0NhD^ab&j>-+GoZTD^4|Hk|}OXZ=`9T_SCEn!*>aH16^9&tN331k|R!f!OK(l=kkd0Eze3}Xp# zGa6>?VG8Oc%oEHG=9rK%TdY%bc3&nrQW~GC$y=Z`a6dT^t+(SH>jEw5B{T(gL}Q)M zLDr}Zc0#({vfFOAq}WM~pIvQPZSJ(I>QRBpF4FX}E6QzVS-mG|XFV#o(2Axt8b7kj z?B96BnhR`=rK%#iew$DNDDQM@b6T<;qXi!^8o0L{Y(1K^&_hfD?w|wfcpY#wZ$#Xr z>vxqS?syP!f4VNu&1ST2dHNFq!1o)eeNF-N2L&tzxcP3P&q#C&lV7RyRZ}xcMSfBw z_KU>iS$vXOVBf>mY$T_qqD(<_Tk}SfPpL#4aeWShxaWf9SRQByakH+&)!Ft;K_u_l zm?;qQ{TovWa=>1bWJ26eLpbik;kfxjh$|e4OUOTFaV;+n<(4CEWe{q)t|e%Z|T z8@2AFbX>;>N7%d&*Ol5Asp(TY?ouB8wVsF0)NGXcN$Kxv=o-RxGX#qn#PLDIT_5`6 z8a9RFW{2Z`8$=vC5|@yF%8KaCteD=!5_COF(kk|2DbOe7K%bTZefr~%?>O|v8+TBe z;(j&Z@F7|`0F{+K10=a;8&Hv>^%}NFg2zqe>RFGsh#Cv(Ma*-I^5tVfNi+GJL^r;( z`?89oKnJs69W(<6@4`u_shn0H@2V+7+)IH2IR{L%` zJ?MB(v=Zw*W5(u z`F7AbJs4# zr9gin1^P27(EpSIJ?d4Uro9T(y{`hb^;Mt_yb9ErSAiOS6{zg}K;2_mRGw7tWTf@? z%ke?Yh&VW>V@GlwthPd6r#*Ntxo@C6wsNKBU=ee8h`1PYR&N zru;^t@83eqGtMiH-`g!v&#CN(>P36!m457gOdy&oIg#{Gy~^>%#=eav zAECLREOKOTTDd9m`(a1+#+4)f9ga6P^^N#X5Td3Zs-IUnr3=>sqk_(Jr9WKra2aFr z5$e;*&wC*bbAc%W_9Z4qa+@F!)4ylaERP9{A={NwuVzxp5SJm0w(JrZI<>xmZ4y8a zI)^vXdQ}sRnBOEHCe@N2!3=Q`W?l0U|xPKPx2kPzpKpou=6z(_Q zexPulIramU{u)qc_5+3cQv4dmaS@G;VYatqTv*N^u11J2w=n_!%OKITf-wy=*I^lZ zq7iY6gj^R&uN>ID4xa(OISd()Nm<5NL;TS}NqFTzGf|c?*60>J*596tWhJmY2`2@* zQwnqsUKh5-Fedd7KW#XR!6!x^gg`5I<8#T79^%SYo-r+L7tcnYT^KDT+9sd>w_p0bN6ncrMa_oMpuVlsVMPG z6;eUmcTU{gze10=v4gl*-V!jctPLP;#h{@-wBFIW!>DyDnQy5N`ifPez7I(e^K&b8 zuq;_Aq@`VG6%CR@#XdO#AuWwMXiK9?ltS8PANna&GKl=Yh8#GUhDtv4D?wBoC#1<` z1tJp?(kf(rB)iz!#wMp@{UP8QhNDOHM=#@kA%On;v(Ki7_&>Sv`Nr8K)Cig#6_{h5 z>IL*n`BA@8p|Cw&?qTH4QA{$Egk_aDCH2J2ZeemO+WUeLIKh!QX^JO#(qvCU4shL$ zsLrQcq8#8pa7<%>p3;Mn+nzT{Jcetb_0JpgJcj;I{PRZ6n^(UnEh)Qqfua9Bhpt|s z8}E6bTxLTB*hX0IVnvpL+wB1L+Ol&gPZ`r0L%-W`axuyS0qL%>NCWA~%;b7Sj)7}& zAWe=vnKAT-GE$h)44Tr)PGdMW=T8Yaq^Gb`?h@sk_kO^@gd?SeUGj9G7h7Nl5tQWb z6o{~%L##(tzp#9#fO-}!)(pm8*(s1cavv#|55^8*{RU_f<)Z>p2aW&oB5q3HZmjDU zs^m7$L0p4?xW@wWCg@+G*9fBlP5qP=asL)&IXl_mN62K01y=3T*RL$;5Glf^w@E`OAgd9^Mwyv|oT7eUGR(il97*j%`Y^E1Ht* zNQ2an{xqxNL?lM|Aw027DRw^_yH{mb>{Z!+VX;l1>DR397_Nmj0R1D2Y23SDdV5yA zB8%{-QWJHt0X^g;H@NWs$rgYD{bTyqDN&VM6@& z;Y2(Ow%%M%T*sEHFNEdoUa;qD3*GrNVL2S+xQO*#e8!3A%ZV!=YGgWW!&BLL;))bx zcEsfg@#Vms7zS?du(BMu2SSK@SfCxM9=VQam3krD&_A^P#Fb*rn{Ii5vVib8;5$~K z6MG?>{5qtO*|COK1mpLS>50?fL{$mK5|)|eWh$%O9R73M^mfS2s31W{3q&fu*%!Rw z8v*wbInXva(C^EEJ|PEswjAhMInejZf!-k>kttW>vkYg8z&8gWXY7UL;k%Xn*DD9U z&BGixF+ekrfw{0I%!P?Y;PzaXSkkN^9tlxljvL}zLRiC6If9sBl>=Rd`&OfGOJ*>K zjy9{TBY%!)v&tSFuJ6fSD2>|^u??v|25wxx+vGqO%7He>ffmAhr|1fe4Rqw!m}6H3 zX={i-8^ZEB%^|9twnfSevQ7f?^)Z$h2KV;l3nFcPSYD8ns*)!H))go;;Fbz}PDiB8 z0zV-bk-wSaF242z;NJp&_$Xw&X-xRKo*w339>y}BPk_M32eJDv!#E{&OM_@~AB$}) zf6o>k!(}7QClul&2qT+pT`V&uxKUVdJ@PI2yccPXnWGpyl26?j$>Mf%9I0?{ z&p2>94V=?~?ZF}GyOwUD=h4*qZUGKIEhOpR5|Rzr^2uAm7{liRG(-Asfl%%i$Q@5p zm02P`5|#T<)?30T`e`9?Hn}US^YoQy0M0l3YB_KluLC#l`YYwYO&kVpawv;=OBe-F z2SWU-AxI~iKx^QhaTE!OG|JZri8S>F(RW@4s1xYO*D1t1hB0lX53GMDzNyZ& z-qIKduTA>nwy69Lg~AP*HReUevy0>Rs_c-l)Y2^+*n%nNe6Cw~>XmNcDc23k?G_0A zZh?d@0cIO~Bn=vAE~een!YKV)LXzIVMQzzCRq3$X(Eq*T?h$(^N7$W=g9QyVzRHIC zhnjDmKNHthE&WE89j5ims?kIxCu-kkjvN9Eoc&TYim;>2&$OiR)kuW|Wy= zgLEYm$A|~fP?}GqSl|Keou1puD>)SF72pJ@Cd~!vd2XZ!eAp0Q?Xdm#( zvtMAu;V-g0wp;l4#BBoc+nLxV=QFu#SAHLC|lf>=_<6O zIHc)QEiN|1*IvK)V}s_(k0-)2Vd?cb3~&QM;QkrJ-|T}!z`b+*Eg=c^38R#utXn;j z1MnQSXAOI7t`J{(J@lh2w_)J=+fi@C_(vg(6ARkQ-hm%cdW@fz%lo|YG(q$e;v^(7 zNM14dVo6n&ToB#X(@8Y#F`u6L78F{bq-eS~*=rQ(aOw(nL{P?sm3ch2X7DmzCEVwcM#&p5-#QYJ{7xNeq z^CXkV09`VxwDPeQqK}j=C^dKrY)LwdU{zo(vKQ4`Tu3*@12npYW0ksVBmUFE-`96X zWXV?c5z)o{&ish^;qM6^OL)9t`r+}0>4(pi{@k*M zd6e0~D41dXFGF#&@ZA-io)?zi=n%-UiI-ycY8kwyQZu8h$AmKa^IoVI?^Ny;?Tj*i z-b?JvXF~jKp(wnHcsx?(*Eh;&`?C$(DjjWuiN&6R<)-%6`(=5etUEfpFC$KVJFT4r zis~PX=aJ5$YyZ?ml7~=1|nJ=!50AzqW~xaG-n_NSQN$xjt*e^ z!<72x5jP6Z93rl9{2fZcLv50`76_u1 z=pgJ0@)^UTu~8SYnLGyV4)M>2HLnFe`#(?%>|xzC<-4S9o9%)m>U@Puv%JXFWYf7Y z3g}u+4iT9P3I^G!ZE^-3Ok{%mwdnk?l2! z@(tQc^o3gc?nyx%Q)buCahQ3 z^Z`fpAWPbQVdHJ2jo>_lhVzgr6F%Ypfi?6i^5ShF9_tWOR3804>97mbyLaV^A6qTRU94-G|7ErR?P*CVJF03?G- zl#ISDs^Uo_c=g9r0$Oykb%kNRkWN49Q_>iFr~Y-I6z{6qL|7Waf*OH~4X7EkH$e|v zihy+-l?JTMtZz$T#xPRm@s>F*Y@;E_-x&yxf&)RmE`U)q@Gom%fYrC*-N9omh{1>_ zsRE8=V{n+bML*J4TIO3Da8?27NzZ;8D!?|J1imXs+C(}I+kS`NaM(`t5_$F+pT7{n zX~UQlawf>X9IP6vONtBIs!OqB#0qSu?iwp;i^7uQ#TW?z?ZkHZYJ?j$i=HBY{{1Yv zd=ssujG-UJh4_WXG0xH`CWg7y1>AH2%dO!P4dBKL`6G6VD+ji$3-PAoQ`@A>(}D!> z_>1vN0LvC737{2S#OE0i>lV_SLGPhWizIv)pDD-}20`op`WIHpR1Q3`F2vKv@s0!| zv|?}C4gHC6AwK5#uXGLL->I2w8>V=hfT8MOpACf?pppy8(|n$}W8G;!@4THpxuVf0SFH7< zlsJ4yZXlA}OMJ!Zv7Sl}YD~V_N|SF+TyqfR$vH5%u7ctYcR3XM&aPVo1|NcIe*G>5=GV+RKcIR_P#yPT--&87{$IdsFclkPMA5NYt8oNb?B7y!V# zS2XS3i}C(s#YH!8V6e|P=?9`1)Pscoq@N~WkQiFx%L9WSV4kmnq};UcqTN1YCx;OM zp@MK5Ksf*kG%eu4rx1-CC{WnH9OAHlQNV-Gf1YLKBdtwxSbb1{= zgsplSBK02a-oZg7#XQG>;yDf~6Fito$ZI4NUYwTnT8Jb!#ZbSdGjPKO+#dUk$S(uf zXE-=85Ix}D!a=bS#$l?GJAI_0!I!?O!KYcZ!c*ysuRhbKs`zr=;3?&DZI7gamFeJGR#yActiw6LyxrX8-cexSgG~M&69}&fa{fRMf^+`ie>1 z#w0gxrP_mQD)Qj|qTCc?pRvP-6lrd}7mw!}Dn(ucXx)Y|?c@Eh{x&yU#dO52F#Sr^ z248yhC^yA4_%t<$F%nUjni`ywVZZ_Q8F(CtdcdvZz*Oco7?=1!PPy$fOxz15jO`4N z)-&6O2<(UBN_)V4!yOl zJmm#6NDS4K;E{<(==*-Cqz8$9Qy&30MnB=F2^)Ij%$dLh*$N&k* zFsXLwgOh63uBltQYAJ*r8#Zj5r~mC|J>ZAl0YK9T=-ZQkdVMrd{!u`k9R*Y<5vVbV zKut{qYH1=+;n)DMF%hVz6M@i-sN|&Ir#qbp)Vf5VR*wGTbXDtDKR7ja^`xrx ztC!fS*4N%&^}wnP6Kif11Ni`e0KftO1OTi701^QH3;-ekcmV*!0PryYL;*k!0+0Z} zLIT2twuk_t0f3SKNCCha4Il%66Y&6Y0B~di#Ng5_0I~S}0)PU)zXd=_062+&!0&A& zkoZkWK!gQ1Uy*rUYot91n6O zz=?n4LW~liXW1J5T@j$H|2UKr*gl_E_Ks~MoDzk1P3&I2X88tKzJA@LZPk8`~FpHt2ShVcFhB;R%HQzYZg%W59iZ9@|MnC zIJvxX8eO-cYW;@gYnDl2X6ejn^!@9XE~PUc$e~wNtzWjZZUde9fD|(8q)cJL=5fcaolP&g-g6&$>y7=d1%`Rg|Kge{F4hzvG4HcE9|@)7!Rw)8+Z<6 z|9kNIz?EMoaXu8!rspoCH!NShbnOEh;0}8BT{G#WYpSYOEv+4K)h@4#I2Y6b^?4mo z-a4Sp*8$a42h^oHp#EM5)akl#e;U%1gaMBen6d`t;P;J(iiF|$hQlV_@W+oR5{Bm{ z4$HkcY-G;jJaPDqKRkc&Z=4_g!{On)7Xm;t_8^GK__YuKVK*TX6#_ugLV%=Z07O#M zZ2*wG5Fi=5MM|Xs&;Nx0QOy9vNP5Xa010-Fq*pEkP-6E;`nZJvao9bQ9svPjkU|3F zLoHk;>3IugmMkeLHC2|BE}1vKcz)@U1+(VeJHk!N!f@7M zLXA{c3bbjiiO!`fEtqUG)^4C5tXj3aHr!kE?5diTOE>(df7x&Ta~DPkUrW=AOtz{u zb<^m~+IwlK6l!dk&~&)0%zAne{zubvW-X0r6e;ums^zO_{FMm-0!f%hB61uMFdegH zAfiYhAw-A(Nkymu#Gt^RDiDhl5Csk4DG((=Ab}8~fQ+Vy=@1R$@MjI8#S)N02u~f5 zK?vj{`OC?15Hpg0OcV*R5F!+Sqzcpk%8~q)A_c^alRhUUci!$@!CV zvnOciv?NtbxXJ(PGifBo3wp^zIaRU09Gw%rC;8NB$+H71?6q zxwvs`@A)2Q*O|ZQD&C7ew%PS3N$wH$xiRh?waUMZ>8D#(l&x#O@=fvaZI7J$I;HQ2 zORML!og4V*v9nw6+`US5;=szCsMCD?X(6uV6rsj6V;MExF>5u6(MfWL} zSNu?HJ->e4vRA!7khje^C$e0BEauG0HtFkw2ik5~wY$~$<>`(KhQRx8e!r?S|AEEb zrC-E+dGYSsy@%e^ca5J0-^_8$`Re0QRUUWSz`JVAsl#{f*>&^_#p}n8ymW-nQcm%b z)-L%!GJ0jM$2YH?6jxzecxX)32e;?m|K5(`iw{PvyMW%O&+N`tp8xmu$6V{iwY?Yh z`Va1x3(g(6{OV~*{p0{Bo;eWxLelQoZ#!aEH00jCn)=JC+UL;XnjZ(|tXx@LwJNPs zzx1(LP~HFYuBcTHzm8Hr7@!`#>y+Z^Ke}3$&hGtNyzj{ICpSCWhq}jHsxHs{I`PWp zvrjFpI2T-4)jIQ^bK0hDfcN)*r|&pd{CcSA5ItmgY|MYYf0v0`=1fqPbLl6?{T!@f{-Uv4?NJ*oDJF}m9+}4lA4xfx#e(zn?{HM#7I@V2DmCQ!1T=QV+x#1q&+3oji`day! z`O>nr-R*(EzmGp-I{vrO=bve@Uf%!Zmw#&XzH;hhXW`ub?n?i%4)3zN-v6&lQEk&L zQ(DWu*2Q#Iq)OkpW+tatH;IZr{I|7xLHDu)|9)mo-p1q0nWqj{eVVfM{`Z^Dtj(PA zoo()}hvc7;Gh`j@qlolLX|uXdHIEh*4$xU;eGq!JzsX+FY7jNMV(XUz5V{dzrWP+O~V(hGkQa9 z`n3LI$DTNO^b>vK;d%eH|J@An6wlGdsINZHy6cyn!u&mZLf+hc&)&eJdjAf$juJOKDV!nYA$ literal 0 HcmV?d00001 diff --git a/boards/px4/fmu-v6c/firmware.prototype b/boards/px4/fmu-v6c/firmware.prototype new file mode 100644 index 0000000000..c1e965a647 --- /dev/null +++ b/boards/px4/fmu-v6c/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 56, + "magic": "PX4FWv1", + "description": "Firmware for the PX4FMUv6C board", + "image": "", + "build_time": 0, + "summary": "PX4FMUv6C", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/px4/fmu-v6c/init/rc.board_defaults b/boards/px4/fmu-v6c/init/rc.board_defaults new file mode 100644 index 0000000000..4a263a2b73 --- /dev/null +++ b/boards/px4/fmu-v6c/init/rc.board_defaults @@ -0,0 +1,20 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + + +param set-default BAT1_V_DIV 18.1 +param set-default BAT2_V_DIV 18.1 + +param set-default BAT1_A_PER_V 36.367515152 +param set-default BAT2_A_PER_V 36.367515152 + +# Mavlink ethernet (CFG 1000) +param set-default MAV_2_CONFIG 1000 +param set-default MAV_2_BROADCAST 1 +param set-default MAV_2_MODE 0 +param set-default MAV_2_RADIO_CTL 0 +param set-default MAV_2_RATE 100000 +param set-default MAV_2_REMOTE_PRT 14550 +param set-default MAV_2_UDP_PRT 14550 diff --git a/boards/px4/fmu-v6c/init/rc.board_sensors b/boards/px4/fmu-v6c/init/rc.board_sensors new file mode 100644 index 0000000000..964ad3df25 --- /dev/null +++ b/boards/px4/fmu-v6c/init/rc.board_sensors @@ -0,0 +1,21 @@ +#!/bin/sh +# +# PX4 FMUv6C specific board sensors init +#------------------------------------------------------------------------------ +board_adc start + +# Internal SPI bus BMI055 accel/gyro +bmi055 -A -R 4 -s start +bmi055 -G -R 4 -s start + +# Internal SPI bus ICM42688p +icm42688p -R 6 -s start + +# Internal barometer on I2C4 +ms5611 -I -b 4 -a 0x77 start + +# Internal compass on IMU I2C4 +ist8310 -I -b 4 -a 0xc -R 6 start + +# External compass on GPS/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) +ist8310 -X -b 1 -R 10 start diff --git a/boards/px4/fmu-v6c/nuttx-config/Kconfig b/boards/px4/fmu-v6c/nuttx-config/Kconfig new file mode 100644 index 0000000000..bb33d3cfda --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-5, CAP1-6 to provide timing signals from selected drivers. diff --git a/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..b1a80c8587 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig @@ -0,0 +1,96 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H753II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL FMU v6C.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_PTHREAD=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIB_BOARDCTL=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_UART7=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_TXDMA=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="bootloader_main" diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board.h b/boards/px4/fmu-v6c/nuttx-config/include/board.h new file mode 100644 index 0000000000..abaaef0a2a --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/include/board.h @@ -0,0 +1,505 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6c/include/board.h + * + * Copyright (C) 2016-2019 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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 NuttX 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 __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The px4_fmu-v6C board provides the following clock sources: + * + * X1: 16 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 16 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (16,000,000 / 1) * 60 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(60) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 60) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 2 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The PX4 FMUV6C board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and + * LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +/* LED definitions ******************************************************************/ +/* The px4_fmu-v6c board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and + * LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */ +#define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ +// GPIO_UART5_RTS no remap /* PC8 */ +// GPIO_UART5_CTS No remap /* PC9 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RTS GPIO_UART7_RTS_1 /* PE9 */ +#define GPIO_UART7_CTS GPIO_UART7_CTS_1 /* PE10 */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* CAN + * + * CAN1 is routed to transceiver. + * CAN2 is routed to transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ +#define GPIO_CAN2_RX GPIO_CAN2_RX_2 /* PB5 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + +/* SPI + * SPI1 is sensors + * SPI2 is FRAM + * + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 /* PC2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PC3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3 */ + +/* I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 /* PB10 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 /* PB11*/ + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11) + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_1 /* PD12 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_1 /* PD13 */ + +#define GPIO_I2C4_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN12) +#define GPIO_I2C4_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN13) + +/* SDMMC2 + * + * VDD 3.3 + * GND + * SDMMC2_CK PD6 + * SDMMC2_CMD PD7 + * SDMMC2_D0 PB14 + * SDMMC2_D1 PB15 + * SDMMC2_D2 PB3 + * SDMMC2_D3 PB4 + */ + +#define GPIO_SDMMC2_CK GPIO_SDMMC2_CK_1 /* PD6 */ +#define GPIO_SDMMC2_CMD GPIO_SDMMC2_CMD_1 /* PD7 */ +// GPIO_SDMMC2_D0 No Remap /* PB14 */ +// GPIO_SDMMC2_D1 No Remap /* PB15 */ +#define GPIO_SDMMC2_D2 GPIO_SDMMC2_D2_2 /* PB3 */ +// GPIO_SDMMC2_D3 No Remap /* PB4 */ + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# include "stm32_gpio.h" +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN8) /* PA8 AUX1 */ +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 AUX2 */ +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN13) /* PE13 AUX3 */ +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN14) /* PE14 AUX4 */ +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* PD14 AUX5 */ +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) /* PD15 AUX6 */ +# define PROBE_7 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) /* PA0 AUX7 */ +# define PROBE_8 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) /* PA1 AUX8 */ + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \ + if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \ + if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \ + } while(0) + +# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +#endif /*__NUTTX_CONFIG_PX4_FMU_V6C_INCLUDE_BOARD_H */ diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h b/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h new file mode 100644 index 0000000000..fb688dc517 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/include/board_dma_map.h @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 Using at most 8 Channels on DMA1 -------- Assigned +// V + +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 BMI055, ICM-42688-P */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 BMI055, ICM-42688-P */ + +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 FRAM */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 FRAM */ + +//#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */ +//#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */ + +//#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_0 /* DMA1:43 Telem3 */ +//#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_0 /* DMA1:44 Telem3 */ + +//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ +//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ + +//#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 EXT2 */ +//#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 EXT2 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */ + +// Assigned in timer_config.cpp + +// Timer 4 /* 7 DMA1:32 TIM4UP */ +// Timer 5 /* 8 DMA1:50 TIM5UP */ + +// DMAMUX2 Using at most 8 Channels on DMA2 -------- Assigned +// V + + +#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_1 /* 3 DMA2:45 DEBUG */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* 4 DMA2:46 DEBUG */ + +#define DMAMAP_UART5_RX DMAMAP_DMA12_UART5RX_1 /* 5 DMA2:65 TELEM2 */ +#define DMAMAP_UART5_TX DMAMAP_DMA12_UART5TX_1 /* 6 DMA2:66 TELEM2 */ + +#define DMAMAP_UART7_RX DMAMAP_DMA12_UART7RX_1 /* 7 DMA1:79 TELEM1 */ +#define DMAMAP_UART7_TX DMAMAP_DMA12_UART7TX_1 /* 8 DMA1:80 TELEM1 */ + +//#define DMAMAP_UART8_RX DMAMAP_DMA12_UART8RX_1 /* DMA1:81 GPS2 */ +//#define DMAMAP_UART8_TX DMAMAP_DMA12_UART8TX_1 /* DMA1:82 GPS2 */ diff --git a/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..4ba3218abf --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig @@ -0,0 +1,241 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v6c/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95751 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v6C.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SEM_NNESTPRIO=8 +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC2=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI_DMA=y +CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=y +CONFIG_STM32H7_UART5=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART5_IFLOWCONTROL=y +CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART5_RXDMA=y +CONFIG_UART5_TXBUFSIZE=3000 +CONFIG_UART5_TXDMA=y +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_UART7_TXDMA=y +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART3_TXDMA=y +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y +CONFIG_WQUEUE_NOTIFIER=y diff --git a/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld b/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..c10e790824 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The PX4 FMUV6C uses an STM32H753II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The bootloader uses the first sector of the flash, which is 128K in length. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 128K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/px4/fmu-v6c/nuttx-config/scripts/script.ld b/boards/px4/fmu-v6c/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..f14f0377f1 --- /dev/null +++ b/boards/px4/fmu-v6c/nuttx-config/scripts/script.ld @@ -0,0 +1,223 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The PX4 FMUV6C uses an STM32H753II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The PX4 FMUV6C has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) +EXTERN(board_get_manifest) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > sram4 + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/px4/fmu-v6c/src/CMakeLists.txt b/boards/px4/fmu-v6c/src/CMakeLists.txt new file mode 100644 index 0000000000..d7b08bb9c3 --- /dev/null +++ b/boards/px4/fmu-v6c/src/CMakeLists.txt @@ -0,0 +1,75 @@ +############################################################################ +# +# Copyright (c) 2016 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_compile_definitions(BOOTLOADER) + add_library(drivers_board + bootloader_main.c + init.c + usb.c + timer_config.cpp + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer #gpio + arch_io_pins # iotimer + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + led.c + mtd.cpp + manifest.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/px4/fmu-v6c/src/board_config.h b/boards/px4/fmu-v6c/src/board_config.h new file mode 100644 index 0000000000..d92fa394a0 --- /dev/null +++ b/boards/px4/fmu-v6c/src/board_config.h @@ -0,0 +1,321 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 board_config.h + * + * PX4FMU-v6c internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#undef TRACE_PINS + +/* PX4IO connection configuration */ + +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_USART6_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_USART6_RX +#define PX4IO_SERIAL_BASE STM32_USART6_BASE +#define PX4IO_SERIAL_VECTOR STM32_IRQ_USART6 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_USART6_TX +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_USART6_RX +#define PX4IO_SERIAL_RCC_REG STM32_RCC_APB2ENR +#define PX4IO_SERIAL_RCC_EN RCC_APB2ENR_USART6EN +#define PX4IO_SERIAL_CLOCK STM32_PCLK2_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* PX4FMU GPIOs ***********************************************************************************/ + + +/* LEDs are driven with push pull Anodes to 3.3V */ + +#define GPIO_nLED_RED /* PD10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_nLED_BLUE /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define ADC1_CH(n) (n) + +/* N.B. there is no offset mapping needed for ADC3 because */ +#define ADC3_CH(n) (n) + +/* We are only use ADC3 for REV/VER. */ + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PC4 */ GPIO_ADC12_INP4, \ + /* PB1 */ GPIO_ADC12_INP5, \ + /* PC5 */ GPIO_ADC12_INP8, \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11, \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA4 */ GPIO_ADC12_INP18 \ + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY1_CURRENT_CHANNEL /* PC4 */ ADC1_CH(4) +#define ADC_BATTERY2_VOLTAGE_CHANNEL /* PB1 */ ADC1_CH(5) +#define ADC_BATTERY1_VOLTAGE_CHANNEL /* PC5 */ ADC1_CH(8) +#define ADC_HW_REV_SENSE_CHANNEL /* PC0 */ ADC3_CH(10) +#define ADC_HW_VER_SENSE_CHANNEL /* PC1 */ ADC3_CH(11) +#define ADC_BATTERY2_CURRENT_CHANNEL /* PA2 */ ADC1_CH(14) +#define ADC_SCALED_V5_CHANNEL /* PA4 */ ADC1_CH(18) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY1_CURRENT_CHANNEL) | \ + (1 << ADC_BATTERY2_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY1_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY2_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL )) + +#define HW_REV_VER_ADC_BASE STM32_ADC3_BASE + +#define SYSTEM_ADC_BASE STM32_ADC1_BASE + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* HW Version and Revision drive signals Default to 1 to detect */ +#define BOARD_HAS_HW_VERSIONING + +#define GPIO_HW_VER_REV_DRIVE /* PE12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12) +#define GPIO_HW_REV_SENSE /* PC0 */ GPIO_ADC123_INP10 +#define GPIO_HW_VER_SENSE /* PC1 */ GPIO_ADC123_INP11 +#define HW_INFO_INIT {'V','6','C','x', 'x',0} +#define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ +#define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ + +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets +// Base/FMUM +#define V6C00 HW_VER_REV(0x0,0x0) // FMUV6C, Rev 0 +#define V6C10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 + + +/* HEATER + * PWM in future + */ +#define GPIO_HEATER_OUTPUT /* PB9 T17CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + + +/* Power supply control and monitoring GPIOs */ + +#define GPIO_nPOWER_IN_A /* PA15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN15) +#define GPIO_nPOWER_IN_B /* PB12 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN12) +#define GPIO_nPOWER_IN_C /* PE15 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN15) + +#define GPIO_nVDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define GPIO_nVDD_BRICK2_VALID GPIO_nPOWER_IN_B /* Brick 2 Is Chosen */ +#define BOARD_NUMBER_BRICKS 2 +#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_C /* USB Is Chosen */ + +#define GPIO_VDD_5V_PERIPH_nEN /* PE2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_VDD_5V_PERIPH_nOC /* PE3 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN3) +#define GPIO_VDD_5V_HIPOWER_nEN /* PC10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN10) +#define GPIO_VDD_5V_HIPOWER_nOC /* PC11 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTC|GPIO_PIN11) +#define GPIO_VDD_3V3_SENSORS_EN /* PB2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN2) + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) +#define VDD_3V3_SENSORS_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 3 /* Timer 3 */ +#define TONE_ALARM_CHANNEL 3 /* PB0 GPIO_TIM3_CH3OUT_1 */ + +#define GPIO_BUZZER_1 /* PPB0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM3_CH3OUT_1 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 3 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL /* T4C3 */ 3 +#define GPIO_PWM_IN /* PD14 */ GPIO_TIM4_CH3IN_2 + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) + +/* FMUv6C never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK1_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK1_VALID)) +#define BOARD_ADC_BRICK2_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK2_VALID)) + +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) + + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_HW_VER_REV_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_HEATER_OUTPUT, \ + GPIO_nPOWER_IN_A, \ + GPIO_nPOWER_IN_B, \ + GPIO_nPOWER_IN_C, \ + GPIO_VDD_5V_PERIPH_nEN, \ + GPIO_VDD_5V_PERIPH_nOC, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + GPIO_VDD_3V3_SENSORS_EN, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_I2C_BUS_MTD 4,5 + + +#define BOARD_NUM_IO_TIMERS 5 + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/px4/fmu-v6c/src/bootloader_main.c b/boards/px4/fmu-v6c/src/bootloader_main.c new file mode 100644 index 0000000000..c6189fbd04 --- /dev/null +++ b/boards/px4/fmu-v6c/src/bootloader_main.c @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2019, 2021 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/px4/fmu-v6c/src/can.c b/boards/px4/fmu-v6c/src/can.c new file mode 100644 index 0000000000..3834074f37 --- /dev/null +++ b/boards/px4/fmu-v6c/src/can.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_arch.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} + +#endif + +#endif /* CONFIG_CAN */ diff --git a/boards/px4/fmu-v6c/src/hw_config.h b/boards/px4/fmu-v6c/src/hw_config.h new file mode 100644 index 0000000000..07bf6f40aa --- /dev/null +++ b/boards/px4/fmu-v6c/src/hw_config.h @@ -0,0 +1,128 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 56 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED // RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/px4/fmu-v6c/src/i2c.cpp b/boards/px4/fmu-v6c/src/i2c.cpp new file mode 100644 index 0000000000..61785c89bc --- /dev/null +++ b/boards/px4/fmu-v6c/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(2), + initI2CBusInternal(4), +}; diff --git a/boards/px4/fmu-v6c/src/init.c b/boards/px4/fmu-v6c/src/init.c new file mode 100644 index 0000000000..5a853b31b1 --- /dev/null +++ b/boards/px4/fmu-v6c/src/init.c @@ -0,0 +1,259 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 init.c + * + * PX4FMU-specific early startup code. This file implements the + * board_app_initialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + /* set the peripheral rails off */ + + VDD_5V_PERIPH_EN(false); + board_control_spi_sensors_power(false, 0xffff); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + board_control_spi_sensors_power(true, 0xffff); + VDD_5V_PERIPH_EN(true); + +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(io_timer_channel_get_gpio_output(i)); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the initialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure USB interfaces */ + + stm32_usbinitialize(); +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + +__EXPORT int board_app_initialize(uintptr_t arg) +{ +#if !defined(BOOTLOADER) + + /* Power on Interfaces */ + VDD_5V_PERIPH_EN(true); + VDD_5V_HIPOWER_EN(true); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + stm32_spiinitialize(); + + board_spi_reset(10, 0xffff); + + /* configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +# if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL); +# endif + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + +# ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + } + +# endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); +#endif + return OK; +} diff --git a/boards/px4/fmu-v6c/src/led.c b/boards/px4/fmu-v6c/src/led.c new file mode 100644 index 0000000000..442e7636f1 --- /dev/null +++ b/boards/px4/fmu-v6c/src/led.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +#ifdef CONFIG_ARCH_LEDS +static bool nuttx_owns_leds = true; +// B R S G +// 0 1 2 3 +static const uint8_t xlatpx4[] = {1, 2, 4, 0}; +# define xlat(p) xlatpx4[(p)] +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED + GPIO_nSAFETY_SWITCH_LED_OUT, // Indexed by LED_SAFETY by xlatpx4 +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + 0, // Indexed by LED_SAFETY (defaulted to an input) + 0, // Indexed by LED_GREEN +}; + +#endif + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} + +#ifdef CONFIG_ARCH_LEDS +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/px4/fmu-v6c/src/manifest.c b/boards/px4/fmu-v6c/src/manifest.c new file mode 100644 index 0000000000..04d2386163 --- /dev/null +++ b/boards/px4/fmu-v6c/src/manifest.c @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2018, 2021 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 manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h +static const px4_hw_mft_item_t hw_mft_list_v0600[] = { + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0610[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver_rev + {V6C00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, + {V6C10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/px4/fmu-v6c/src/mtd.cpp b/boards/px4/fmu-v6c/src/mtd.cpp new file mode 100644 index 0000000000..530c7a3bef --- /dev/null +++ b/boards/px4/fmu-v6c/src/mtd.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (C) 2020 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. + * + ****************************************************************************/ + +#include +#include +// KiB BS nB +static const px4_mft_device_t spi2 = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(4, 0x50) +}; + + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi2, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + +static const px4_mtd_entry_t imu_eeprom = { + .device = &i2c4, + .npart = 2, + .partd = { + { + .type = MTD_CALDATA, + .path = "/fs/mtd_caldata", + .nblocks = 248 + }, + { + .type = MTD_ID, + .path = "/fs/mtd_id", + .nblocks = 8 // 256 = 32 * 8 + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 2, + .entries = { + &fmum_fram, + &imu_eeprom + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = &mtd_mft +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/px4/fmu-v6c/src/sdio.c b/boards/px4/fmu-v6c/src/sdio.c new file mode 100644 index 0000000000..869d757756 --- /dev/null +++ b/boards/px4/fmu-v6c/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/px4/fmu-v6c/src/spi.cpp b/boards/px4/fmu-v6c/src/spi.cpp new file mode 100644 index 0000000000..9e636e0ac4 --- /dev/null +++ b/boards/px4/fmu-v6c/src/spi.cpp @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (C) 2020, 2022 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIHWVersion(V6C00, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), + }, {GPIO::PortB, GPIO::Pin2}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) + }), + }), + initSPIHWVersion(V6C10, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin14}, SPI::DRDY{GPIO::PortE, GPIO::Pin5}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI055, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin6}), + }, {GPIO::PortB, GPIO::Pin2}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) + }), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/px4/fmu-v6c/src/timer_config.cpp b/boards/px4/fmu-v6c/src/timer_config.cpp new file mode 100644 index 0000000000..e26a8c5d1e --- /dev/null +++ b/boards/px4/fmu-v6c/src/timer_config.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +/* Timer allocation + * + * TIM1_CH1 T FMU_CH1 + * TIM1_CH2 T FMU_CH2 + * TIM1_CH3 T FMU_CH3 + * TIM1_CH4 T FMU_CH4 + * + * TIM4_CH3 T FMU_CH5 + * TIM4_CH4 T FMU_CH6 + * + * TIM5_CH1 T FMU_CH7 + * TIM5_CH2 T FMU_CH8 + * + * TIM17_CH1 T HEATER > PWM OUT or GPIO + * + * TIM3_CH3 T BUZZER_1 - Driven by other driver + */ + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer5), + initIOTimer(Timer::Timer17), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin8}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/px4/fmu-v6c/src/usb.c b/boards/px4/fmu-v6c/src/usb.c new file mode 100644 index 0000000000..9d5915c0e6 --- /dev/null +++ b/boards/px4/fmu-v6c/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 2016 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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h index ba2292b97e..c28fa7e86c 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/hw_description.h @@ -92,7 +92,13 @@ enum Timer { Timer13, Timer14, #ifdef STM32_TIM15_BASE - Timer15 + Timer15, +#endif +#ifdef STM32_TIM16_BASE + Timer16, +#endif +#ifdef STM32_TIM17_BASE + Timer17 #endif }; enum Channel { @@ -161,6 +167,16 @@ static inline constexpr uint32_t timerBaseRegister(Timer::Timer timer) case Timer::Timer15: return STM32_TIM15_BASE; #endif +#ifdef STM32_TIM16_BASE + + case Timer::Timer16: return STM32_TIM16_BASE; +#endif + +#ifdef STM32_TIM17_BASE + + case Timer::Timer17: return STM32_TIM17_BASE; +#endif + default: break; } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h index 9627d9a4cd..a93b9538be 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/hw_description.h @@ -86,6 +86,8 @@ static inline constexpr uint32_t getTimerUpdateDMAMap(Timer::Timer timer, const case Timer::Timer13: case Timer::Timer14: case Timer::Timer15: + case Timer::Timer16: + case Timer::Timer17: break; } diff --git a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h index 06e7db74ea..b1299b04e3 100644 --- a/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32h7/include/px4_arch/io_timer_hw_description.h @@ -49,6 +49,8 @@ static inline constexpr timer_io_channels_t initIOTimerGPIOInOut(Timer::TimerCha switch (timer.timer) { case Timer::Timer1: case Timer::Timer2: + case Timer::Timer16: + case Timer::Timer17: gpio_af = GPIO_AF1; break; @@ -274,6 +276,28 @@ static inline constexpr io_timers_t initIOTimer(Timer::Timer timer, DMA dshot_dm ret.vectorno = STM32_IRQ_TIM15; #ifdef CONFIG_STM32_TIM15 nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer16: + ret.base = STM32_TIM16_BASE; + ret.clock_register = STM32_RCC_APB2ENR; + ret.clock_bit = RCC_APB2ENR_TIM16EN; + ret.clock_freq = STM32_APB2_TIM16_CLKIN; + ret.vectorno = STM32_IRQ_TIM16; +#ifdef CONFIG_STM32_TIM16 + nuttx_config_timer_enabled = true; +#endif + break; + + case Timer::Timer17: + ret.base = STM32_TIM17_BASE; + ret.clock_register = STM32_RCC_APB2ENR; + ret.clock_bit = RCC_APB2ENR_TIM17EN; + ret.clock_freq = STM32_APB2_TIM17_CLKIN; + ret.vectorno = STM32_IRQ_TIM17; +#ifdef CONFIG_STM32_TIM17 + nuttx_config_timer_enabled = true; #endif break; } From 2bdeb36ea30eda1105ecfb4ae438e889bf728e01 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 25 Apr 2022 10:54:50 -0700 Subject: [PATCH 13/13] px4_fmu-v6x:Add Rev 4 Sensors --- boards/px4/fmu-v6x/init/rc.board_sensors | 14 ++++++++++---- boards/px4/fmu-v6x/src/board_config.h | 4 +++- boards/px4/fmu-v6x/src/manifest.c | 2 ++ boards/px4/fmu-v6x/src/spi.cpp | 23 +++++++++++++++++++++++ 4 files changed, 38 insertions(+), 5 deletions(-) diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 0733e2bd73..741eb16f32 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -25,17 +25,23 @@ then ina238 -X -b 2 -t 2 -k start fi +if ver hwtypecmp V6X04 V6X14 +then + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start +else # Internal SPI BMI088 -bmi088 -A -R 4 -s start -bmi088 -G -R 4 -s start + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start +fi # Internal SPI bus ICM42688p icm42688p -R 6 -s start -if ver hwtypecmp V6X03 V6X13 +if ver hwtypecmp V6X03 V6X13 V6X04 V6X14 then # Internal SPI bus ICM-42670-P (hard-mounted) - icm42670p -R 14 -s start + icm42670p -R 10 -s start else # Internal SPI bus ICM-20649 (hard-mounted) icm20649 -R 14 -s start diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index 800cc9aef1..e4086ed98c 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -215,13 +215,15 @@ #define HW_INFO_INIT_VER 3 /* Offset in above string of the VER */ #define HW_INFO_INIT_REV 4 /* Offset in above string of the REV */ -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3 // Rev 0 and Rev 3,4 Sensor sets // Base/FMUM #define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0 #define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1 #define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3 +#define V6X04 HW_VER_REV(0x0,0x4) // FMUV6X, Sensor Set Rev 4 #define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 #define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 +#define V6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 /* HEATER diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c index 3a4fc93bff..19a435db86 100644 --- a/boards/px4/fmu-v6x/src/manifest.c +++ b/boards/px4/fmu-v6x/src/manifest.c @@ -102,6 +102,8 @@ static px4_hw_mft_list_entry_t mft_lists[] = { {V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 {V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 + {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 + {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 }; /************************************************************************************ diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index 45ff0a29dc..341cd7d90d 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -83,6 +83,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + + initSPIHWVersion(HW_VER_REV(0, 4), { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);