Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into master

This commit is contained in:
Józef Wołoch 2024-12-04 14:39:03 +01:00 committed by GitHub
parent 5247d98995 9a5f81aa95
commit fd254fbbb0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
77 changed files with 2615 additions and 261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
ArduCopter/Attitude.cpp
View File

@ -4,18 +4,21 @@
* Attitude Rate controllers and timing
****************************************************************/
// update rate controllers and output to roll, pitch and yaw actuators
// called at 400hz by default
void Copter::run_rate_controller()
/*
update rate controller when run from main thread (normal operation)
*/
void Copter::run_rate_controller_main()
{
// set attitude and position controller loop time
const float last_loop_time_s = AP::scheduler().get_last_loop_time_s();
motors->set_dt(last_loop_time_s);
attitude_control->set_dt(last_loop_time_s);
pos_control->set_dt(last_loop_time_s);
attitude_control->set_dt(last_loop_time_s);
// run low level rate controllers that only require IMU data
attitude_control->rate_controller_run();
if (!using_rate_thread) {
motors->set_dt(last_loop_time_s);
// only run the rate controller if we are not using the rate thread
attitude_control->rate_controller_run();
}
// reset sysid and other temporary inputs
attitude_control->rate_controller_target_reset();
}

42
ArduCopter/Copter.cpp
View File

@ -75,6 +75,7 @@
*/
#include "Copter.h"
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
#define FORCE_VERSION_H_INCLUDE
#include "version.h"
@ -113,7 +114,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
// update INS immediately to get current gyro data populated
FAST_TASK_CLASS(AP_InertialSensor, &copter.ins, update),
// run low level rate controllers that only require IMU data
FAST_TASK(run_rate_controller),
FAST_TASK(run_rate_controller_main),
#if AC_CUSTOMCONTROL_MULTI_ENABLED
FAST_TASK(run_custom_controller),
#endif
@ -121,7 +122,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
FAST_TASK(heli_update_autorotation),
#endif //HELI_FRAME
// send outputs to the motors library immediately
FAST_TASK(motors_output),
FAST_TASK(motors_output_main),
// run EKF state estimator (expensive)
FAST_TASK(read_AHRS),
#if FRAME_CONFIG == HELI_FRAME
@ -259,6 +260,10 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#if HAL_BUTTON_ENABLED
SCHED_TASK_CLASS(AP_Button, &copter.button, update, 5, 100, 168),
#endif
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
// don't delete this, there is an equivalent (virtual) in AP_Vehicle for the non-rate loop case
SCHED_TASK(update_dynamic_notch_at_specified_rate_main, LOOP_RATE, 200, 215),
#endif
};
void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks,
@ -618,12 +623,15 @@ void Copter::update_batt_compass(void)
// should be run at loop rate
void Copter::loop_rate_logging()
{
if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
if (should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
Log_Write_Attitude();
Log_Write_PIDS(); // only logs if PIDS bitmask is set
if (!using_rate_thread) {
Log_Write_Rate();
Log_Write_PIDS(); // only logs if PIDS bitmask is set
}
}
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
if (should_log(MASK_LOG_FTN_FAST)) {
if (should_log(MASK_LOG_FTN_FAST) && !using_rate_thread) {
AP::ins().write_notch_log_messages();
}
#endif
@ -641,10 +649,15 @@ void Copter::ten_hz_logging_loop()
// log attitude controller data if we're not already logging at the higher rate
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
Log_Write_Attitude();
if (!using_rate_thread) {
Log_Write_Rate();
}
}
if (!should_log(MASK_LOG_ATTITUDE_FAST) && !copter.flightmode->logs_attitude()) {
// log at 10Hz if PIDS bitmask is selected, even if no ATT bitmask is selected; logs at looprate if ATT_FAST and PIDS bitmask set
Log_Write_PIDS();
if (!using_rate_thread) {
Log_Write_PIDS();
}
}
// log EKF attitude data always at 10Hz unless ATTITUDE_FAST, then do it in the 25Hz loop
if (!should_log(MASK_LOG_ATTITUDE_FAST)) {
@ -789,11 +802,26 @@ void Copter::one_hz_loop()
AP_Notify::flags.flying = !ap.land_complete;
// slowly update the PID notches with the average loop rate
attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
if (!using_rate_thread) {
attitude_control->set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
}
pos_control->get_accel_z_pid().set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
#if AC_CUSTOMCONTROL_MULTI_ENABLED
custom_control.set_notch_sample_rate(AP::scheduler().get_filtered_loop_rate_hz());
#endif
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
// see if we should have a separate rate thread
if (!started_rate_thread && get_fast_rate_type() != FastRateType::FAST_RATE_DISABLED) {
if (hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Copter::rate_controller_thread, void),
"rate",
1536, AP_HAL::Scheduler::PRIORITY_RCOUT, 1)) {
started_rate_thread = true;
} else {
AP_BoardConfig::allocation_error("rate thread");
}
}
#endif
}
void Copter::init_simple_bearing()

39
ArduCopter/Copter.h
View File

@ -631,6 +631,17 @@ private:
RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4
REQUIRE_POSITION_FOR_ARMING = (1<<3), // 8
};
// type of fast rate attitude controller in operation
enum class FastRateType : uint8_t {
FAST_RATE_DISABLED = 0,
FAST_RATE_DYNAMIC = 1,
FAST_RATE_FIXED_ARMED = 2,
FAST_RATE_FIXED = 3,
};
FastRateType get_fast_rate_type() const { return FastRateType(g2.att_enable.get()); }
// returns true if option is enabled for this vehicle
bool option_is_enabled(FlightOption option) const {
return (g2.flight_options & uint32_t(option)) != 0;
@ -728,7 +739,25 @@ private:
void set_accel_throttle_I_from_pilot_throttle();
void rotate_body_frame_to_NE(float &x, float &y);
uint16_t get_pilot_speed_dn() const;
void run_rate_controller();
void run_rate_controller_main();
// if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
struct RateControllerRates {
uint8_t fast_logging_rate;
uint8_t medium_logging_rate;
uint8_t filter_rate;
uint8_t main_loop_rate;
};
uint8_t calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz);
void rate_controller_thread();
void rate_controller_filter_update();
void rate_controller_log_update();
void rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high);
void enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates);
void disable_fast_rate_loop(RateControllerRates& rates);
void update_dynamic_notch_at_specified_rate_main();
// endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
#if AC_CUSTOMCONTROL_MULTI_ENABLED
void run_custom_controller() { custom_control.update(); }
@ -878,6 +907,7 @@ private:
// Log.cpp
void Log_Write_Control_Tuning();
void Log_Write_Attitude();
void Log_Write_Rate();
void Log_Write_EKF_POS();
void Log_Write_PIDS();
void Log_Write_Data(LogDataID id, int32_t value);
@ -892,6 +922,7 @@ private:
void Log_Write_SysID_Setup(uint8_t systemID_axis, float waveform_magnitude, float frequency_start, float frequency_stop, float time_fade_in, float time_const_freq, float time_record, float time_fade_out);
void Log_Write_SysID_Data(float waveform_time, float waveform_sample, float waveform_freq, float angle_x, float angle_y, float angle_z, float accel_x, float accel_y, float accel_z);
void Log_Write_Vehicle_Startup_Messages();
void Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin);
#endif // HAL_LOGGING_ENABLED
// mode.cpp
@ -921,7 +952,8 @@ private:
// motors.cpp
void arm_motors_check();
void auto_disarm_check();
void motors_output();
void motors_output(bool full_push = true);
void motors_output_main();
void lost_vehicle_check();
// navigation.cpp
@ -1086,6 +1118,9 @@ private:
Mode *mode_from_mode_num(const Mode::Number mode);
void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode);
bool started_rate_thread;
bool using_rate_thread;
public:
void failsafe_check(); // failsafe.cpp
};

42
ArduCopter/Log.cpp
View File

@ -1,4 +1,5 @@
#include "Copter.h"
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
#if HAL_LOGGING_ENABLED
@ -76,6 +77,10 @@ void Copter::Log_Write_Control_Tuning()
void Copter::Log_Write_Attitude()
{
attitude_control->Write_ANG();
}
void Copter::Log_Write_Rate()
{
attitude_control->Write_Rate(*pos_control);
}
@ -339,6 +344,16 @@ struct PACKED log_Guided_Attitude_Target {
float climb_rate;
};
// rate thread dt stats
struct PACKED log_Rate_Thread_Dt {
LOG_PACKET_HEADER;
uint64_t time_us;
float dt;
float dtAvg;
float dtMax;
float dtMin;
};
// Write a Guided mode position target
// pos_target is lat, lon, alt OR offset from ekf origin in cm
// terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain
@ -386,6 +401,21 @@ void Copter::Log_Write_Guided_Attitude_Target(ModeGuided::SubMode target_type, f
logger.WriteBlock(&pkt, sizeof(pkt));
}
void Copter::Log_Write_Rate_Thread_Dt(float dt, float dtAvg, float dtMax, float dtMin)
{
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
const log_Rate_Thread_Dt pkt {
LOG_PACKET_HEADER_INIT(LOG_RATE_THREAD_DT_MSG),
time_us : AP_HAL::micros64(),
dt : dt,
dtAvg : dtAvg,
dtMax : dtMax,
dtMin : dtMin
};
logger.WriteBlock(&pkt, sizeof(pkt));
#endif
}
// type and unit information can be found in
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
// units and "Format characters" for field type information
@ -527,6 +557,18 @@ const struct LogStructure Copter::log_structure[] = {
{ LOG_GUIDED_ATTITUDE_TARGET_MSG, sizeof(log_Guided_Attitude_Target),
"GUIA", "QBffffffff", "TimeUS,Type,Roll,Pitch,Yaw,RollRt,PitchRt,YawRt,Thrust,ClimbRt", "s-dddkkk-n", "F-000000-0" , true },
// @LoggerMessage: RTDT
// @Description: Attitude controller time deltas
// @Field: TimeUS: Time since system startup
// @Field: dt: current time delta
// @Field: dtAvg: current time delta average
// @Field: dtMax: Max time delta since last log output
// @Field: dtMin: Min time delta since last log output
{ LOG_RATE_THREAD_DT_MSG, sizeof(log_Rate_Thread_Dt),
"RTDT", "Qffff", "TimeUS,dt,dtAvg,dtMax,dtMin", "sssss", "F----" , true },
};
uint8_t Copter::get_num_log_structures() const

17
ArduCopter/Parameters.cpp
View File

@ -1,6 +1,7 @@
#include "Copter.h"
#include <AP_Gripper/AP_Gripper.h>
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
/*
This program is free software: you can redistribute it and/or modify
@ -1232,6 +1233,22 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = {
// @User: Advanced
AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT),
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
// @Param: FSTRATE_ENABLE
// @DisplayName: Enable the fast Rate thread
// @Description: Enable the fast Rate thread. In the default case the fast rate divisor, which controls the update frequency of the thread, is dynamically scaled from FSTRATE_DIV to avoid overrun in the gyro sample buffer and main loop slow-downs. Other values can be selected to fix the divisor to FSTRATE_DIV on arming or always.
// @User: Advanced
// @Values: 0:Disabled,1:Enabled-Dynamic,2:Enabled-FixedWhenArmed,3:Enabled-Fixed
AP_GROUPINFO("FSTRATE_ENABLE", 9, ParametersG2, att_enable, 0),
// @Param: FSTRATE_DIV
// @DisplayName: Fast rate thread divisor
// @Description: Fast rate thread divisor used to control the maximum fast rate update rate. The actual rate is the gyro rate in Hz divided by this value. This value is scaled depending on the configuration of FSTRATE_ENABLE.
// @User: Advanced
// @Range: 1 10
AP_GROUPINFO("FSTRATE_DIV", 10, ParametersG2, att_decimation, 1),
#endif
// ID 62 is reserved for the AP_SUBGROUPEXTENSION
AP_GROUPEND

3
ArduCopter/Parameters.h
View File

@ -690,6 +690,9 @@ public:
AP_Float pldp_range_finder_maximum_m;
AP_Float pldp_delay_s;
AP_Float pldp_descent_speed_ms;
AP_Int8 att_enable;
AP_Int8 att_decimation;
};
extern const AP_Param::Info var_info[];

3
ArduCopter/defines.h
View File

@ -87,7 +87,8 @@ enum LoggingParameters {
LOG_GUIDED_POSITION_TARGET_MSG,
LOG_SYSIDD_MSG,
LOG_SYSIDS_MSG,
LOG_GUIDED_ATTITUDE_TARGET_MSG
LOG_GUIDED_ATTITUDE_TARGET_MSG,
LOG_RATE_THREAD_DT_MSG
};
#define MASK_LOG_ATTITUDE_FAST (1<<0)

1
ArduCopter/mode_systemid.cpp
View File

@ -414,6 +414,7 @@ void ModeSystemId::log_data() const
// Full rate logging of attitude, rate and pid loops
copter.Log_Write_Attitude();
copter.Log_Write_Rate();
copter.Log_Write_PIDS();
if (is_poscontrol_axis_type()) {

6
ArduCopter/motor_test.cpp
View File

@ -158,6 +158,12 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
ap.motor_test = true;
EXPECT_DELAY_MS(3000);
// wait for rate thread to stop running due to motor test
while (using_rate_thread) {
hal.scheduler->delay(1);
}
// enable and arm motors
if (!motors->armed()) {
motors->output_min(); // output lowest possible value to motors

19
ArduCopter/motors.cpp
View File

@ -133,7 +133,8 @@ void Copter::auto_disarm_check()
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
// full_push is true when slower rate updates (e.g. servo output) need to be performed at the main loop rate.
void Copter::motors_output(bool full_push)
{
#if AP_COPTER_ADVANCED_FAILSAFE_ENABLED
// this is to allow the failsafe module to deliberately crash
@ -183,7 +184,21 @@ void Copter::motors_output()
}
// push all channels
srv.push();
if (full_push) {
// motor output including servos and other updates that need to run at the main loop rate
srv.push();
} else {
// motor output only at main loop rate or faster
hal.rcout->push();
}
}
// motors_output from main thread at main loop rate
void Copter::motors_output_main()
{
if (!using_rate_thread) {
motors_output();
}
}
// check for pilot stick input to trigger lost vehicle alarm

490
ArduCopter/rate_thread.cpp Normal file
View File

@ -0,0 +1,490 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "Copter.h"
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
#pragma GCC optimize("O2")
/*
Attitude Rate controller thread design.
Rationale: running rate outputs linked to fast gyro outputs achieves two goals:
1. High frequency gyro processing allows filters to be applied with high sample rates
which is advantageous in removing high frequency noise and associated aliasing
2. High frequency rate control reduces the latency between control and action leading to
better disturbance rejection and faster responses which generally means higher
PIDs can be used without introducing control oscillation
(1) is already mostly achieved through the higher gyro rates that are available via
INS_GYRO_RATE. (2) requires running the rate controller at higher rates via a separate thread
Goal: the ideal scenario is to run in a single cycle:
gyro read->filter->publish->rate control->motor output
This ensures the minimum latency between gyro sample and motor output. Other functions need
to also run faster than they would normally most notably logging and filter frequencies - most
notably the harmonic notch frequency.
Design assumptions:
1. The sample rate of the IMUs is consistent and accurate.
This is the most basic underlying assumption. An alternative approach would be to rely on
the timing of when samples are received but this proves to not work in practice due to
scheduling delays. Thus the dt used by the attitude controller is the delta between IMU
measurements, not the delta between processing cycles in the rate thread.
2. Every IMU reading must be processed or consistently sub-sampled.
This is an assumption that follows from (1) - so it means that attitude control should
process every sample or every other sample or every third sample etc. Note that these are
filtered samples - all incoming samples are processed for filtering purposes, it is only
for the purposes of rate control that we are sub-sampling.
3. The data that the rate loop requires is timely, consistent and accurate.
Rate control essentially requires two components - the target and the actuals. The actuals
come from the incoming gyro sample combined with the state of the PIDs. The target comes
from attitude controller which is running at a slower rate in the main loop. Since the rate
thread can read the attitude target at any time it is important that this is always available
consistently and is updated consistently.
4. The data that the rest of the vehicle uses is the same data that the rate thread uses.
Put another way any gyro value that the vehicle uses (e.g. in the EKF etc), must have already
been processed by the rate thread. Where this becomes important is with sub-sampling - if
rate gyro values are sub-sampled we need to make sure that the vehicle is also only using
the sub-sampled values.
Design:
1. Filtered gyro samples are (sub-sampled and) pushed into an ObjectBuffer from the INS backend.
2. The pushed sample is published to the INS front-end so that the rest of the vehicle only
sees published values that have been used by the rate controller. When the rate thread is not
in use the filtered samples are effectively sub-sampled at the main loop rate. The EKF is unaffected
as it uses delta angles calculated from the raw gyro values. (It might be possible to avoid publishing
from the rate thread by only updating _gyro_filtered when a value is pushed).
3. A notification is sent that a sample is available
4. The rate thread is blocked waiting for a sample. When it receives a notification it:
4a. Runs the rate controller
4b. Pushes the new pwm values. Periodically at the main loop rate all of the SRV_Channels::push()
functionality is run as well.
5. The rcout dshot thread is blocked waiting for a new pwm value. When it is signalled by the
rate thread it wakes up and runs the dshot motor output logic.
6. Periodically the rate thread:
6a. Logs the rate outputs (1Khz)
6b. Updates the notch filter centers (Gyro rate/2)
6c. Checks the ObjectBuffer length and main loop delay (10Hz)
If the ObjectBuffer length has been longer than 2 for the last 5 cycles or the main loop has
been slowed down then the rate thread is slowed down by telling the INS to sub-sample. This
mechanism is continued until the rate thread is able to keep up with the sub-sample rate.
The inverse of this mechanism is run if the rate thread is able to keep up but is running slower
than the gyro sample rate.
6d. Updates the PID notch centers (1Hz)
7. When the rate rate changes through sub-sampling the following values are updated:
7a. The PID notch sample rate
7b. The dshot rate is constrained to be never greater than the gyro rate or rate rate
7c. The motors dt
8. Independently of the rate thread the attitude control target is updated in the main loop. In order
for target values to be consistent all updates are processed using local variables and the final
target is only written at the end of the update as a vector. Direct control of the target (e.g. in
autotune) is also constrained to be on all axes simultaneously using the new desired value. The
target makes use of the current PIDs and the "latest" gyro, it might be possible to use a loop
delayed gyro value, but that is currently out-of-scope.
Performance considerations:
On an H754 using ICM42688 and gyro sampling at 4KHz and rate thread at 4Khz the main CPU users are:
ArduCopter PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=18.6%
idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD= 4.3%
rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD=10.7%
SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=17.5%
SPI4 PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=18.3%
rate PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=22.4%
There is a direct correlation between the rate rate and CPU load, so if the rate rate is half the gyro
rate (i.e. 2Khz) we observe the following:
ArduCopter PRI=182 sp=0x30000600 STACK=4392/7168 LOAD=16.7%
idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD=21.3%
rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 6.2%
SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD=16.7%
SPI4 PRI=181 sp=0x3002D4A0 STACK= 888/1464 LOAD=17.8%
rate PRI=182 sp=0x3002B1D0 STACK=1272/1976 LOAD=11.5%
So we get almost a halving of CPU load in the rate and rcout threads. This is the main way that CPU
load can be handled on lower-performance boards, with the other mechanism being lowering the gyro rate.
So at a very respectable gyro rate and rate rate both of 2Khz (still 5x standard main loop rate) we see:
ArduCopter PRI=182 sp=0x30000600 STACK=4440/7168 LOAD=15.6%
idle PRI= 1 sp=0x300217B0 STACK= 296/ 504 LOAD=39.4%
rcout PRI=181 sp=0x3001DAF0 STACK= 504/ 952 LOAD= 5.9%
SPI1 PRI=181 sp=0x3002DAB8 STACK= 856/1464 LOAD= 8.9%
SPI4 PRI=181 sp=0x3002D4A0 STACK= 896/1464 LOAD= 9.1%
rate PRI=182 sp=0x30029FB0 STACK=1296/1976 LOAD=11.8%
This essentially means that its possible to run this scheme successfully on all MCUs by careful setting of
the maximum rates.
Enabling rate thread timing debug for 4Khz reads with fast logging and armed we get the following data:
Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us
Rate loop timing: gyro=178us, rate=13us, motors=45us, log=7us, ctrl=1us
Rate loop timing: gyro=177us, rate=13us, motors=46us, log=7us, ctrl=1us
The log output is an average since it only runs at 1Khz, so roughly 28us elapsed. So the majority of the time
is spent waiting for a gyro sample (higher is better here since it represents the idle time) updating the PIDs
and outputting to the motors. Everything else is relatively cheap. Since the total cycle time is 250us the duty
cycle is thus 29%
*/
#define DIV_ROUND_INT(x, d) ((x + d/2) / d)
uint8_t Copter::calc_gyro_decimation(uint8_t gyro_decimation, uint16_t rate_hz)
{
return MAX(uint8_t(DIV_ROUND_INT(ins.get_raw_gyro_rate_hz() / gyro_decimation, rate_hz)), 1U);
}
static inline bool run_decimated_callback(uint8_t decimation_rate, uint8_t& decimation_count)
{
return decimation_rate > 0 && ++decimation_count >= decimation_rate;
}
//#define RATE_LOOP_TIMING_DEBUG
/*
thread for rate control
*/
void Copter::rate_controller_thread()
{
uint8_t target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400));
uint8_t rate_decimation = target_rate_decimation;
// set up the decimation rates
RateControllerRates rates;
rate_controller_set_rates(rate_decimation, rates, false);
uint32_t rate_loop_count = 0;
uint32_t prev_loop_count = 0;
uint32_t last_run_us = AP_HAL::micros();
float max_dt = 0.0;
float min_dt = 1.0;
uint32_t now_ms = AP_HAL::millis();
uint32_t last_rate_check_ms = 0;
uint32_t last_rate_increase_ms = 0;
#if HAL_LOGGING_ENABLED
uint32_t last_rtdt_log_ms = now_ms;
#endif
uint32_t last_notch_sample_ms = now_ms;
bool was_using_rate_thread = false;
bool notify_fixed_rate_active = true;
bool was_armed = false;
uint32_t running_slow = 0;
#ifdef RATE_LOOP_TIMING_DEBUG
uint32_t gyro_sample_time_us = 0;
uint32_t rate_controller_time_us = 0;
uint32_t motor_output_us = 0;
uint32_t log_output_us = 0;
uint32_t ctrl_output_us = 0;
uint32_t timing_count = 0;
uint32_t last_timing_msg_us = 0;
#endif
// run the filters at half the gyro rate
#if HAL_LOGGING_ENABLED
uint8_t log_loop_count = 0;
#endif
uint8_t main_loop_count = 0;
uint8_t filter_loop_count = 0;
while (true) {
#ifdef RATE_LOOP_TIMING_DEBUG
uint32_t rate_now_us = AP_HAL::micros();
#endif
// allow changing option at runtime
if (get_fast_rate_type() == FastRateType::FAST_RATE_DISABLED || ap.motor_test) {
if (was_using_rate_thread) {
disable_fast_rate_loop(rates);
was_using_rate_thread = false;
}
hal.scheduler->delay_microseconds(500);
last_run_us = AP_HAL::micros();
continue;
}
// set up rate thread requirements
if (!using_rate_thread) {
enable_fast_rate_loop(rate_decimation, rates);
}
ins.set_rate_decimation(rate_decimation);
// wait for an IMU sample
Vector3f gyro;
if (!ins.get_next_gyro_sample(gyro)) {
continue; // go around again
}
#ifdef RATE_LOOP_TIMING_DEBUG
gyro_sample_time_us += AP_HAL::micros() - rate_now_us;
rate_now_us = AP_HAL::micros();
#endif
// we must use multiples of the actual sensor rate
const float sensor_dt = 1.0f * rate_decimation / ins.get_raw_gyro_rate_hz();
const uint32_t now_us = AP_HAL::micros();
const uint32_t dt_us = now_us - last_run_us;
const float dt = dt_us * 1.0e-6;
last_run_us = now_us;
// check if we are falling behind
if (ins.get_num_gyro_samples() > 2) {
running_slow++;
} else if (running_slow > 0) {
running_slow--;
}
if (AP::scheduler().get_extra_loop_us() == 0) {
rate_loop_count++;
}
// run the rate controller on all available samples
// it is important not to drop samples otherwise the filtering will be fubar
// there is no need to output to the motors more than once for every batch of samples
attitude_control->rate_controller_run_dt(gyro + ahrs.get_gyro_drift(), sensor_dt);
#ifdef RATE_LOOP_TIMING_DEBUG
rate_controller_time_us += AP_HAL::micros() - rate_now_us;
rate_now_us = AP_HAL::micros();
#endif
// immediately output the new motor values
if (run_decimated_callback(rates.main_loop_rate, main_loop_count)) {
main_loop_count = 0;
}
motors_output(main_loop_count == 0);
// process filter updates
if (run_decimated_callback(rates.filter_rate, filter_loop_count)) {
filter_loop_count = 0;
rate_controller_filter_update();
}
max_dt = MAX(dt, max_dt);
min_dt = MIN(dt, min_dt);
#if HAL_LOGGING_ENABLED
if (now_ms - last_rtdt_log_ms >= 100) { // 10 Hz
Log_Write_Rate_Thread_Dt(dt, sensor_dt, max_dt, min_dt);
max_dt = sensor_dt;
min_dt = sensor_dt;
last_rtdt_log_ms = now_ms;
}
#endif
#ifdef RATE_LOOP_TIMING_DEBUG
motor_output_us += AP_HAL::micros() - rate_now_us;
rate_now_us = AP_HAL::micros();
#endif
#if HAL_LOGGING_ENABLED
// fast logging output
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
if (run_decimated_callback(rates.fast_logging_rate, log_loop_count)) {
log_loop_count = 0;
rate_controller_log_update();
}
} else if (should_log(MASK_LOG_ATTITUDE_MED)) {
if (run_decimated_callback(rates.medium_logging_rate, log_loop_count)) {
log_loop_count = 0;
rate_controller_log_update();
}
}
#endif
#ifdef RATE_LOOP_TIMING_DEBUG
log_output_us += AP_HAL::micros() - rate_now_us;
rate_now_us = AP_HAL::micros();
#endif
now_ms = AP_HAL::millis();
// make sure we have the latest target rate
target_rate_decimation = constrain_int16(g2.att_decimation.get(), 1, DIV_ROUND_INT(ins.get_raw_gyro_rate_hz(), 400));
if (now_ms - last_notch_sample_ms >= 1000 || !was_using_rate_thread) {
// update the PID notch sample rate at 1Hz if we are
// enabled at runtime
last_notch_sample_ms = now_ms;
attitude_control->set_notch_sample_rate(1.0 / sensor_dt);
#ifdef RATE_LOOP_TIMING_DEBUG
hal.console->printf("Sample rate %.1f, main loop %u, fast rate %u, med rate %u\n", 1.0 / sensor_dt,
rates.main_loop_rate, rates.fast_logging_rate, rates.medium_logging_rate);
#endif
}
// interlock for printing fixed rate active
if (was_armed != motors->armed()) {
notify_fixed_rate_active = !was_armed;
was_armed = motors->armed();
}
// Once armed, switch to the fast rate if configured to do so
if ((rate_decimation != target_rate_decimation || notify_fixed_rate_active)
&& ((get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && motors->armed())
|| get_fast_rate_type() == FastRateType::FAST_RATE_FIXED)) {
rate_decimation = target_rate_decimation;
rate_controller_set_rates(rate_decimation, rates, false);
notify_fixed_rate_active = false;
}
// check that the CPU is not pegged, if it is drop the attitude rate
if (now_ms - last_rate_check_ms >= 100
&& (get_fast_rate_type() == FastRateType::FAST_RATE_DYNAMIC
|| (get_fast_rate_type() == FastRateType::FAST_RATE_FIXED_ARMED && !motors->armed())
|| target_rate_decimation > rate_decimation)) {
last_rate_check_ms = now_ms;
const uint32_t att_rate = ins.get_raw_gyro_rate_hz()/rate_decimation;
if (running_slow > 5 || AP::scheduler().get_extra_loop_us() > 0
#if HAL_LOGGING_ENABLED
|| AP::logger().in_log_download()
#endif
|| target_rate_decimation > rate_decimation) {
const uint8_t new_rate_decimation = MAX(rate_decimation + 1, target_rate_decimation);
const uint32_t new_attitude_rate = ins.get_raw_gyro_rate_hz() / new_rate_decimation;
if (new_attitude_rate > AP::scheduler().get_filtered_loop_rate_hz()) {
rate_decimation = new_rate_decimation;
rate_controller_set_rates(rate_decimation, rates, true);
prev_loop_count = rate_loop_count;
rate_loop_count = 0;
running_slow = 0;
}
} else if (rate_decimation > target_rate_decimation && rate_loop_count > att_rate/10 // ensure 100ms worth of good readings
&& (prev_loop_count > att_rate/10 // ensure there was 100ms worth of good readings at the higher rate
|| prev_loop_count == 0 // last rate was actually a lower rate so keep going quickly
|| now_ms - last_rate_increase_ms >= 10000)) { // every 10s retry
rate_decimation = rate_decimation - 1;
rate_controller_set_rates(rate_decimation, rates, false);
prev_loop_count = 0;
rate_loop_count = 0;
last_rate_increase_ms = now_ms;
}
}
#ifdef RATE_LOOP_TIMING_DEBUG
timing_count++;
ctrl_output_us += AP_HAL::micros() - rate_now_us;
rate_now_us = AP_HAL::micros();
if (rate_now_us - last_timing_msg_us > 1e6) {
hal.console->printf("Rate loop timing: gyro=%uus, rate=%uus, motors=%uus, log=%uus, ctrl=%uus\n",
unsigned(gyro_sample_time_us/timing_count), unsigned(rate_controller_time_us/timing_count),
unsigned(motor_output_us/timing_count), unsigned(log_output_us/timing_count), unsigned(ctrl_output_us/timing_count));
last_timing_msg_us = rate_now_us;
timing_count = 0;
gyro_sample_time_us = rate_controller_time_us = motor_output_us = log_output_us = ctrl_output_us = 0;
}
#endif
was_using_rate_thread = true;
}
}
/*
update rate controller filters. on an H7 this is about 30us
*/
void Copter::rate_controller_filter_update()
{
// update the frontend center frequencies of notch filters
for (auto &notch : ins.harmonic_notches) {
update_dynamic_notch(notch);
}
// this copies backend data to the frontend and updates the notches
ins.update_backend_filters();
}
/*
update rate controller rates and return the logging rate
*/
void Copter::rate_controller_set_rates(uint8_t rate_decimation, RateControllerRates& rates, bool warn_cpu_high)
{
const uint32_t attitude_rate = ins.get_raw_gyro_rate_hz() / rate_decimation;
attitude_control->set_notch_sample_rate(attitude_rate);
hal.rcout->set_dshot_rate(SRV_Channels::get_dshot_rate(), attitude_rate);
motors->set_dt(1.0f / attitude_rate);
gcs().send_text(warn_cpu_high ? MAV_SEVERITY_WARNING : MAV_SEVERITY_INFO,
"Rate CPU %s, rate set to %uHz",
warn_cpu_high ? "high" : "normal", (unsigned) attitude_rate);
#if HAL_LOGGING_ENABLED
if (attitude_rate > 1000) {
rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, 1000); // 1Khz
} else {
rates.fast_logging_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
}
rates.medium_logging_rate = calc_gyro_decimation(rate_decimation, 10); // 10Hz
#endif
rates.main_loop_rate = calc_gyro_decimation(rate_decimation, AP::scheduler().get_filtered_loop_rate_hz());
rates.filter_rate = calc_gyro_decimation(rate_decimation, ins.get_raw_gyro_rate_hz() / 2);
}
// enable the fast rate thread using the provided decimation rate and record the new output rates
void Copter::enable_fast_rate_loop(uint8_t rate_decimation, RateControllerRates& rates)
{
ins.enable_fast_rate_buffer();
rate_controller_set_rates(rate_decimation, rates, false);
hal.rcout->force_trigger_groups(true);
using_rate_thread = true;
}
// disable the fast rate thread and record the new output rates
void Copter::disable_fast_rate_loop(RateControllerRates& rates)
{
using_rate_thread = false;
uint8_t rate_decimation = calc_gyro_decimation(1, AP::scheduler().get_filtered_loop_rate_hz());
rate_controller_set_rates(rate_decimation, rates, false);
hal.rcout->force_trigger_groups(false);
ins.disable_fast_rate_buffer();
}
/*
log only those items that are updated at the rate loop rate
*/
void Copter::rate_controller_log_update()
{
#if HAL_LOGGING_ENABLED
if (!copter.flightmode->logs_attitude()) {
Log_Write_Rate();
Log_Write_PIDS(); // only logs if PIDS bitmask is set
}
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
if (should_log(MASK_LOG_FTN_FAST)) {
AP::ins().write_notch_log_messages();
}
#endif
#endif
}
// run notch update at either loop rate or 200Hz
void Copter::update_dynamic_notch_at_specified_rate_main()
{
if (using_rate_thread) {
return;
}
update_dynamic_notch_at_specified_rate();
}
#endif // AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED

12
ArduPlane/AP_Arming.cpp
View File

@ -433,9 +433,15 @@ bool AP_Arming_Plane::mission_checks(bool report)
{
// base checks
bool ret = AP_Arming::mission_checks(report);
if (plane.mission.contains_item(MAV_CMD_DO_LAND_START) && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
ret = false;
check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
if (plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
if (plane.mission.contains_item(MAV_CMD_DO_LAND_START)) {
ret = false;
check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
}
if (plane.mission.contains_item(MAV_CMD_DO_RETURN_PATH_START)) {
ret = false;
check_failed(ARMING_CHECK_MISSION, report, "DO_RETURN_PATH_START set and RTL_AUTOLAND disabled");
}
}
#if HAL_QUADPLANE_ENABLED
if (plane.quadplane.available()) {

20
ArduPlane/GCS_Mavlink.cpp
View File

@ -1072,11 +1072,27 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
case MAV_CMD_DO_GO_AROUND:
return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
case MAV_CMD_DO_RETURN_PATH_START:
// attempt to rejoin after the next DO_RETURN_PATH_START command in the mission
if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) {
plane.mission.set_force_resume(true);
if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
// mode change failed, revert force resume flag
plane.mission.set_force_resume(false);
}
return MAV_RESULT_FAILED;
case MAV_CMD_DO_LAND_START:
// attempt to switch to next DO_LAND_START command in the mission
if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
return MAV_RESULT_ACCEPTED;
plane.mission.set_force_resume(true);
if (plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND)) {
return MAV_RESULT_ACCEPTED;
}
// mode change failed, revert force resume flag
plane.mission.set_force_resume(false);
}
return MAV_RESULT_FAILED;

2
ArduPlane/Parameters.cpp
View File

@ -746,7 +746,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: RTL_AUTOLAND
// @DisplayName: RTL auto land
// @Description: Automatically begin landing sequence after arriving at RTL location. This requires the addition of a DO_LAND_START mission item, which acts as a marker for the start of a landing sequence. The closest landing sequence will be chosen to the current location. If this is set to 0 and there is a DO_LAND_START mission item then you will get an arming check failure. You can set to a value of 3 to avoid the arming check failure and use the DO_LAND_START for go-around without it changing RTL behaviour. For a value of 1 a rally point will be used instead of HOME if in range (see rally point documentation).
// @Values: 0:Disable,1:Fly HOME then land,2:Go directly to landing sequence, 3:OnlyForGoAround
// @Values: 0:Disable,1:Fly HOME then land via DO_LAND_START mission item, 2:Go directly to landing sequence via DO_LAND_START mission item, 3:OnlyForGoAround, 4:Go directly to landing sequence via DO_RETURN_PATH_START mission item
// @User: Standard
GSCALAR(rtl_autoland, "RTL_AUTOLAND", float(RtlAutoland::RTL_DISABLE)),

3
ArduPlane/Plane.h
View File

@ -453,7 +453,7 @@ private:
float throttle_lim_max;
float throttle_lim_min;
uint32_t throttle_max_timer_ms;
// Good candidate for keeping the initial time for TKOFF_THR_MAX_T.
uint32_t level_off_start_time_ms;
} takeoff_state;
// ground steering controller state
@ -1147,6 +1147,7 @@ private:
int16_t get_takeoff_pitch_min_cd(void);
void landing_gear_update(void);
bool check_takeoff_timeout(void);
bool check_takeoff_timeout_level_off(void);
// avoidance_adsb.cpp
void avoidance_adsb_update(void);

7
ArduPlane/commands_logic.cpp
View File

@ -138,6 +138,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
}
break;
case MAV_CMD_DO_RETURN_PATH_START:
case MAV_CMD_DO_LAND_START:
break;
@ -304,6 +305,7 @@ bool Plane::verify_command(const AP_Mission::Mission_Command& cmd) // Ret
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_INVERTED_FLIGHT:
case MAV_CMD_DO_RETURN_PATH_START:
case MAV_CMD_DO_LAND_START:
case MAV_CMD_DO_FENCE_ENABLE:
case MAV_CMD_DO_AUTOTUNE_ENABLE:
@ -587,7 +589,10 @@ bool Plane::verify_takeoff()
// see if we have reached takeoff altitude
int32_t relative_alt_cm = adjusted_relative_altitude_cm();
if (relative_alt_cm > auto_state.takeoff_altitude_rel_cm) {
if (
relative_alt_cm > auto_state.takeoff_altitude_rel_cm || // altitude reached
plane.check_takeoff_timeout_level_off() // pitch level-off maneuver has timed out
) {
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff complete at %.2fm",
(double)(relative_alt_cm*0.01f));
steer_state.hold_course_cd = -1;

2
ArduPlane/defines.h
View File

@ -61,8 +61,8 @@ enum class RtlAutoland {
RTL_THEN_DO_LAND_START = 1,
RTL_IMMEDIATE_DO_LAND_START = 2,
NO_RTL_GO_AROUND = 3,
DO_RETURN_PATH_START = 4,
};
// PID broadcast bitmask
enum tuning_pid_bits {

17
ArduPlane/mode_rtl.cpp
View File

@ -106,8 +106,7 @@ void ModeRTL::navigate()
if ((plane.g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) ||
(plane.g.rtl_autoland == RtlAutoland::RTL_THEN_DO_LAND_START &&
plane.reached_loiter_target() &&
labs(plane.calc_altitude_error_cm()) < 1000))
{
labs(plane.calc_altitude_error_cm()) < 1000)) {
// we've reached the RTL point, see if we have a landing sequence
if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
// switch from RTL -> AUTO
@ -116,12 +115,26 @@ void ModeRTL::navigate()
// return here so we don't change the radius and don't run the rtl update_loiter()
return;
}
// mode change failed, revert force resume flag
plane.mission.set_force_resume(false);
}
// prevent running the expensive jump_to_landing_sequence
// on every loop
plane.auto_state.checked_for_autoland = true;
} else if (plane.g.rtl_autoland == RtlAutoland::DO_RETURN_PATH_START) {
if (plane.have_position && plane.mission.jump_to_closest_mission_leg(plane.current_loc)) {
plane.mission.set_force_resume(true);
if (plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND)) {
// return here so we don't change the radius and don't run the rtl update_loiter()
return;
}
// mode change failed, revert force resume flag
plane.mission.set_force_resume(false);
}
plane.auto_state.checked_for_autoland = true;
}
}
}

7
ArduPlane/mode_takeoff.cpp
View File

@ -119,6 +119,7 @@ void ModeTakeoff::update()
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff to %.0fm for %.1fm heading %.1f deg",
alt, dist, direction);
plane.takeoff_state.start_time_ms = millis();
plane.takeoff_state.level_off_start_time_ms = 0;
plane.takeoff_state.throttle_max_timer_ms = millis();
takeoff_mode_setup = true;
plane.steer_state.hold_course_cd = wrap_360_cd(direction*100); // Necessary to allow Plane::takeoff_calc_roll() to function.
@ -157,6 +158,12 @@ void ModeTakeoff::update()
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
}
// If we have timed-out on the attempt to close the final few meters
// during pitch level-off, continue to NORMAL flight stage.
if (plane.check_takeoff_timeout_level_off()) {
plane.set_flight_stage(AP_FixedWing::FlightStage::NORMAL);
}
if (plane.flight_stage == AP_FixedWing::FlightStage::TAKEOFF) {
//below TKOFF_ALT
plane.takeoff_calc_roll();

19
ArduPlane/takeoff.cpp
View File

@ -121,6 +121,7 @@ bool Plane::auto_takeoff_check(void)
takeoff_state.launchTimerStarted = false;
takeoff_state.last_tkoff_arm_time = 0;
takeoff_state.start_time_ms = now;
takeoff_state.level_off_start_time_ms = 0;
takeoff_state.throttle_max_timer_ms = now;
steer_state.locked_course_err = 0; // use current heading without any error offset
return true;
@ -316,6 +317,7 @@ int16_t Plane::get_takeoff_pitch_min_cd(void)
// make a note of that altitude to use it as a start height for scaling
gcs().send_text(MAV_SEVERITY_INFO, "Takeoff level-off starting at %dm", int(remaining_height_to_target_cm/100));
auto_state.height_below_takeoff_to_level_off_cm = remaining_height_to_target_cm;
takeoff_state.level_off_start_time_ms = AP_HAL::millis();
}
}
}
@ -376,9 +378,8 @@ void Plane::landing_gear_update(void)
#endif
/*
check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred and disarms on timeout
check takeoff_timeout; checks time after the takeoff start time; returns true if timeout has occurred
*/
bool Plane::check_takeoff_timeout(void)
{
if (takeoff_state.start_time_ms != 0 && g2.takeoff_timeout > 0) {
@ -400,3 +401,17 @@ bool Plane::check_takeoff_timeout(void)
return false;
}
/*
check if the pitch level-off time has expired; returns true if timeout has occurred
*/
bool Plane::check_takeoff_timeout_level_off(void)
{
if (takeoff_state.level_off_start_time_ms > 0) {
// A takeoff is in progress.
uint32_t now = AP_HAL::millis();
if ((now - takeoff_state.level_off_start_time_ms) > (uint32_t)(1000U * g.takeoff_pitch_limit_reduction_sec)) {
return true;
}
}
return false;
}

9
Tools/AP_Periph/AP_Periph.h
View File

@ -568,6 +568,15 @@ public:
uint16_t pool_peak_percent();
void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue);
#if AP_SIM_ENABLED
// update simulation of servos
void sim_update_actuator(uint8_t actuator_id);
struct {
uint32_t mask;
uint32_t last_send_ms;
} sim_actuator;
#endif
struct dronecan_protocol_t {
CanardInstance canard;
uint32_t canard_memory_pool[HAL_CAN_POOL_SIZE/sizeof(uint32_t)];

51
Tools/AP_Periph/rc_out.cpp
View File

@ -15,6 +15,9 @@
#include <AP_HAL/AP_HAL.h>
#ifdef HAL_PERIPH_ENABLE_RC_OUT
#include "AP_Periph.h"
#if AP_SIM_ENABLED
#include <dronecan_msgs.h>
#endif
// magic value from UAVCAN driver packet
// dsdl/uavcan/equipment/esc/1030.RawCommand.uavcan
@ -112,6 +115,9 @@ void AP_Periph_FW::rcout_srv_unitless(uint8_t actuator_id, const float command_v
SRV_Channels::set_output_norm(function, command_value);
rcout_has_new_data_to_update = true;
#if AP_SIM_ENABLED
sim_update_actuator(actuator_id);
#endif
#endif
}
@ -122,6 +128,9 @@ void AP_Periph_FW::rcout_srv_PWM(uint8_t actuator_id, const float command_value)
SRV_Channels::set_output_pwm(function, uint16_t(command_value+0.5));
rcout_has_new_data_to_update = true;
#if AP_SIM_ENABLED
sim_update_actuator(actuator_id);
#endif
#endif
}
@ -172,4 +181,46 @@ void AP_Periph_FW::rcout_update()
#endif
}
#if AP_SIM_ENABLED
/*
update simulation of servos, sending actuator status
*/
void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id)
{
sim_actuator.mask |= 1U << actuator_id;
// send status at 10Hz
const uint32_t period_ms = 100;
const uint32_t now_ms = AP_HAL::millis();
if (now_ms - sim_actuator.last_send_ms < period_ms) {
return;
}
sim_actuator.last_send_ms = now_ms;
for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
if ((sim_actuator.mask & (1U<<i)) == 0) {
continue;
}
const SRV_Channel::Aux_servo_function_t function = SRV_Channel::Aux_servo_function_t(SRV_Channel::k_rcin1 + actuator_id - 1);
uavcan_equipment_actuator_Status pkt {};
pkt.actuator_id = i;
// assume 45 degree angle for simulation
pkt.position = radians(SRV_Channels::get_output_norm(function) * 45);
pkt.force = 0;
pkt.speed = 0;
pkt.power_rating_pct = UAVCAN_EQUIPMENT_ACTUATOR_STATUS_POWER_RATING_PCT_UNKNOWN;
uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE];
uint16_t total_size = uavcan_equipment_actuator_Status_encode(&pkt, buffer, !canfdout());
canard_broadcast(UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE,
UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer[0],
total_size);
}
}
#endif // AP_SIM_ENABLED
#endif // HAL_PERIPH_ENABLE_RC_OUT

1
Tools/GIT_Test/GIT_Success.txt
View File

@ -206,3 +206,4 @@ Amr Attia
Alessandro Martini
Eren Mert YİĞİT
Prashant Powar
Barış Kaya :)

4
Tools/ardupilotwaf/ap_library.py
View File

@ -54,7 +54,8 @@ def _vehicle_index(vehicle):
# note that AP_NavEKF3_core.h is needed for AP_NavEKF3_feature.h
_vehicle_macros = ['APM_BUILD_DIRECTORY', 'AP_BUILD_TARGET_NAME',
'APM_BUILD_TYPE', 'APM_BUILD_COPTER_OR_HELI',
'AP_NavEKF3_core.h', 'lua_generated_bindings.h']
'AP_NavEKF3_core.h', 'lua_generated_bindings.h',
'AP_InertialSensor_rate_config.h']
_macros_re = re.compile(r'\b(%s)\b' % '|'.join(_vehicle_macros))
# some cpp files are not available at the time we run this check so need to be
@ -174,6 +175,7 @@ class ap_library_check_headers(Task.Task):
'libraries/AP_Scripting/lua_generated_bindings.h',
'libraries/AP_NavEKF3/AP_NavEKF3_feature.h',
'libraries/AP_LandingGear/AP_LandingGear_config.h',
'libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h',
)
whitelist = tuple(os.path.join(*p.split('/')) for p in whitelist)

1
Tools/ardupilotwaf/ardupilotwaf.py
View File

@ -107,6 +107,7 @@ COMMON_VEHICLE_DEPENDENT_LIBRARIES = [
'AP_EFI',
'AP_Hott_Telem',
'AP_ESC_Telem',
'AP_Servo_Telem',
'AP_Stats',
'AP_GyroFFT',
'AP_RCTelemetry',

2
Tools/ardupilotwaf/chibios.py
View File

@ -103,7 +103,7 @@ class upload_fw(Task.Task):
except subprocess.CalledProcessError:
#if where.exe can't find the file it returns a non-zero result which throws this exception
where_python = ""
if not where_python or "\Python\Python" not in where_python or "python.exe" not in where_python:
if "python.exe" not in where_python:
print(self.get_full_wsl2_error_msg("Windows python.exe not found"))
return False
return True

13
Tools/autotest/ArduPlane_Tests/TakeoffBadLevelOff/flaps_tkoff_50.txt Normal file
View File

@ -0,0 +1,13 @@
QGC WPL 110
0 0 0 16 0.000000 0.000000 0.000000 0.000000 -35.363262 149.165237 584.390015 1
1 0 3 22 15.000000 0.000000 0.000000 0.000000 -35.361279 149.164230 50.000000 1
2 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361229 149.163025 80.000000 1
3 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364563 149.163773 80.000000 1
4 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.364384 149.164795 80.000000 1
5 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.361027 149.164093 80.000000 1
6 0 0 177 2.000000 3.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1
7 0 3 189 0.000000 0.000000 0.000000 0.000000 -35.362915 149.162613 60.000000 1
8 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.363136 149.162750 60.000000 1
9 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365467 149.164215 55.000000 1
10 0 3 16 0.000000 0.000000 0.000000 0.000000 -35.365009 149.165482 39.889999 1
11 0 3 21 0.000000 0.000000 0.000000 1.000000 -35.363041 149.165222 0.000000 1

72
Tools/autotest/arducopter.py
View File

@ -6293,6 +6293,77 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
if ex is not None:
raise ex
def DynamicRpmNotchesRateThread(self):
"""Use dynamic harmonic notch to control motor noise via ESC telemetry."""
self.progress("Flying with ESC telemetry driven dynamic notches")
self.context_push()
self.set_rc_default()
self.set_parameters({
"AHRS_EKF_TYPE": 10,
"INS_LOG_BAT_MASK": 3,
"INS_LOG_BAT_OPT": 0,
"INS_GYRO_FILTER": 300, # set gyro filter high so we can observe behaviour
"LOG_BITMASK": 959,
"LOG_DISARMED": 0,
"SIM_VIB_MOT_MAX": 350,
"SIM_GYR1_RND": 20,
"SIM_ESC_TELEM": 1,
"FSTRATE_ENABLE": 1
})
self.reboot_sitl()
self.takeoff(10, mode="ALT_HOLD")
# find a motor peak, the peak is at about 190Hz, so checking between 50 and 320Hz should be safe.
# there is a second harmonic at 380Hz which should be avoided to make the test reliable
# detect at -5dB so we don't pick some random noise as the peak. The actual peak is about +15dB
freq, hover_throttle, peakdb = self.hover_and_check_matched_frequency_with_fft(-5, 50, 320)
# now add a dynamic notch and check that the peak is squashed
self.set_parameters({
"INS_LOG_BAT_OPT": 4,
"INS_HNTCH_ENABLE": 1,
"INS_HNTCH_FREQ": 80,
"INS_HNTCH_REF": 1.0,
"INS_HNTCH_HMNCS": 5, # first and third harmonic
"INS_HNTCH_ATT": 50,
"INS_HNTCH_BW": 40,
"INS_HNTCH_MODE": 3,
"FSTRATE_ENABLE": 1
})
self.reboot_sitl()
# -10dB is pretty conservative - actual is about -25dB
freq, hover_throttle, peakdb1, psd = \
self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)
# find the noise at the motor frequency
esc_hz = self.get_average_esc_frequency()
esc_peakdb1 = psd["X"][int(esc_hz)]
# now add notch-per motor and check that the peak is squashed
self.set_parameter("INS_HNTCH_OPTS", 2)
self.reboot_sitl()
freq, hover_throttle, peakdb2, psd = \
self.hover_and_check_matched_frequency_with_fft_and_psd(-10, 50, 320, reverse=True, instance=2)
# find the noise at the motor frequency
esc_hz = self.get_average_esc_frequency()
esc_peakdb2 = psd["X"][int(esc_hz)]
# notch-per-motor will be better at the average ESC frequency
if esc_peakdb2 > esc_peakdb1:
raise NotAchievedException(
"Notch-per-motor peak was higher than single-notch peak %fdB > %fdB" %
(esc_peakdb2, esc_peakdb1))
# check that the noise is being squashed at all. this needs to be an aggresive check so that failure happens easily
# testing shows this to be -58dB on average
if esc_peakdb2 > -25:
raise NotAchievedException(
"Notch-per-motor had a peak of %fdB there should be none" % esc_peakdb2)
self.context_pop()
self.reboot_sitl()
def hover_and_check_matched_frequency(self, dblevel=-15, minhz=200, maxhz=300, fftLength=32, peakhz=None):
'''do a simple up-and-down test flight with current vehicle state.
Check that the onboard filter comes up with the same peak-frequency that
@ -12160,6 +12231,7 @@ class AutoTestCopter(vehicle_test_suite.TestSuite):
Test(self.DynamicNotches, attempts=4),
self.PositionWhenGPSIsZero,
self.DynamicRpmNotches, # Do not add attempts to this - failure is sign of a bug
self.DynamicRpmNotchesRateThread,
self.PIDNotches,
self.StaticNotches,
self.RefindGPS,

41
Tools/autotest/arduplane.py
View File

@ -4860,6 +4860,46 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.fly_home_land_and_disarm()
def TakeoffBadLevelOff(self):
'''Ensure that the takeoff can be completed under 0 pitch demand.'''
'''
When using no airspeed, the pitch level-off will eventually command 0
pitch demand. Ensure that the plane can climb the final 2m to deem the
takeoff complete.
'''
self.customise_SITL_commandline(
[],
model='plane-catapult',
defaults_filepath=self.model_defaults_filepath("plane")
)
self.set_parameters({
"ARSPD_USE": 0.0,
"PTCH_TRIM_DEG": -10.0,
"RTL_AUTOLAND": 2, # The mission contains a DO_LAND_START item.
"TKOFF_ALT": 50.0,
"TKOFF_DIST": 1000.0,
"TKOFF_THR_MAX": 75.0,
"TKOFF_THR_MINACC": 3.0,
})
self.load_mission("flaps_tkoff_50.txt")
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
# Throw the catapult.
self.set_servo(7, 2000)
# Wait until we've reached the takeoff altitude.
target_alt = 50
self.wait_altitude(target_alt-1, target_alt+1, relative=True, timeout=30)
self.delay_sim_time(5)
self.disarm_vehicle(force=True)
def DCMFallback(self):
'''Really annoy the EKF and force fallback'''
self.reboot_sitl()
@ -6452,6 +6492,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.TakeoffTakeoff4,
self.TakeoffGround,
self.TakeoffIdleThrottle,
self.TakeoffBadLevelOff,
self.ForcedDCM,
self.DCMFallback,
self.MAVFTP,

49
Tools/autotest/ardusub.py
View File

@ -947,6 +947,54 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
self.wait_ready_to_arm()
self.assert_mag_fusion_selection(MagFuseSel.FUSE_MAG)
def INA3221(self):
'''test INA3221 driver'''
self.set_parameters({
"BATT2_MONITOR": 30,
"BATT3_MONITOR": 30,
"BATT4_MONITOR": 30,
})
self.reboot_sitl()
self.set_parameters({
"BATT2_I2C_ADDR": 0x42, # address defined in libraries/SITL/SIM_I2C.cpp
"BATT2_I2C_BUS": 1,
"BATT2_CHANNEL": 1,
"BATT3_I2C_ADDR": 0x42,
"BATT3_I2C_BUS": 1,
"BATT3_CHANNEL": 2,
"BATT4_I2C_ADDR": 0x42,
"BATT4_I2C_BUS": 1,
"BATT4_CHANNEL": 3,
})
self.reboot_sitl()
seen_1 = False
seen_3 = False
tstart = self.get_sim_time()
while not (seen_1 and seen_3):
m = self.assert_receive_message('BATTERY_STATUS')
if self.get_sim_time() - tstart > 10:
# expected to take under 1 simulated second, but don't hang if
# e.g. the driver gets stuck
raise NotAchievedException("INA3221 status timeout")
if m.id == 1:
self.assert_message_field_values(m, {
# values close to chip limits
"voltages[0]": int(25 * 1000), # millivolts
"current_battery": int(160 * 100), # centi-amps
}, epsilon=10) # allow rounding
seen_1 = True
# id 2 is the first simulated battery current/voltage
if m.id == 3:
self.assert_message_field_values(m, {
# values different from above to test channel switching
"voltages[0]": int(3.14159 * 1000), # millivolts
"current_battery": int(2.71828 * 100), # centi-amps
}, epsilon=10) # allow rounding
seen_3 = True
def tests(self):
'''return list of all tests'''
ret = super(AutoTestSub, self).tests()
@ -978,6 +1026,7 @@ class AutoTestSub(vehicle_test_suite.TestSuite):
self.SetGlobalOrigin,
self.BackupOrigin,
self.FuseMag,
self.INA3221,
])
return ret

8
Tools/autotest/vehicle_test_suite.py
View File

@ -4364,7 +4364,13 @@ class TestSuite(ABC):
def message_has_field_values(self, m, fieldvalues, verbose=True, epsilon=None):
for (fieldname, value) in fieldvalues.items():
got = getattr(m, fieldname)
if "[" in fieldname: # fieldname == "arrayname[index]"
assert fieldname[-1] == "]", fieldname
arrayname, index = fieldname.split("[", 1)
index = int(index[:-1])
got = getattr(m, arrayname)[index]
else:
got = getattr(m, fieldname)
value_string = value
got_string = got

3
Tools/scripts/build_options.py
View File

@ -56,6 +56,7 @@ BUILD_OPTIONS = [
Feature('Battery', 'BATTERY_FUELLEVEL_ANALOG', 'AP_BATTERY_FUELLEVEL_ANALOG_ENABLED', 'Enable Analog Fuel level battry monitor', 0, None), # noqa: E501
Feature('Battery', 'BATTERY_SMBUS', 'AP_BATTERY_SMBUS_ENABLED', 'Enable SMBUS battery monitor', 0, None),
Feature('Battery', 'BATTERY_INA2XX', 'AP_BATTERY_INA2XX_ENABLED', 'Enable INA2XX battery monitor', 0, None),
Feature('Battery', 'BATTERY_INA3221', 'AP_BATTERY_INA3221_ENABLED', 'Enable INA3221 battery monitor', 0, None),
Feature('Battery', 'BATTERY_SYNTHETIC_CURRENT', 'AP_BATTERY_SYNTHETIC_CURRENT_ENABLED', 'Enable Synthetic Current monitor', 0, None), # noqa: E501
Feature('Battery', 'BATTERY_ESC_TELEM_OUT', 'AP_BATTERY_ESC_TELEM_OUTBOUND_ENABLED', 'Enable Ability to put battery monitor data into ESC telem stream', 0, None), # noqa: E501
Feature('Battery', 'BATTERY_SUM', 'AP_BATTERY_SUM_ENABLED', 'Enable Synthetic sum-of-other-batteries backend', 0, None), # noqa: E501
@ -342,6 +343,7 @@ BUILD_OPTIONS = [
Feature('IMU', 'HarmonicNotches', 'AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', 'Enable InertialSensor harmonic notch filters', 0, None), # noqa
Feature('IMU', 'BatchSampler', 'AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', 'Enable Batch sampler', 0, None), # noqa
Feature('Other', 'RateLoopThread', 'AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED', 'Enable Rate Loop Thread', 0, None), # noqa
Feature('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight gyro FFT calculations', 0, None),
Feature('Other', 'NMEA_OUTPUT', 'HAL_NMEA_OUTPUT_ENABLED', 'Enable NMEA output', 0, None),
Feature('Other', 'SDCARD_FORMATTING', 'AP_FILESYSTEM_FORMAT_ENABLED', 'Enable Formatting of microSD cards', 0, None),
@ -405,6 +407,7 @@ BUILD_OPTIONS = [
Feature('Airspeed Drivers', 'SDP3X', 'AP_AIRSPEED_SDP3X_ENABLED', 'Enable SDP3X AIRSPEED', 0, 'AIRSPEED'),
Feature('Airspeed Drivers', 'DRONECAN_ASPD', 'AP_AIRSPEED_DRONECAN_ENABLED', 'Enable DroneCAN AIRSPEED', 0, 'AIRSPEED,DroneCAN'), # NOQA: E501
Feature('Actuators', 'Servo telem', 'AP_SERVO_TELEM_ENABLED', 'Enable servo telemetry library', 0, None),
Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None),
Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, "DroneCAN,Volz"), # noqa: E501
Feature('Actuators', 'RobotisServo', 'AP_ROBOTISSERVO_ENABLED', 'Enable RobotisServo protocol', 0, None),

2
Tools/scripts/extract_features.py
View File

@ -190,6 +190,7 @@ class ExtractFeatures(object):
('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P<type>.*)::_process_byte\b',),
('AP_RCPROTOCOL_{type}_ENABLED', r'AP_RCProtocol_(?P<type>.*)::process_pulse\b',),
('AP_SERVO_TELEM_ENABLED', r'AP_Servo_Telem::update\b',),
('AP_VOLZ_ENABLED', r'AP_Volz_Protocol::init\b',),
('AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', r'AP_DroneCAN::handle_actuator_status_Volz\b',),
('AP_ROBOTISSERVO_ENABLED', r'AP_RobotisServo::init\b',),
@ -209,6 +210,7 @@ class ExtractFeatures(object):
('HAL_WITH_DSP', r'AP_HAL::DSP::find_peaks\b',),
('AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', r'AP_InertialSensor::HarmonicNotch::update_params\b',),
('AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', r'AP_InertialSensor::BatchSampler::init'),
('AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED', r'FastRateBuffer::get_next_gyro_sample\b',),
('HAL_GYROFFT_ENABLED', r'AP_GyroFFT::AP_GyroFFT\b',),
('HAL_DISPLAY_ENABLED', r'Display::init\b',),
('HAL_NMEA_OUTPUT_ENABLED', r'AP_NMEA_Output::update\b',),

6
libraries/AP_BattMonitor/AP_BattMonitor.cpp
View File

@ -21,6 +21,7 @@
#include "AP_BattMonitor_EFI.h"
#include "AP_BattMonitor_INA2xx.h"
#include "AP_BattMonitor_INA239.h"
#include "AP_BattMonitor_INA3221.h"
#include "AP_BattMonitor_LTC2946.h"
#include "AP_BattMonitor_Torqeedo.h"
#include "AP_BattMonitor_FuelLevel_Analog.h"
@ -604,6 +605,11 @@ AP_BattMonitor::init()
drivers[instance] = NEW_NOTHROW AP_BattMonitor_Scripting(*this, state[instance], _params[instance]);
break;
#endif // AP_BATTERY_SCRIPTING_ENABLED
#if AP_BATTERY_INA3221_ENABLED
case Type::INA3221:
drivers[instance] = NEW_NOTHROW AP_BattMonitor_INA3221(*this, state[instance], _params[instance]);
break;
#endif // AP_BATTERY_INA3221_ENABLED
case Type::NONE:
default:
break;

2
libraries/AP_BattMonitor/AP_BattMonitor.h
View File

@ -70,6 +70,7 @@ class AP_BattMonitor
friend class AP_BattMonitor_INA239;
friend class AP_BattMonitor_LTC2946;
friend class AP_BattMonitor_AD7091R5;
friend class AP_BattMonitor_INA3221;
friend class AP_BattMonitor_Torqeedo;
friend class AP_BattMonitor_FuelLevel_Analog;
@ -116,6 +117,7 @@ public:
EFI = 27,
AD7091R5 = 28,
Scripting = 29,
INA3221 = 30,
};
FUNCTOR_TYPEDEF(battery_failsafe_handler_fn_t, void, const char *, const int8_t);

350
libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp Normal file
View File

@ -0,0 +1,350 @@
#include "AP_BattMonitor_config.h"
#if AP_BATTERY_INA3221_ENABLED
#include "AP_BattMonitor_INA3221.h"
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <GCS_MAVLink/GCS.h>
#define INA3221_DEBUGGING 0
#if INA3221_DEBUGGING
#include <stdio.h>
#define debug(fmt, args ...) do {printf("INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
// #define debug(fmt, args ...) do {gcs().send_text(MAV_SEVERITY_INFO, "INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
#else
#define debug(fmt, args ...)
#endif
#ifndef HAL_BATTMON_INA3221_BUS
#define HAL_BATTMON_INA3221_BUS 0
#endif
#ifndef HAL_BATTMON_INA3221_ADDR
#define HAL_BATTMON_INA3221_ADDR 64
#endif
#ifndef HAL_BATTMON_INA3221_SHUNT_OHMS
#define HAL_BATTMON_INA3221_SHUNT_OHMS 0.001
#endif
#define HAL_BATTMON_INA3221_CONV_TIME_140US 0b000
#define HAL_BATTMON_INA3221_CONV_TIME_204US 0b001
#define HAL_BATTMON_INA3221_CONV_TIME_332US 0b010
#define HAL_BATTMON_INA3221_CONV_TIME_588US 0b011
#define HAL_BATTMON_INA3221_CONV_TIME_1100US 0b100
#define HAL_BATTMON_INA3221_CONV_TIME_2116US 0b101
#define HAL_BATTMON_INA3221_CONV_TIME_4156US 0b110
#define HAL_BATTMON_INA3221_CONV_TIME_8244US 0b111
#define HAL_BATTMON_INA3221_AVG_MODE_1 0b000
#define HAL_BATTMON_INA3221_AVG_MODE_4 0b001
#define HAL_BATTMON_INA3221_AVG_MODE_16 0b010
#define HAL_BATTMON_INA3221_AVG_MODE_64 0b011
#define HAL_BATTMON_INA3221_AVG_MODE_128 0b100
#define HAL_BATTMON_INA3221_AVG_MODE_256 0b101
#define HAL_BATTMON_INA3221_AVG_MODE_512 0b110
#define HAL_BATTMON_INA3221_AVG_MODE_1024 0b111
#ifndef HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL
#define HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US
#endif
#ifndef HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL
#define HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US
#endif
#ifndef HAL_BATTMON_INA3221_AVG_MODE_SEL
#define HAL_BATTMON_INA3221_AVG_MODE_SEL HAL_BATTMON_INA3221_AVG_MODE_1024
#endif
struct AP_BattMonitor_INA3221::AddressDriver AP_BattMonitor_INA3221::address_driver[HAL_BATTMON_INA3221_MAX_DEVICES];
uint8_t AP_BattMonitor_INA3221::address_driver_count;
const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = {
// Param indexes must be between 56 and 61 to avoid conflict with other battery monitor param tables loaded by pointer
// @Param: I2C_BUS
// @DisplayName: Battery monitor I2C bus number
// @Description: Battery monitor I2C bus number
// @Range: 0 3
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("I2C_BUS", 56, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS),
// @Param: I2C_ADDR
// @DisplayName: Battery monitor I2C address
// @Description: Battery monitor I2C address. If this is zero then probe list of supported addresses
// @Range: 0 127
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("I2C_ADDR", 57, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR),
// @Param: CHANNEL
// @DisplayName: INA3221 channel
// @Description: INA3221 channel to return data for
// @Range: 1 3
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO("CHANNEL", 58, AP_BattMonitor_INA3221, channel, 1),
AP_GROUPEND
};
extern const AP_HAL::HAL &hal;
AP_BattMonitor_INA3221::AP_BattMonitor_INA3221(
AP_BattMonitor &mon,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_BattMonitor_Params &params) :
AP_BattMonitor_Backend(mon, mon_state, params)
{
AP_Param::setup_object_defaults(this, var_info);
_state.var_info = var_info;
}
bool AP_BattMonitor_INA3221::AddressDriver::read_register(uint8_t addr, uint16_t &ret)
{
if (!dev->transfer(&addr, 1, (uint8_t*)&ret, 2)) {
return false;
}
ret = be16toh(ret);
return true;
}
bool AP_BattMonitor_INA3221::AddressDriver::write_register(uint8_t addr, uint16_t val)
{
uint8_t buf[3] { addr, uint8_t(val >> 8), uint8_t(val & 0xFF) };
return dev->transfer(buf, sizeof(buf), nullptr, 0);
}
#define REG_CONFIGURATION 0x00
#define REG_MANUFACTURER_ID 0xFE
#define REG_DIE_ID 0xFF
bool AP_BattMonitor_INA3221::AddressDriver::write_config(void)
{
// update device configuration
union {
struct PACKED {
uint16_t mode : 3;
uint16_t shunt_voltage_conversiontime : 3;
uint16_t bus_voltage_conversiontime : 3;
uint16_t averaging_mode : 3;
uint16_t ch1_enable : 1;
uint16_t ch2_enable : 1;
uint16_t ch3_enable : 1;
uint16_t reset : 1;
} bits;
uint16_t word;
} configuration {{
0b111, // continuous operation
HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL,
HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL,
HAL_BATTMON_INA3221_AVG_MODE_SEL,
// dynamically enable channels to not waste time converting unused data
(channel_mask & (1 << 1)) != 0, // enable ch1?
(channel_mask & (1 << 2)) != 0, // enable ch2?
(channel_mask & (1 << 3)) != 0, // enable ch3?
0b0, // don't reset...
}};
if (!write_register(REG_CONFIGURATION, configuration.word)) {
return false;
}
dev_channel_mask = channel_mask; // what's actually in the device now
return true;
}
void AP_BattMonitor_INA3221::init()
{
uint8_t dev_address = i2c_address.get();
uint8_t dev_bus = i2c_bus.get();
uint8_t dev_channel = channel.get();
if ((dev_channel < 1) || (dev_channel > 3)) {
debug("INA3221: nonexistent channel");
return;
}
debug("INA3221: probe ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus);
// check to see if we already have the underlying driver reading the bus:
for (uint8_t i=0; i<address_driver_count; i++) {
AddressDriver *d = &address_driver[i];
if (!d->dev) {
continue;
}
if (d->address != dev_address) {
continue;
}
if (d->bus != dev_bus) {
continue;
}
debug("Reusing INA3221 driver @0x%02x on bus %u", dev_address, dev_bus);
address_driver_state = NEW_NOTHROW AddressDriver::StateList;
if (address_driver_state == nullptr) {
return;
}
address_driver_state->channel = dev_channel;
address_driver_state->next = d->statelist;
d->statelist = address_driver_state;
d->channel_mask |= (1 << dev_channel);
return;
}
if (address_driver_count == ARRAY_SIZE(address_driver)) {
debug("INA3221: out of address drivers");
return;
}
AddressDriver *d = &address_driver[address_driver_count];
d->dev = std::move(hal.i2c_mgr->get_device(i2c_bus, i2c_address, 100000, true, 20));
if (!d->dev) {
return;
}
d->bus = i2c_bus;
d->address = i2c_address;
WITH_SEMAPHORE(d->dev->get_semaphore());
// check manufacturer_id
uint16_t manufacturer_id;
if (!d->read_register(REG_MANUFACTURER_ID, manufacturer_id)) {
debug("read register (%u (0x%02x)) failed", REG_MANUFACTURER_ID, REG_MANUFACTURER_ID);
return;
}
if (manufacturer_id != 0x5449) { // 8.6.1 p24
debug("Bad manufacturer_id: 0x%02x", manufacturer_id);
return;
}
uint16_t die_id;
if (!d->read_register(REG_DIE_ID, die_id)) {
debug("Bad die: 0x%02x", manufacturer_id);
return;
}
if (die_id != 0x3220) { // 8.6.1 p24
return;
}
d->channel_mask = (1 << dev_channel);
if (!d->write_config()) {
return;
}
address_driver_state = NEW_NOTHROW AddressDriver::StateList;
if (address_driver_state == nullptr) {
return;
}
address_driver_state->channel = dev_channel;
address_driver_state->next = d->statelist;
d->statelist = address_driver_state;
debug("Found INA3221 ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus);
address_driver_count++;
d->register_timer();
}
void AP_BattMonitor_INA3221::AddressDriver::register_timer(void)
{
dev->register_periodic_callback(
100000,
FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA3221::AddressDriver::timer, void));
}
void AP_BattMonitor_INA3221::AddressDriver::timer(void)
{
bool healthy = true;
if (channel_mask != dev_channel_mask) {
if (write_config()) { // update enabled channels
return; // data is now out of date, read it next time
}
// continue on to reading if update failed so health gets cleared
healthy = false;
}
for (uint8_t i=1; i<=3; i++) {
if ((channel_mask & (1U<<i)) == 0) {
continue;
}
const uint8_t channel_offset = (i-1)*2;
const uint8_t reg_shunt = 1 + channel_offset;
const uint8_t reg_bus = 2 + channel_offset;
uint16_t shunt_val;
if (!read_register(reg_shunt, shunt_val)) {
healthy = false;
shunt_val = 0;
}
uint16_t bus_val;
if (!read_register(reg_bus, bus_val)) {
healthy = false;
bus_val = 0;
}
// 2s complement number with 3 lowest bits not used, 1 count is 8mV
const float bus_voltage = ((int16_t)bus_val >> 3)*8e-3;
// 2s complement number with 3 lowest bits not used, 1 count is 40uV
const float shunt_voltage = ((int16_t)shunt_val >> 3)*40e-6;
const float shunt_resistance = HAL_BATTMON_INA3221_SHUNT_OHMS;
const float shunt_current = shunt_voltage/shunt_resistance; // I = V/R
// transfer readings to state
for (auto *state = statelist; state != nullptr; state = state->next) {
if (state->channel != i) {
continue;
}
WITH_SEMAPHORE(state->sem);
// calculate time since last data read
const uint32_t tnow = AP_HAL::micros();
const uint32_t dt_us = tnow - state->last_time_micros;
state->healthy = healthy;
state->voltage = bus_voltage;
state->current_amps = shunt_current;
// update current drawn since last reading for read to accumulate
if (state->last_time_micros != 0 && dt_us < 2000000) {
const float mah = calculate_mah(state->current_amps, dt_us);
state->delta_mah += mah;
state->delta_wh += 0.001 * mah * state->voltage;
}
state->last_time_micros = tnow;
}
}
}
void AP_BattMonitor_INA3221::read()
{
if (address_driver_state == nullptr) {
return;
}
WITH_SEMAPHORE(address_driver_state->sem);
// copy state data to front-end under semaphore
_state.healthy = address_driver_state->healthy;
_state.voltage = address_driver_state->voltage;
_state.current_amps = address_driver_state->current_amps;
_state.consumed_mah += address_driver_state->delta_mah;
_state.consumed_wh += address_driver_state->delta_wh;
_state.last_time_micros = address_driver_state->last_time_micros;
address_driver_state->delta_mah = 0;
address_driver_state->delta_wh = 0;
}
#endif // AP_BATTERY_INA3221_ENABLED

110
libraries/AP_BattMonitor/AP_BattMonitor_INA3221.h Normal file
View File

@ -0,0 +1,110 @@
#pragma once
#include "AP_BattMonitor_config.h"
#if AP_BATTERY_INA3221_ENABLED
/*
*
* Datasheet: https://www.ti.com/lit/ds/symlink/ina3221.pdf?ts=1597369254046
*/
// The INA3221 takes two measurements for each channel: one for shunt voltage
// and one for bus voltage. Each measurement can be independently or
// sequentially measured, based on the mode setting (bits 2-0 in the
// Configuration register). When the INA3221 is in normal operating mode
// (that is, the MODE bits of the Configuration register are set to 111), the
// device continuously converts a shunt-voltage reading followed by a
// bus-voltage reading. This procedure converts one channel, and then continues
// to the shunt voltage reading of the next enabled channel, followed by the
// bus-voltage reading for that channel, and so on, until all enabled channels
// have been measured. The programmed Configuration register mode setting
// applies to all channels. Any channels that are not enabled are bypassed in
// the measurement sequence, regardless of mode setting.
// 8.3.3 Software Reset
// The INA3221 features a software reset that reinitializes the device and
// register settings to default power-up values without having to cycle power
// to the device. Use bit 15 (RST) of the Configuration register to perform a
// software reset. Setting RST reinitializes all registers and settings to the
// default power state with the exception of the power-valid output state. If a
// software reset is issued, the INA3221 holds the output of the PV pin until
// the power-valid detection sequence completes. The Power-Valid UpperLimit and
// Power-Valid Lowerlimit registers return to the default state when the
// software reset has been issued. Therefore, any reprogrammed limit registers
// are reset, resulting in the original power-valid thresholds validating the
// power-valid conditions. This architecture prevents interruption to circuitry
// connected to the powervalid output during a software reset event.
// The INA3221 has programmable conversion times for both the shunt- and
// bus-voltage measurements. The selectable conversion times for these
// measurements range from 140μs to 8.244ms.
#include "AP_BattMonitor.h"
#include "AP_BattMonitor_Backend.h"
#ifndef HAL_BATTMON_INA3221_MAX_DEVICES
#define HAL_BATTMON_INA3221_MAX_DEVICES 1
#endif
class AP_BattMonitor_INA3221 : public AP_BattMonitor_Backend
{
public:
/// Constructor
AP_BattMonitor_INA3221(AP_BattMonitor &mon,
AP_BattMonitor::BattMonitor_State &mon_state,
AP_BattMonitor_Params &params);
void init() override;
/// Read the battery voltage and current. Should be called at 10hz
void read() override;
bool has_current() const override {
return true;
}
static const struct AP_Param::GroupInfo var_info[];
private:
AP_Int8 i2c_bus;
AP_Int8 i2c_address;
AP_Int8 channel;
static struct AddressDriver {
bool read_register(uint8_t addr, uint16_t &ret);
bool write_register(uint8_t addr, uint16_t val);
bool write_config(void);
void timer(void);
void register_timer();
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev;
uint8_t bus;
uint8_t address;
uint8_t channel_mask;
uint8_t dev_channel_mask;
struct StateList {
struct StateList *next;
HAL_Semaphore sem;
uint8_t channel;
bool healthy;
float voltage;
float current_amps;
float delta_mah;
float delta_wh;
uint32_t last_time_micros;
};
StateList *statelist;
} address_driver[HAL_BATTMON_INA3221_MAX_DEVICES];
static uint8_t address_driver_count;
AddressDriver::StateList *address_driver_state;
};
#endif // AP_BATTERY_INA3221_ENABLED

2
libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
View File

@ -21,7 +21,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
// @Param: MONITOR
// @DisplayName: Battery monitoring
// @Description: Controls enabling monitoring of the battery's voltage and current
// @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting
// @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Generic,8:DroneCAN-BatteryInfo,9:ESC,10:Sum Of Selected Monitors,11:FuelFlow,12:FuelLevelPWM,13:SMBUS-SUI3,14:SMBUS-SUI6,15:NeoDesign,16:SMBus-Maxell,17:Generator-Elec,18:Generator-Fuel,19:Rotoye,20:MPPT,21:INA2XX,22:LTC2946,23:Torqeedo,24:FuelLevelAnalog,25:Synthetic Current and Analog Voltage,26:INA239_SPI,27:EFI,28:AD7091R5,29:Scripting,30:INA3221
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),

5
libraries/AP_BattMonitor/AP_BattMonitor_config.h
View File

@ -70,6 +70,11 @@
#define AP_BATTERY_INA2XX_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && (BOARD_FLASH_SIZE > 1024))
#endif
#ifndef AP_BATTERY_INA3221_ENABLED
// turned on in hwdefs (except for sim test), requires config
#define AP_BATTERY_INA3221_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif
#ifndef AP_BATTERY_LTC2946_ENABLED
#define AP_BATTERY_LTC2946_ENABLED (AP_BATTERY_BACKEND_DEFAULT_ENABLED && defined(HAL_BATTMON_LTC2946_BUS) && defined(HAL_BATTMON_LTC2946_ADDR))
#endif

106
libraries/AP_DroneCAN/AP_DroneCAN.cpp
View File

@ -50,6 +50,7 @@
#include <AP_OpenDroneID/AP_OpenDroneID.h>
#include <AP_Mount/AP_Mount_Xacti.h>
#include <string.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>
#if AP_DRONECAN_SERIAL_ENABLED
#include "AP_DroneCAN_serial.h"
@ -1379,62 +1380,83 @@ void AP_DroneCAN::handle_traffic_report(const CanardRxTransfer& transfer, const
/*
handle actuator status message
*/
#if AP_SERVO_TELEM_ENABLED
void AP_DroneCAN::handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg)
{
#if HAL_LOGGING_ENABLED
// log as CSRV message
AP::logger().Write_ServoStatus(AP_HAL::micros64(),
msg.actuator_id,
msg.position,
msg.force,
msg.speed,
msg.power_rating_pct,
0, 0, 0, 0, 0, 0);
#endif
}
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem == nullptr) {
return;
}
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
const AP_Servo_Telem::TelemetryData telem_data {
.measured_position = ToDeg(msg.position),
.force = msg.force,
.speed = msg.speed,
.duty_cycle = msg.power_rating_pct,
.valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::FORCE |
AP_Servo_Telem::TelemetryData::Types::SPEED |
AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE
};
servo_telem->update_telem_data(msg.actuator_id, telem_data);
}
#endif
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
/*
handle himark ServoInfo message
*/
void AP_DroneCAN::handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg)
{
#if HAL_LOGGING_ENABLED
// log as CSRV message
AP::logger().Write_ServoStatus(AP_HAL::micros64(),
msg.servo_id,
msg.pos_sensor*0.01,
0,
0,
0,
msg.pos_cmd*0.01,
msg.voltage*0.01,
msg.current*0.01,
msg.motor_temp*0.2-40,
msg.pcb_temp*0.2-40,
msg.error_status);
#endif
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem == nullptr) {
return;
}
const AP_Servo_Telem::TelemetryData telem_data {
.command_position = msg.pos_cmd * 0.01,
.measured_position = msg.pos_sensor * 0.01,
.voltage = msg.voltage * 0.01,
.current = msg.current * 0.01,
.motor_temperature_cdeg = int16_t(((msg.motor_temp * 0.2) - 40) * 100),
.pcb_temperature_cdeg = int16_t(((msg.pcb_temp * 0.2) - 40) * 100),
.status_flags = msg.error_status,
.valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
AP_Servo_Telem::TelemetryData::Types::CURRENT |
AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
AP_Servo_Telem::TelemetryData::Types::PCB_TEMP |
AP_Servo_Telem::TelemetryData::Types::STATUS
};
servo_telem->update_telem_data(msg.servo_id, telem_data);
}
#endif // AP_DRONECAN_HIMARK_SERVO_SUPPORT
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg)
{
#if HAL_LOGGING_ENABLED
AP::logger().WriteStreaming(
"CVOL",
"TimeUS,Id,Pos,Cur,V,Pow,T",
"s#dAv%O",
"F-00000",
"QBfffBh",
AP_HAL::micros64(),
msg.actuator_id,
ToDeg(msg.actual_position),
msg.current * 0.025f,
msg.voltage * 0.2f,
uint8_t(msg.motor_pwm * (100.0/255.0)),
int16_t(msg.motor_temperature) - 50);
#endif
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem == nullptr) {
return;
}
const AP_Servo_Telem::TelemetryData telem_data {
.measured_position = ToDeg(msg.actual_position),
.voltage = msg.voltage * 0.2,
.current = msg.current * 0.025,
.duty_cycle = msg.motor_pwm * (100.0/255.0),
.motor_temperature_cdeg = (int16_t(msg.motor_temperature) - 50)) * 100,
.valid_types = AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
AP_Servo_Telem::TelemetryData::Types::CURRENT |
AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE |
AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP
};
servo_telem->update_telem_data(msg.actuator_id, telem_data);
}
#endif

17
libraries/AP_DroneCAN/AP_DroneCAN.h
View File

@ -36,6 +36,7 @@
#include <dronecan_msgs.h>
#include <AP_SerialManager/AP_SerialManager_config.h>
#include <AP_Relay/AP_Relay_config.h>
#include <AP_Servo_Telem/AP_Servo_Telem_config.h>
#ifndef DRONECAN_SRV_NUMBER
#define DRONECAN_SRV_NUMBER NUM_SERVO_CHANNELS
@ -329,8 +330,10 @@ private:
Canard::ObjCallback<AP_DroneCAN, ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_cb{this, &AP_DroneCAN::handle_traffic_report};
Canard::Subscriber<ardupilot_equipment_trafficmonitor_TrafficReport> traffic_report_listener{traffic_report_cb, _driver_index};
#if AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_actuator_Status> actuator_status_cb{this, &AP_DroneCAN::handle_actuator_status};
Canard::Subscriber<uavcan_equipment_actuator_Status> actuator_status_listener{actuator_status_cb, _driver_index};
#endif
Canard::ObjCallback<AP_DroneCAN, uavcan_equipment_esc_Status> esc_status_cb{this, &AP_DroneCAN::handle_ESC_status};
Canard::Subscriber<uavcan_equipment_esc_Status> esc_status_listener{esc_status_cb, _driver_index};
@ -343,7 +346,11 @@ private:
Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo};
Canard::Subscriber<com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb_listener{himark_servo_ServoInfo_cb, _driver_index};
#endif
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED
Canard::ObjCallback<AP_DroneCAN, com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz};
Canard::Subscriber<com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index};
#endif
@ -401,15 +408,19 @@ private:
void handle_hobbywing_StatusMsg2(const CanardRxTransfer& transfer, const com_hobbywing_esc_StatusMsg2& msg);
#endif // AP_DRONECAN_HOBBYWING_ESC_SUPPORT
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT
#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
void handle_himark_servoinfo(const CanardRxTransfer& transfer, const com_himark_servo_ServoInfo &msg);
#endif
// incoming button handling
void handle_button(const CanardRxTransfer& transfer, const ardupilot_indication_Button& msg);
void handle_traffic_report(const CanardRxTransfer& transfer, const ardupilot_equipment_trafficmonitor_TrafficReport& msg);
#if AP_SERVO_TELEM_ENABLED
void handle_actuator_status(const CanardRxTransfer& transfer, const uavcan_equipment_actuator_Status& msg);
#endif
#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED && AP_SERVO_TELEM_ENABLED
void handle_actuator_status_Volz(const CanardRxTransfer& transfer, const com_volz_servo_ActuatorStatus& msg);
#endif
void handle_ESC_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_Status& msg);
#if AP_EXTENDED_ESC_TELEM_ENABLED
void handle_esc_ext_status(const CanardRxTransfer& transfer, const uavcan_equipment_esc_StatusExtended& msg);

4
libraries/AP_HAL/AP_HAL_Boards.h
View File

@ -385,4 +385,8 @@
#define HAL_WITH_POSTYPE_DOUBLE BOARD_FLASH_SIZE > 1024
#endif
#ifndef HAL_INS_RATE_LOOP
#define HAL_INS_RATE_LOOP 0
#endif
#define HAL_GPIO_LED_OFF (!HAL_GPIO_LED_ON)

5
libraries/AP_HAL/RCOutput.h
View File

@ -376,6 +376,11 @@ public:
*/
virtual void write_gpio(uint8_t chan, bool active) {};
/*
Force group trigger from all callers rather than just from the main thread
*/
virtual void force_trigger_groups(bool onoff) {};
/*
* calculate the prescaler required to achieve the desire bitrate
*/

6
libraries/AP_HAL/board/chibios.h
View File

@ -140,3 +140,9 @@
#endif
#define HAL_USE_QUADSPI (HAL_USE_QUADSPI1 || HAL_USE_QUADSPI2)
#define HAL_USE_OCTOSPI (HAL_USE_OCTOSPI1 || HAL_USE_OCTOSPI2)
#if defined(STM32H7) || defined(STM32F7)
#define HAL_INS_RATE_LOOP 1
#else
#define HAL_INS_RATE_LOOP 0
#endif

4
libraries/AP_HAL/board/linux.h
View File

@ -422,3 +422,7 @@
#else
#define HAL_LINUX_USE_VIRTUAL_CAN 0
#endif
#ifndef HAL_INS_RATE_LOOP
#define HAL_INS_RATE_LOOP 1
#endif

4
libraries/AP_HAL/board/sitl.h
View File

@ -96,3 +96,7 @@
#endif
#define HAL_SOLO_GIMBAL_ENABLED 1
#ifndef HAL_INS_RATE_LOOP
#define HAL_INS_RATE_LOOP 1
#endif

26
libraries/AP_HAL_ChibiOS/RCOutput.cpp
View File

@ -553,19 +553,19 @@ void RCOutput::set_dshot_rate(uint8_t dshot_rate, uint16_t loop_rate_hz)
}
uint16_t drate = dshot_rate * loop_rate_hz;
_dshot_rate = dshot_rate;
// BLHeli32 uses a 16 bit counter for input calibration which at 48Mhz will wrap
// at 732Hz so never allow rates below 800hz
while (drate < 800) {
_dshot_rate++;
drate = _dshot_rate * loop_rate_hz;
dshot_rate++;
drate = dshot_rate * loop_rate_hz;
}
// prevent stupidly high rates, ideally should also prevent high rates
// prevent stupidly high rate multiples, ideally should also prevent high rates
// with slower dshot variants
if (drate > 4000) {
_dshot_rate = 4000 / loop_rate_hz;
drate = _dshot_rate * loop_rate_hz;
while (dshot_rate > 1 && drate > MAX(4096, loop_rate_hz)) {
dshot_rate--;
drate = dshot_rate * loop_rate_hz;
}
_dshot_rate = dshot_rate;
_dshot_period_us = 1000000UL / drate;
#if HAL_WITH_IO_MCU
if (iomcu_dshot) {
@ -1329,6 +1329,9 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len)
*/
void RCOutput::cork(void)
{
if (corked) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
corked = true;
#if HAL_WITH_IO_MCU
if (iomcu_enabled) {
@ -1342,6 +1345,9 @@ void RCOutput::cork(void)
*/
void RCOutput::push(void)
{
if (!corked) {
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
corked = false;
push_local();
#if HAL_WITH_IO_MCU
@ -1398,7 +1404,11 @@ void RCOutput::trigger_groups()
osalSysUnlock();
#if !defined(HAL_NO_RCOUT_THREAD) || HAL_DSHOT_ENABLED
// trigger a PWM send
if (!in_soft_serial() && hal.scheduler->in_main_thread() && rcout_thread_ctx) {
if (!in_soft_serial() &&
// we always trigger an output if we are in the main thread
// we also always trigger an output if we are in the rate thread and thus
// force_trigger has been set
(hal.scheduler->in_main_thread() || force_trigger) && rcout_thread_ctx) {
chEvtSignal(rcout_thread_ctx, EVT_PWM_SEND);
}
#endif

7
libraries/AP_HAL_ChibiOS/RCOutput.h
View File

@ -285,6 +285,11 @@ public:
*/
void rcout_thread();
/*
Force group trigger from all callers rather than just from the main thread
*/
void force_trigger_groups(bool onoff) override { force_trigger = onoff; }
/*
timer information
*/
@ -579,6 +584,8 @@ private:
uint8_t _dshot_cycle;
// virtual timer for post-push() pulses
virtual_timer_t _dshot_rate_timer;
// force triggering of groups, this is used by the rate thread to ensure output occurs
bool force_trigger;
#if HAL_DSHOT_ENABLED
// dshot commands

1
libraries/AP_HAL_ChibiOS/hwdef/MambaH743v4/hwdef.dat
View File

@ -20,6 +20,7 @@ define CH_CFG_ST_RESOLUTION 16
# leave 1 sectors free
FLASH_RESERVE_START_KB 384
env OPTIMIZE -O2
# use last 2 pages for flash storage
# H743 has 16 pages of 128k each

3
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1863,8 +1863,7 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
{
if (!is_equal(last_bandwidth_hz[instance], params.bandwidth_hz()) ||
!is_equal(last_attenuation_dB[instance], params.attenuation_dB()) ||
(params.tracking_mode() == HarmonicNotchDynamicMode::Fixed &&
!is_equal(last_center_freq_hz[instance], params.center_freq_hz())) ||
!is_equal(last_center_freq_hz[instance], params.center_freq_hz()) ||
converging) {
filter[instance].init(gyro_rate, params);
last_center_freq_hz[instance] = params.center_freq_hz();

30
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -34,6 +34,7 @@
class AP_InertialSensor_Backend;
class AuxiliaryBus;
class AP_AHRS;
class FastRateBuffer;
/*
forward declare AP_Logger class. We can't include logger.h
@ -51,6 +52,7 @@ class AP_Logger;
class AP_InertialSensor : AP_AccelCal_Client
{
friend class AP_InertialSensor_Backend;
friend class FastRateBuffer;
public:
AP_InertialSensor();
@ -161,12 +163,12 @@ public:
const Vector3f& get_gyro_for_fft(void) const { return _gyro_for_fft[_first_usable_gyro]; }
FloatBuffer& get_raw_gyro_window(uint8_t instance, uint8_t axis) { return _gyro_window[instance][axis]; }
FloatBuffer& get_raw_gyro_window(uint8_t axis) { return get_raw_gyro_window(_first_usable_gyro, axis); }
uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); }
uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; }
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
bool has_fft_notch() const;
#endif
#endif
uint16_t get_raw_gyro_rate_hz(uint8_t instance) const { return _gyro_raw_sample_rates[_first_usable_gyro]; }
uint16_t get_raw_gyro_rate_hz() const { return get_raw_gyro_rate_hz(_first_usable_gyro); }
bool set_gyro_window_size(uint16_t size);
// get accel offsets in m/s/s
const Vector3f &get_accel_offsets(uint8_t i) const { return _accel_offset(i); }
@ -263,6 +265,7 @@ public:
AuxiliaryBus *get_auxiliary_bus(int16_t backend_id, uint8_t instance);
void detect_backends(void);
void update_backends();
// accel peak hold detector
void set_accel_peak_hold(uint8_t instance, const Vector3f &accel);
@ -798,6 +801,29 @@ private:
bool raw_logging_option_set(RAW_LOGGING_OPTION option) const {
return (raw_logging_options.get() & int32_t(option)) != 0;
}
// if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
// Support for the fast rate thread in copter
FastRateBuffer* fast_rate_buffer;
bool fast_rate_buffer_enabled;
public:
// enable the fast rate buffer and start pushing samples to it
void enable_fast_rate_buffer();
// disable the fast rate buffer and stop pushing samples to it
void disable_fast_rate_buffer();
// get the next available gyro sample from the fast rate buffer
bool get_next_gyro_sample(Vector3f& gyro);
// get the number of available gyro samples in the fast rate buffer
uint32_t get_num_gyro_samples();
// set the rate at which samples are collected, unused samples are dropped
void set_rate_decimation(uint8_t rdec);
// push a new gyro sample into the fast rate buffer
bool push_next_gyro_sample(const Vector3f& gyro);
// run the filter parmeter update code.
void update_backend_filters();
// are rate loop samples enabled for this instance?
bool is_rate_loop_gyro_enabled(uint8_t instance) const;
// endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
};
namespace AP {

14
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -2,6 +2,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_AHRS/AP_AHRS.h>
#include "AP_InertialSensor_rate_config.h"
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
#include <AP_Logger/AP_Logger.h>
@ -254,9 +255,21 @@ void AP_InertialSensor_Backend::apply_gyro_filters(const uint8_t instance, const
notch.filter[instance].reset();
}
#endif
gyro_filtered = _imu._gyro_filtered[instance];
}
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
if (_imu.is_rate_loop_gyro_enabled(instance)) {
if (_imu.push_next_gyro_sample(gyro_filtered)) {
// if we used the value, record it for publication to the front-end
_imu._gyro_filtered[instance] = gyro_filtered;
}
} else {
_imu._gyro_filtered[instance] = gyro_filtered;
}
#else
_imu._gyro_filtered[instance] = gyro_filtered;
#endif
}
void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
@ -772,6 +785,7 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
if (has_been_killed(instance)) {
return;
}
if (_imu._new_gyro_data[instance]) {
_publish_gyro(instance, _imu._gyro_filtered[instance]);
#if HAL_GYROFFT_ENABLED

9
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -53,6 +53,15 @@ public:
*/
virtual bool update() = 0; /* front end */
// if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
/*
* Update the filter parameters. Called by the frontend to propagate
* filter parameters to the frontend structure via the
* update_gyro_filters() and update_accel_filters() functions
*/
void update_filters() __RAMFUNC__; /* front end */
// endif AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
/*
* optional function to accumulate more samples. This is needed for drivers that don't use a timer to gather samples
*/

9
libraries/AP_InertialSensor/AP_InertialSensor_rate_config.h Normal file
View File

@ -0,0 +1,9 @@
#pragma once
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_InertialSensor/AP_InertialSensor_config.h>
#ifndef AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
#define AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED (AP_INERTIALSENSOR_ENABLED && HAL_INS_RATE_LOOP && AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && APM_BUILD_TYPE(APM_BUILD_ArduCopter))
#endif

150
libraries/AP_InertialSensor/FastRateBuffer.cpp Normal file
View File

@ -0,0 +1,150 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_AHRS/AP_AHRS.h>
#include "AP_InertialSensor_rate_config.h"
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
#include "FastRateBuffer.h"
#include <stdio.h>
extern const AP_HAL::HAL& hal;
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
// hal.console can be accessed from bus threads on ChibiOS
#define debug(fmt, args ...) do {hal.console->printf("IMU: " fmt "\n", ## args); } while(0)
#else
#define debug(fmt, args ...) do {printf("IMU: " fmt "\n", ## args); } while(0)
#endif
// enable the fast rate buffer and start pushing samples to it
void AP_InertialSensor::enable_fast_rate_buffer()
{
if (fast_rate_buffer_enabled) {
return;
}
if (fast_rate_buffer == nullptr) {
fast_rate_buffer = NEW_NOTHROW FastRateBuffer();
}
fast_rate_buffer_enabled = fast_rate_buffer != nullptr;
}
// disable the fast rate buffer and stop pushing samples to it
void AP_InertialSensor::disable_fast_rate_buffer()
{
fast_rate_buffer_enabled = false;
if (fast_rate_buffer != nullptr) {
fast_rate_buffer->reset();
}
}
// get the number of available gyro samples in the fast rate buffer
uint32_t AP_InertialSensor::get_num_gyro_samples()
{
if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return 0;
}
return fast_rate_buffer->get_num_gyro_samples();
}
// set the rate at which samples are collected, unused samples are dropped
void AP_InertialSensor::set_rate_decimation(uint8_t rdec)
{
if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return;
}
fast_rate_buffer->set_rate_decimation(rdec);
}
// whether or not to push the current gyro sample
bool AP_InertialSensor::is_rate_loop_gyro_enabled(uint8_t instance) const
{
if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return false;
}
return fast_rate_buffer->use_rate_loop_gyro_samples() && instance == AP::ahrs().get_primary_gyro_index();
}
// get the next available gyro sample from the fast rate buffer
bool AP_InertialSensor::get_next_gyro_sample(Vector3f& gyro)
{
if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return false;
}
return fast_rate_buffer->get_next_gyro_sample(gyro);
}
bool FastRateBuffer::get_next_gyro_sample(Vector3f& gyro)
{
if (!use_rate_loop_gyro_samples()) {
return false;
}
if (_rate_loop_gyro_window.available() == 0) {
_notifier.wait_blocking();
}
WITH_SEMAPHORE(_mutex);
return _rate_loop_gyro_window.pop(gyro);
}
void FastRateBuffer::reset()
{
_rate_loop_gyro_window.clear();
}
bool AP_InertialSensor::push_next_gyro_sample(const Vector3f& gyro)
{
if (!fast_rate_buffer_enabled || fast_rate_buffer == nullptr) {
return false;
}
if (++fast_rate_buffer->rate_decimation_count < fast_rate_buffer->rate_decimation) {
return false;
}
/*
tell the rate thread we have a new sample
*/
WITH_SEMAPHORE(fast_rate_buffer->_mutex);
if (!fast_rate_buffer->_rate_loop_gyro_window.push(gyro)) {
debug("dropped rate loop sample");
}
fast_rate_buffer->rate_decimation_count = 0;
fast_rate_buffer->_notifier.signal();
return true;
}
void AP_InertialSensor::update_backend_filters()
{
for (uint8_t i=0; i<_backend_count; i++) {
_backends[i]->update_filters();
}
}
void AP_InertialSensor_Backend::update_filters()
{
WITH_SEMAPHORE(_sem);
update_accel_filters(accel_instance);
update_gyro_filters(gyro_instance);
}
#endif // AP_INERTIALSENSOR_RATE_LOOP_WINDOW_ENABLED

51
libraries/AP_InertialSensor/FastRateBuffer.h Normal file
View File

@ -0,0 +1,51 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "AP_InertialSensor.h"
#if AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
#define AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE 8 // gyro buffer size for rate loop
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Math/AP_Math.h>
#include <AP_HAL/Semaphores.h>
class FastRateBuffer
{
friend class AP_InertialSensor;
public:
bool get_next_gyro_sample(Vector3f& gyro);
uint32_t get_num_gyro_samples() { return _rate_loop_gyro_window.available(); }
void set_rate_decimation(uint8_t rdec) { rate_decimation = rdec; }
// whether or not to push the current gyro sample
bool use_rate_loop_gyro_samples() const { return rate_decimation > 0; }
bool gyro_samples_available() { return _rate_loop_gyro_window.available() > 0; }
void reset();
private:
/*
binary semaphore for rate loop to use to start a rate loop when
we hav finished filtering the primary IMU
*/
ObjectBuffer<Vector3f> _rate_loop_gyro_window{AP_INERTIAL_SENSOR_RATE_LOOP_BUFFER_SIZE};
uint8_t rate_decimation; // 0 means off
uint8_t rate_decimation_count;
HAL_BinarySemaphore _notifier;
HAL_Semaphore _mutex;
};
#endif

2
libraries/AP_Logger/AP_Logger.h
View File

@ -262,8 +262,6 @@ public:
void Write_Radio(const mavlink_radio_t &packet);
void Write_Message(const char *message);
void Write_MessageF(const char *fmt, ...);
void Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct,
float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error);
void Write_Compass();
void Write_Mode(uint8_t mode, const ModeReason reason);

25
libraries/AP_Logger/LogFile.cpp
View File

@ -452,31 +452,6 @@ bool AP_Logger_Backend::Write_Mode(uint8_t mode, const ModeReason reason)
return WriteCriticalBlock(&pkt, sizeof(pkt));
}
/*
write servo status from CAN servo
*/
void AP_Logger::Write_ServoStatus(uint64_t time_us, uint8_t id, float position, float force, float speed, uint8_t power_pct,
float pos_cmd, float voltage, float current, float mot_temp, float pcb_temp, uint8_t error)
{
const struct log_CSRV pkt {
LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
time_us : time_us,
id : id,
position : position,
force : force,
speed : speed,
power_pct : power_pct,
pos_cmd : pos_cmd,
voltage : voltage,
current : current,
mot_temp : mot_temp,
pcb_temp : pcb_temp,
error : error,
};
WriteBlock(&pkt, sizeof(pkt));
}
// Write a Yaw PID packet
void AP_Logger::Write_PID(uint8_t msg_type, const AP_PIDInfo &info)
{

38
libraries/AP_Logger/LogStructure.h
View File

@ -67,6 +67,7 @@ const struct UnitStructure log_Units[] = {
{ '%', "%" }, // percent
{ 'S', "satellites" }, // number of satellites
{ 's', "s" }, // seconds
{ 't', "N.m" }, // Newton meters, torque
{ 'q', "rpm" }, // rounds per minute. Not SI, but sometimes more intuitive than Hertz
{ 'r', "rad" }, // radians
{ 'U', "deglongitude" }, // degrees of longitude
@ -147,6 +148,7 @@ const struct MultiplierStructure log_Multipliers[] = {
#include <AC_AttitudeControl/LogStructure.h>
#include <AP_HAL/LogStructure.h>
#include <AP_Mission/LogStructure.h>
#include <AP_Servo_Telem/LogStructure.h>
// structure used to define logging format
// It is packed on ChibiOS to save flash space; however, this causes problems
@ -476,22 +478,6 @@ struct PACKED log_TERRAIN {
float reference_offset;
};
struct PACKED log_CSRV {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
float position;
float force;
float speed;
uint8_t power_pct;
float pos_cmd;
float voltage;
float current;
float mot_temp;
float pcb_temp;
uint8_t error;
};
struct PACKED log_ARSP {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -698,21 +684,6 @@ struct PACKED log_VER {
// @Field: TR: innovation test ratio
// @Field: Pri: True if sensor is the primary sensor
// @LoggerMessage: CSRV
// @Description: Servo feedback data
// @Field: TimeUS: Time since system startup
// @Field: Id: Servo number this data relates to
// @Field: Pos: Current servo position
// @Field: Force: Force being applied
// @Field: Speed: Current servo movement speed
// @Field: Pow: Amount of rated power being applied
// @Field: PosCmd: commanded servo position
// @Field: V: Voltage
// @Field: A: Current
// @Field: MotT: motor temperature
// @Field: PCBT: PCB temperature
// @Field: Err: error flags
// @LoggerMessage: DMS
// @Description: DataFlash-Over-MAVLink statistics
// @Field: TimeUS: Time since system startup
@ -1222,8 +1193,7 @@ LOG_STRUCTURE_FROM_AVOIDANCE \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHHf","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded,ROfs", "s-DU-mm--m", "F-GG-00--0", true }, \
LOG_STRUCTURE_FROM_ESC_TELEM \
{ LOG_CSRV_MSG, sizeof(log_CSRV), \
"CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#---%dvAOO-", "F-000000000-", false }, \
LOG_STRUCTURE_FROM_SERVO_TELEM \
{ LOG_PIDR_MSG, sizeof(log_PID), \
"PIDR", PID_FMT, PID_LABELS, PID_UNITS, PID_MULTS, true }, \
{ LOG_PIDP_MSG, sizeof(log_PID), \
@ -1306,7 +1276,7 @@ enum LogMessages : uint8_t {
LOG_IDS_FROM_CAMERA,
LOG_IDS_FROM_MOUNT,
LOG_TERRAIN_MSG,
LOG_CSRV_MSG,
LOG_IDS_FROM_SERVO_TELEM,
LOG_IDS_FROM_ESC_TELEM,
LOG_IDS_FROM_BATTMONITOR,
LOG_IDS_FROM_HAL_CHIBIOS,

1
libraries/AP_Logger/README.md
View File

@ -68,6 +68,7 @@ Please keep the names consistent with Tools/autotest/param_metadata/param.py:33
| 's' | "s" | seconds|
| 'q' | "rpm" | revolutions per minute| Not an SI unit, but sometimes more intuitive than Hertz|
| 'r' | "rad" | radians|
| 't' | "N.m" | Newton meters | torque |
| 'U' | "deglongitude" | degrees of longitude|
| 'u' | "ppm" | pulses per minute|
| 'v' | "V" | Volt|

56
libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp
View File

@ -31,10 +31,10 @@
#include <AP_HAL/utility/sparse-endian.h>
#include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_CANManager/AP_CANManager.h>
#include <AP_EFI/AP_EFI_Currawong_ECU.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>
#include <stdio.h>
@ -320,8 +320,6 @@ bool AP_PiccoloCAN::read_frame(AP_HAL::CANFrame &recv_frame, uint32_t timeout_us
// called from SRV_Channels
void AP_PiccoloCAN::update()
{
uint64_t timestamp = AP_HAL::micros64();
/* Read out the servo commands from the channel mixer */
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
@ -361,46 +359,44 @@ void AP_PiccoloCAN::update()
}
#endif // AP_EFI_CURRAWONG_ECU_ENABLED
#if HAL_LOGGING_ENABLED
AP_Logger *logger = AP_Logger::get_singleton();
// Push received telemetry data into the logging system
if (logger && logger->logging_enabled()) {
WITH_SEMAPHORE(_telem_sem);
#if AP_SERVO_TELEM_ENABLED
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem != nullptr) {
for (uint8_t ii = 0; ii < PICCOLO_CAN_MAX_NUM_SERVO; ii++) {
AP_PiccoloCAN_Servo &servo = _servos[ii];
if (servo.newTelemetry) {
union {
Servo_ErrorBits_t ebits;
uint8_t errors;
} err;
err.ebits = servo.status.statusA.errors;
logger->Write_ServoStatus(
timestamp,
ii,
servo.position(), // Servo position (represented in microsecond units)
servo.current() * 0.01f, // Servo force (actually servo current, 0.01A per bit)
servo.speed(), // Servo speed (degrees per second)
servo.dutyCycle(), // Servo duty cycle (absolute value as it can be +/- 100%)
uint16_t(servo.commandedPosition()), // Commanded position
servo.voltage(), // Servo voltage
servo.current(), // Servo current
servo.temperature(), // Servo temperature
servo.temperature(), //
err.errors
);
const AP_Servo_Telem::TelemetryData telem_data {
.command_position = servo.commandedPosition(),
.measured_position = servo.position(),
.speed = servo.speed(),
.voltage = servo.voltage(),
.current = servo.current(),
.duty_cycle = servo.dutyCycle(),
.motor_temperature_cdeg = int16_t(servo.temperature() * 100),
.status_flags = err.errors,
.valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::SPEED |
AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
AP_Servo_Telem::TelemetryData::Types::CURRENT |
AP_Servo_Telem::TelemetryData::Types::DUTY_CYCLE |
AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
AP_Servo_Telem::TelemetryData::Types::STATUS
};
servo_telem->update_telem_data(ii, telem_data);
servo.newTelemetry = false;
}
}
}
#else
(void)timestamp;
#endif // HAL_LOGGING_ENABLED
#endif
}

1
libraries/AP_PiccoloCAN/AP_PiccoloCAN.h
View File

@ -131,7 +131,6 @@ private:
AP_Int16 _ecu_id; //!< ECU Node ID
AP_Int16 _ecu_hz; //!< ECU update rate (Hz)
HAL_Semaphore _telem_sem;
};
#endif // HAL_PICCOLO_CAN_ENABLE

100
libraries/AP_Scripting/applets/ahrs-set-origin.lua Normal file
View File

@ -0,0 +1,100 @@
-- Sets the AHRS/EKF origin to a specified Location
--
-- Parameter descriptions
-- AHRS_ORIG_LAT : AHRS Origin Latitude (in degrees)
-- AHRS_ORIG_LON : AHRS Origin Longitude (in degrees)
-- AHRS_ORIGIN_ALT : AHRS Origin Altitude (in meters above sea level)
--
-- How to use
-- 1. Install this script on the flight controller
-- 2. Set AHRS_ORIG_LAT, AHRS_ORIG_LON, AHRS_ORIG_ALT to the desired location
-- 3. A message should appear on the messages screen when the AHRS/EKF origin has been set and the vehicle will most often then appear on the map
local PARAM_TABLE_KEY = 87
PARAM_TABLE_PREFIX = "AHRS_ORIG_"
local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
local SEND_TEXT_PREFIX = "ahrs-set-origin: "
-- bind a parameter to a variable
function bind_param(name)
local p = Parameter()
assert(p:init(name), string.format('could not find %s parameter', name))
return p
end
-- add a parameter and bind it to a variable
local function bind_add_param(name, idx, default_value)
assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
return bind_param(PARAM_TABLE_PREFIX .. name)
end
-- add param table
assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 3), SEND_TEXT_PREFIX .. 'could not add param table')
--[[
// @Param: AHRS_ORIG_LAT
// @DisplayName: AHRS/EKF Origin Latitude
// @Description: AHRS/EKF origin will be set to this latitude if not already set
// @Range: -180 180
// @User: Standard
--]]
local AHRS_ORIG_LAT = bind_add_param('LAT', 1, 0)
--[[
// @Param: AHRS_ORIG_LON
// @DisplayName: AHRS/EKF Origin Longitude
// @Description: AHRS/EKF origin will be set to this longitude if not already set
// @Range: -180 180
// @User: Standard
--]]
local AHRS_ORIG_LON = bind_add_param('LON', 2, 0)
--[[
// @Param: AHRS_ORIG_ALT
// @DisplayName: AHRS/EKF Origin Altitude
// @Description: AHRS/EKF origin will be set to this altitude (in meters above sea level) if not already set
// @Range: 0 10000
// @User: Standard
--]]
local AHRS_ORIG_ALT = bind_add_param('ALT', 3, 0)
-- print welcome message
gcs:send_text(MAV_SEVERITY.INFO, SEND_TEXT_PREFIX .. "started")
function update()
-- wait for AHRS to be initialised
if not ahrs:initialised() then
return update, 5000
end
-- exit if AHRS/EKF origin has already been set
if ahrs:get_origin() then
gcs:send_text(MAV_SEVERITY.WARNING, SEND_TEXT_PREFIX .. "EKF origin already set")
return
end
-- return if parameters have not been set
if AHRS_ORIG_LAT:get() == 0 and AHRS_ORIG_LON:get() == 0 and AHRS_ORIG_ALT == 0 then
-- try again in 5 seconds
return update, 5000
end
-- create new origin
local location = Location()
location:lat(math.floor(AHRS_ORIG_LAT:get() * 10000000.0))
location:lng(math.floor(AHRS_ORIG_LON:get() * 10000000.0))
location:alt(math.floor(AHRS_ORIG_ALT:get() * 100.0))
-- attempt to send origin
if ahrs:set_origin(location) then
gcs:send_text(MAV_SEVERITY.INFO, string.format(SEND_TEXT_PREFIX .. "origin set Lat:%.7f Lon:%.7f Alt:%.1f", AHRS_ORIG_LAT:get(), AHRS_ORIG_LON:get(), AHRS_ORIG_ALT:get()))
else
gcs:send_text(MAV_SEVERITY.WARNING, SEND_TEXT_PREFIX .. "failed to set origin")
end
-- return and do not try again
return
end
return update()

15
libraries/AP_Scripting/applets/ahrs-set-origin.md Normal file
View File

@ -0,0 +1,15 @@
# AHRS set origin
Sets the AHRS/EKF origin to a specified Location
## Parmeter Descriptions
-- AHRS_ORIG_LAT : AHRS Origin Latitude (in degrees)
-- AHRS_ORIG_LON : AHRS Origin Longitude (in degrees)
-- AHRS_ORIGIN_ALT : AHRS Origin Altitude (in meters above sea level)
## How to use
Install this script on the flight controller
Set AHRS_ORIG_LAT, AHRS_ORIG_LON, AHRS_ORIG_ALT to the desired location
A message should appear on the messages screen when the AHRS/EKF origin has been set and the vehicle will most often then appear on the map

23
libraries/AP_Scripting/examples/ahrs-set-origin.lua
View File

@ -1,23 +0,0 @@
-- example script for using "set_origin()"" and "initialised()"
-- sets the ekf origin if not already set
function update ()
if not ahrs:initialised() then
return update, 5000
end
origin = assert(not ahrs:get_origin(),"Refused to set EKF origin - already set")
location = Location() location:lat(-353632640) location:lng(1491652352) location:alt(58409)
if ahrs:set_origin(location) then
gcs:send_text(6, string.format("Origin Set - Lat:%.7f Long:%.7f Alt:%.1f", location:lat()/10000000, location:lng()/10000000, location:alt()/100))
else
gcs:send_text(0, "Refused to set EKF origin")
end
return
end
return update()

161
libraries/AP_Servo_Telem/AP_Servo_Telem.cpp Normal file
View File

@ -0,0 +1,161 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_Servo_Telem.h"
#if AP_SERVO_TELEM_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
AP_Servo_Telem *AP_Servo_Telem::_singleton;
AP_Servo_Telem::AP_Servo_Telem()
{
if (_singleton) {
AP_HAL::panic("Too many AP_Servo_Telem instances");
}
_singleton = this;
}
// return true if the data is stale
bool AP_Servo_Telem::TelemetryData::stale(uint32_t now_ms) const volatile
{
return (last_update_ms == 0) || ((now_ms - last_update_ms) > 5000);
}
// return true if the requested types of data are available
bool AP_Servo_Telem::TelemetryData::present(const uint16_t type_mask) const volatile
{
return (valid_types & type_mask) != 0;
}
// return true if the requested types of data are available and not stale
bool AP_Servo_Telem::TelemetryData::valid(const uint16_t type_mask) const volatile
{
return present(type_mask) && !stale(AP_HAL::millis());
}
// record an update to the telemetry data together with timestamp
// callback to update the data in the frontend, should be called by the driver when new data is available
void AP_Servo_Telem::update_telem_data(const uint8_t servo_index, const TelemetryData& new_data)
{
// telemetry data are not protected by a semaphore even though updated from different threads
// each element is a primitive type and the timestamp is only updated at the end, thus a caller
// can only get slightly more up-to-date information that perhaps they were expecting or might
// read data that has just gone stale - both of these are safe and avoid the overhead of locking
if (servo_index >= ARRAY_SIZE(_telem_data) || (new_data.valid_types == 0)) {
return;
}
active_mask |= 1U << servo_index;
volatile TelemetryData &telemdata = _telem_data[servo_index];
if (new_data.present(TelemetryData::Types::COMMANDED_POSITION)) {
telemdata.command_position = new_data.command_position;
}
if (new_data.present(TelemetryData::Types::MEASURED_POSITION)) {
telemdata.measured_position = new_data.measured_position;
}
if (new_data.present(TelemetryData::Types::FORCE)) {
telemdata.force = new_data.force;
}
if (new_data.present(TelemetryData::Types::SPEED)) {
telemdata.speed = new_data.speed;
}
if (new_data.present(TelemetryData::Types::VOLTAGE)) {
telemdata.voltage = new_data.voltage;
}
if (new_data.present(TelemetryData::Types::CURRENT)) {
telemdata.current = new_data.current;
}
if (new_data.present(TelemetryData::Types::DUTY_CYCLE)) {
telemdata.duty_cycle = new_data.duty_cycle;
}
if (new_data.present(TelemetryData::Types::MOTOR_TEMP)) {
telemdata.motor_temperature_cdeg = new_data.motor_temperature_cdeg;
}
if (new_data.present(TelemetryData::Types::PCB_TEMP)) {
telemdata.pcb_temperature_cdeg = new_data.pcb_temperature_cdeg;
}
if (new_data.present(TelemetryData::Types::PCB_TEMP)) {
telemdata.status_flags = new_data.status_flags;
}
telemdata.valid_types |= new_data.valid_types;
telemdata.last_update_ms = AP_HAL::millis();
}
void AP_Servo_Telem::update()
{
#if HAL_LOGGING_ENABLED
write_log();
#endif
}
#if HAL_LOGGING_ENABLED
void AP_Servo_Telem::write_log()
{
AP_Logger *logger = AP_Logger::get_singleton();
// Check logging is available and enabled
if ((logger == nullptr) || !logger->logging_enabled() || active_mask == 0) {
return;
}
const uint64_t now_us = AP_HAL::micros64();
for (uint8_t i = 0; i < ARRAY_SIZE(_telem_data); i++) {
const volatile TelemetryData &telemdata = _telem_data[i];
if (telemdata.last_update_ms == _last_telem_log_ms[i]) {
// No new data since last log call, skip
continue;
}
// Update last log timestamp
_last_telem_log_ms[i] = telemdata.last_update_ms;
// Log, use nan for float values which are not available
const struct log_CSRV pkt {
LOG_PACKET_HEADER_INIT(LOG_CSRV_MSG),
time_us : now_us,
id : i,
position : telemdata.present(TelemetryData::Types::MEASURED_POSITION) ? telemdata.measured_position : AP::logger().quiet_nanf(),
force : telemdata.present(TelemetryData::Types::FORCE) ? telemdata.force : AP::logger().quiet_nanf(),
speed : telemdata.present(TelemetryData::Types::SPEED) ? telemdata.speed : AP::logger().quiet_nanf(),
power_pct : telemdata.duty_cycle,
pos_cmd : telemdata.present(TelemetryData::Types::COMMANDED_POSITION) ? telemdata.command_position : AP::logger().quiet_nanf(),
voltage : telemdata.present(TelemetryData::Types::VOLTAGE) ? telemdata.voltage : AP::logger().quiet_nanf(),
current : telemdata.present(TelemetryData::Types::CURRENT) ? telemdata.current : AP::logger().quiet_nanf(),
mot_temp : telemdata.present(TelemetryData::Types::MOTOR_TEMP) ? telemdata.motor_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(),
pcb_temp : telemdata.present(TelemetryData::Types::PCB_TEMP) ? telemdata.pcb_temperature_cdeg * 0.01 : AP::logger().quiet_nanf(),
error : telemdata.status_flags,
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
}
#endif // HAL_LOGGING_ENABLED
// Get the AP_Servo_Telem singleton
AP_Servo_Telem *AP_Servo_Telem::get_singleton()
{
return AP_Servo_Telem::_singleton;
}
#endif // AP_SERVO_TELEM_ENABLED

86
libraries/AP_Servo_Telem/AP_Servo_Telem.h Normal file
View File

@ -0,0 +1,86 @@
#pragma once
#include "AP_Servo_Telem_config.h"
#if AP_SERVO_TELEM_ENABLED
#include <AP_HAL/AP_HAL.h>
#include <SRV_Channel/SRV_Channel_config.h>
#ifndef SERVO_TELEM_MAX_SERVOS
#define SERVO_TELEM_MAX_SERVOS NUM_SERVO_CHANNELS
#endif
static_assert(SERVO_TELEM_MAX_SERVOS > 0, "Cannot have 0 Servo telem instances");
class AP_Servo_Telem {
public:
AP_Servo_Telem();
/* Do not allow copies */
CLASS_NO_COPY(AP_Servo_Telem);
static AP_Servo_Telem *get_singleton();
struct TelemetryData {
// Telemetry values
float command_position; // Commanded servo position in degrees
float measured_position; // Measured Servo position in degrees
float force; // Force in Newton meters
float speed; // Speed degrees per second
float voltage; // Voltage in volts
float current; // Current draw in Ampere
uint8_t duty_cycle; // duty cycle 0% to 100%
int16_t motor_temperature_cdeg; // centi-degrees C, negative values allowed
int16_t pcb_temperature_cdeg; // centi-degrees C, negative values allowed
uint8_t status_flags; // Type specific status flags
// last update time in milliseconds, determines data is stale
uint32_t last_update_ms;
// telemetry types present
enum Types {
COMMANDED_POSITION = 1 << 0,
MEASURED_POSITION = 1 << 1,
FORCE = 1 << 2,
SPEED = 1 << 3,
VOLTAGE = 1 << 4,
CURRENT = 1 << 5,
DUTY_CYCLE = 1 << 6,
MOTOR_TEMP = 1 << 7,
PCB_TEMP = 1 << 8,
STATUS = 1 << 9,
};
uint16_t valid_types;
// return true if the data is stale
bool stale(uint32_t now_ms) const volatile;
// return true if the requested types of data are available
bool present(const uint16_t type_mask) const volatile;
// return true if the requested type of data is available and not stale
bool valid(const uint16_t type_mask) const volatile;
};
// update at 10Hz to log telemetry
void update();
// record an update to the telemetry data together with timestamp
// callback to update the data in the frontend, should be called by the driver when new data is available
void update_telem_data(const uint8_t servo_index, const TelemetryData& new_data);
private:
// Log telem of each servo
void write_log();
volatile TelemetryData _telem_data[SERVO_TELEM_MAX_SERVOS];
uint32_t _last_telem_log_ms[SERVO_TELEM_MAX_SERVOS];
static AP_Servo_Telem *_singleton;
uint32_t active_mask;
};
#endif // AP_SERVO_TELEM_ENABLED

8
libraries/AP_Servo_Telem/AP_Servo_Telem_config.h Normal file
View File

@ -0,0 +1,8 @@
#pragma once
#include <AP_HAL/AP_HAL_Boards.h>
#include <SRV_Channel/SRV_Channel_config.h>
#ifndef AP_SERVO_TELEM_ENABLED
#define AP_SERVO_TELEM_ENABLED ((NUM_SERVO_CHANNELS > 0) && ((HAL_SUPPORT_RCOUT_SERIAL || HAL_MAX_CAN_PROTOCOL_DRIVERS)) && !defined(HAL_BUILD_AP_PERIPH))
#endif

47
libraries/AP_Servo_Telem/LogStructure.h Normal file
View File

@ -0,0 +1,47 @@
#pragma once
#include "AP_Servo_Telem_config.h"
#include <AP_Logger/LogStructure.h>
#define LOG_IDS_FROM_SERVO_TELEM \
LOG_CSRV_MSG
// @LoggerMessage: CSRV
// @Description: Servo feedback data
// @Field: TimeUS: Time since system startup
// @Field: Id: Servo number this data relates to
// @Field: Pos: Current servo position
// @Field: Force: Force being applied
// @Field: Speed: Current servo movement speed
// @Field: Pow: Amount of rated power being applied
// @Field: PosCmd: commanded servo position
// @Field: V: Voltage
// @Field: A: Current
// @Field: MotT: motor temperature
// @Field: PCBT: PCB temperature
// @Field: Err: error flags
struct PACKED log_CSRV {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t id;
float position;
float force;
float speed;
uint8_t power_pct;
float pos_cmd;
float voltage;
float current;
float mot_temp;
float pcb_temp;
uint8_t error;
};
#if !AP_SERVO_TELEM_ENABLED
#define LOG_STRUCTURE_FROM_SERVO_TELEM
#else
#define LOG_STRUCTURE_FROM_SERVO_TELEM \
{ LOG_CSRV_MSG, sizeof(log_CSRV), \
"CSRV","QBfffBfffffB","TimeUS,Id,Pos,Force,Speed,Pow,PosCmd,V,A,MotT,PCBT,Err", "s#dtk%dvAOO-", "F-000000000-", true },
#endif

6
libraries/AP_Vehicle/AP_Vehicle.cpp
View File

@ -3,6 +3,7 @@
#if AP_VEHICLE_ENABLED
#include "AP_Vehicle.h"
#include <AP_InertialSensor/AP_InertialSensor_rate_config.h>
#include <AP_BLHeli/AP_BLHeli.h>
#include <AP_Common/AP_FWVersion.h>
@ -621,7 +622,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50, 205),
SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50, 210),
#endif
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED && !AP_INERTIALSENSOR_FAST_SAMPLE_WINDOW_ENABLED
SCHED_TASK(update_dynamic_notch_at_specified_rate, LOOP_RATE, 200, 215),
#endif
#if AP_VIDEOTX_ENABLED
@ -634,6 +635,9 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
#if HAL_WITH_ESC_TELEM
SCHED_TASK_CLASS(AP_ESC_Telem, &vehicle.esc_telem, update, 100, 50, 230),
#endif
#if AP_SERVO_TELEM_ENABLED
SCHED_TASK_CLASS(AP_Servo_Telem, &vehicle.servo_telem, update, 50, 50, 231),
#endif
#if HAL_GENERATOR_ENABLED
SCHED_TASK_CLASS(AP_Generator, &vehicle.generator, update, 10, 50, 235),
#endif

20
libraries/AP_Vehicle/AP_Vehicle.h
View File

@ -51,6 +51,7 @@
#include <AP_OpenDroneID/AP_OpenDroneID.h>
#include <AP_Hott_Telem/AP_Hott_Telem.h>
#include <AP_ESC_Telem/AP_ESC_Telem.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>
#include <AP_GyroFFT/AP_GyroFFT.h>
#include <AP_Networking/AP_Networking.h>
#include <AP_VisualOdom/AP_VisualOdom.h>
@ -415,6 +416,10 @@ protected:
AP_ESC_Telem esc_telem;
#endif
#if AP_SERVO_TELEM_ENABLED
AP_Servo_Telem servo_telem;
#endif
#if AP_OPENDRONEID_ENABLED
AP_OpenDroneID opendroneid;
#endif
@ -517,6 +522,13 @@ protected:
// Check if this mode can be entered from the GCS
bool block_GCS_mode_change(uint8_t mode_num, const uint8_t *mode_list, uint8_t mode_list_length) const;
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
// update the harmonic notch
void update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch);
// run notch update at either loop rate or 200Hz
void update_dynamic_notch_at_specified_rate();
#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
private:
#if AP_SCHEDULER_ENABLED
@ -531,13 +543,7 @@ private:
#if AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
// update the harmonic notch for throttle based notch
void update_throttle_notch(AP_InertialSensor::HarmonicNotch &notch);
// update the harmonic notch
void update_dynamic_notch(AP_InertialSensor::HarmonicNotch &notch);
// run notch update at either loop rate or 200Hz
void update_dynamic_notch_at_specified_rate();
#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
#endif // AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED
// decimation for 1Hz update
uint8_t one_Hz_counter;

70
libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp
View File

@ -14,7 +14,7 @@
#include <SRV_Channel/SRV_Channel.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Servo_Telem/AP_Servo_Telem.h>
// Extended Position Data Format defines -100 as 0x0080 decimal 128, we map this to a PWM of 1000 (if range is default)
#define PWM_POSITION_MIN 1000
@ -77,7 +77,7 @@ void AP_Volz_Protocol::init(void)
}
}
#if HAL_LOGGING_ENABLED
#if AP_SERVO_TELEM_ENABLED
// Request telem data, cycling through each servo and telem item
void AP_Volz_Protocol::request_telem()
{
@ -144,7 +144,7 @@ void AP_Volz_Protocol::loop()
hal.scheduler->delay_microseconds(100);
}
#if HAL_LOGGING_ENABLED
#if AP_SERVO_TELEM_ENABLED
// Send a command for each servo, then one telem request
const uint8_t num_servos = __builtin_popcount(bitmask.get());
if (sent_count < num_servos) {
@ -162,7 +162,7 @@ void AP_Volz_Protocol::loop()
read_telem();
}
#else // No logging, send only
#else // No telem, send only
send_position_cmd();
#endif
}
@ -208,7 +208,7 @@ void AP_Volz_Protocol::send_position_cmd()
send_command(cmd);
#if HAL_LOGGING_ENABLED
#if AP_SERVO_TELEM_ENABLED
{
// Update the commanded angle
WITH_SEMAPHORE(telem.sem);
@ -221,7 +221,7 @@ void AP_Volz_Protocol::send_position_cmd()
}
}
#if HAL_LOGGING_ENABLED
#if AP_SERVO_TELEM_ENABLED
void AP_Volz_Protocol::process_response(const CMD &cmd)
{
// Convert to 0 indexed
@ -348,7 +348,7 @@ void AP_Volz_Protocol::read_telem()
// Used up all attempts without running out of data.
// Really should not end up here
}
#endif // HAL_LOGGING_ENABLED
#endif // AP_SERVO_TELEM_ENABLED
// Called each time the servo outputs are sent
void AP_Volz_Protocol::update()
@ -376,11 +376,11 @@ void AP_Volz_Protocol::update()
servo_pwm[i] = (pwm == 0) ? c->get_trim() : pwm;
}
#if HAL_LOGGING_ENABLED
// take semaphore and log all channels at 5 Hz
const uint32_t now_ms = AP_HAL::millis();
if ((now_ms - telem.last_log_ms) > 200) {
telem.last_log_ms = now_ms;
#if AP_SERVO_TELEM_ENABLED
// Report telem data
AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
if (servo_telem != nullptr) {
const uint32_t now_ms = AP_HAL::millis();
WITH_SEMAPHORE(telem.sem);
for (uint8_t i=0; i<ARRAY_SIZE(telem.data); i++) {
@ -389,37 +389,25 @@ void AP_Volz_Protocol::update()
continue;
}
// @LoggerMessage: VOLZ
// @Description: Volz servo data
// @Field: TimeUS: Time since system startup
// @Field: I: Instance
// @Field: Dang: desired angle
// @Field: ang: reported angle
// @Field: pc: primary supply current
// @Field: sc: secondary supply current
// @Field: pv: primary supply voltage
// @Field: sv: secondary supply voltage
// @Field: mt: motor temperature
// @Field: pt: pcb temperature
AP::logger().WriteStreaming("VOLZ",
"TimeUS,I,Dang,ang,pc,sc,pv,sv,mt,pt",
"s#ddAAvvOO",
"F000000000",
"QBffffffhh",
AP_HAL::micros64(),
i + 1, // convert to 1 indexed to match actuator IDs and SERVOx numbering
telem.data[i].desired_angle,
telem.data[i].angle,
telem.data[i].primary_current,
telem.data[i].secondary_current,
telem.data[i].primary_voltage,
telem.data[i].secondary_voltage,
telem.data[i].motor_temp_deg,
telem.data[i].pcb_temp_deg
);
const AP_Servo_Telem::TelemetryData telem_data {
.command_position = telem.data[i].desired_angle,
.measured_position = telem.data[i].angle,
.voltage = telem.data[i].primary_voltage,
.current = telem.data[i].primary_current,
.motor_temperature_cdeg = int16_t(telem.data[i].motor_temp_deg * 100),
.pcb_temperature_cdeg = int16_t(telem.data[i].pcb_temp_deg * 100),
.valid_types = AP_Servo_Telem::TelemetryData::Types::COMMANDED_POSITION |
AP_Servo_Telem::TelemetryData::Types::MEASURED_POSITION |
AP_Servo_Telem::TelemetryData::Types::VOLTAGE |
AP_Servo_Telem::TelemetryData::Types::CURRENT |
AP_Servo_Telem::TelemetryData::Types::MOTOR_TEMP |
AP_Servo_Telem::TelemetryData::Types::PCB_TEMP
};
servo_telem->update_telem_data(i, telem_data);
}
}
#endif // HAL_LOGGING_ENABLED
#endif // AP_SERVO_TELEM_ENABLED
}
// Return the crc for a given command packet

6
libraries/AP_Volz_Protocol/AP_Volz_Protocol.h
View File

@ -44,8 +44,7 @@
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#include <SRV_Channel/SRV_Channel_config.h>
#include <AP_Logger/AP_Logger_config.h>
#include <AP_Servo_Telem/AP_Servo_Telem_config.h>
class AP_Volz_Protocol {
public:
@ -110,7 +109,7 @@ private:
AP_Int16 range;
bool initialised;
#if HAL_LOGGING_ENABLED
#if AP_SERVO_TELEM_ENABLED
// Request telem data, cycling through each servo and telem item
void request_telem();
@ -143,7 +142,6 @@ private:
int16_t motor_temp_deg;
int16_t pcb_temp_deg;
} data[NUM_SERVO_CHANNELS];
uint32_t last_log_ms;
} telem;
#endif

7
libraries/SITL/SIM_I2C.cpp
View File

@ -37,6 +37,7 @@
#include "SIM_MS5525.h"
#include "SIM_MS5611.h"
#include "SIM_QMC5883L.h"
#include "SIM_INA3221.h"
#include <signal.h>
@ -85,6 +86,9 @@ static IS31FL3195 is31fl3195;
#if AP_SIM_COMPASS_QMC5883L_ENABLED
static QMC5883L qmc5883l;
#endif
#if AP_SIM_INA3221_ENABLED
static INA3221 ina3221;
#endif
struct i2c_device_at_address {
uint8_t bus;
@ -102,6 +106,9 @@ struct i2c_device_at_address {
{ 1, 0x39, ignored }, // NCP5623C
{ 1, 0x40, ignored }, // KellerLD
{ 1, 0x76, ms5525 }, // MS5525: ARSPD_TYPE = 4
#if AP_SIM_INA3221_ENABLED
{ 1, 0x42, ina3221 },
#endif
{ 1, 0x77, tsys01 },
{ 1, 0x0B, rotoye }, // Rotoye: BATTx_MONITOR 19, BATTx_I2C_ADDR 13
{ 2, 0x0B, maxell }, // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13

167
libraries/SITL/SIM_INA3221.cpp Normal file
View File

@ -0,0 +1,167 @@
#include "SIM_config.h"
#if AP_SIM_INA3221_ENABLED
#include "SIM_INA3221.h"
#include <SITL/SITL.h>
SITL::INA3221::INA3221()
{
writable_registers.set(0);
writable_registers.set(7);
writable_registers.set(8);
writable_registers.set(9);
writable_registers.set(10);
writable_registers.set(11);
writable_registers.set(12);
writable_registers.set(14);
writable_registers.set(15);
writable_registers.set(16);
writable_registers.set(254);
writable_registers.set(255);
reset();
}
void SITL::INA3221::reset()
{
// from page 24 of datasheet:
registers.byname.configuration.word = 0x7127;
registers.byname.Channel_1_Shunt_Voltage = 0x0;
registers.byname.Channel_1_Bus_Voltage = 0x0;
registers.byname.Channel_2_Shunt_Voltage = 0x0;
registers.byname.Channel_2_Bus_Voltage = 0x0;
registers.byname.Channel_3_Shunt_Voltage = 0x0;
registers.byname.Channel_3_Bus_Voltage = 0x0;
registers.byname.Channel_1_CriticalAlertLimit = 0x7FF8;
registers.byname.Channel_1_WarningAlertLimit = 0x7FF8;
registers.byname.Channel_2_CriticalAlertLimit = 0x7FF8;
registers.byname.Channel_2_WarningAlertLimit = 0x7FF8;
registers.byname.Channel_3_CriticalAlertLimit = 0x7FF8;
registers.byname.Channel_3_WarningAlertLimit = 0x7FF8;
registers.byname.Shunt_VoltageSum = 0x0;
registers.byname.Shunt_VoltageSumLimit = 0x7FFE;
registers.byname.Mask_Enable = 0x0002;
registers.byname.Power_ValidUpperLimit = 0x2710;
registers.byname.Power_ValidLowerLimit = 0x2328;
registers.byname.ManufacturerID = MANUFACTURER_ID;
registers.byname.Die_ID = DIE_ID;
}
int SITL::INA3221::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
{
if (data->nmsgs == 2) {
if (data->msgs[0].flags != 0) {
AP_HAL::panic("Unexpected flags");
}
if (data->msgs[1].flags != I2C_M_RD) {
AP_HAL::panic("Unexpected flags");
}
const uint8_t reg_addr = data->msgs[0].buf[0];
const uint16_t register_value = registers.word[reg_addr];
data->msgs[1].buf[0] = register_value >> 8;
data->msgs[1].buf[1] = register_value & 0xFF;
data->msgs[1].len = 2;
return 0;
}
if (data->nmsgs == 1) {
if (data->msgs[0].flags != 0) {
AP_HAL::panic("Unexpected flags");
}
const uint8_t reg_addr = data->msgs[0].buf[0];
if (!writable_registers.get(reg_addr)) {
AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
}
const uint16_t register_value = data->msgs[0].buf[2] << 8 | data->msgs[0].buf[1];
registers.word[reg_addr] = register_value;
return 0;
}
return -1;
};
static uint16_t convert_voltage(float voltage_v) {
// 8mV per count, register value is x8 (3 lowest bits not used)
float volt_counts = (voltage_v/8e-3)*8;
if (volt_counts < INT16_MIN) {
volt_counts = INT16_MIN;
} else if (volt_counts > INT16_MAX) {
volt_counts = INT16_MAX;
}
// register value is signed
return (uint16_t)((int16_t)volt_counts);
}
static uint16_t convert_current(float current_a) {
// V = IR
const float shunt_resistance_ohms = 0.001; // default value in driver
const float shunt_voltage = current_a*shunt_resistance_ohms;
// 40uV per count, register value is x8 (3 lowest bits not used)
float shunt_counts = (shunt_voltage/40e-6)*8;
if (shunt_counts < INT16_MIN) {
shunt_counts = INT16_MIN;
} else if (shunt_counts > INT16_MAX) {
shunt_counts = INT16_MAX;
}
// register value is signed
return (uint16_t)((int16_t)shunt_counts);
}
void SITL::INA3221::update(const class Aircraft &aircraft)
{
if (registers.byname.configuration.bits.reset != 0) {
reset();
}
// update readings
if (registers.byname.configuration.bits.mode == 0b000 ||
registers.byname.configuration.bits.mode == 0b100) {
// power-off
return;
}
const bool update_shunt = registers.byname.configuration.bits.mode & 0b001;
const bool update_bus = registers.byname.configuration.bits.mode & 0b010;
if ((registers.byname.configuration.bits.mode & 0b100) == 0) {
// single-shot only
registers.byname.configuration.bits.mode &= ~0b011;
}
// channel 2 gets the first simulated battery's voltage and current, others are test values:
if (registers.byname.configuration.bits.ch1_enable) {
// values close to chip limits (assuming 1mOhm current shunt)
if (update_bus) {
registers.byname.Channel_1_Bus_Voltage = convert_voltage(25); // max of 26V
}
if (update_shunt) {
registers.byname.Channel_1_Shunt_Voltage = convert_current(160); // max of 163.8A
}
}
if (registers.byname.configuration.bits.ch2_enable) {
if (update_bus) {
registers.byname.Channel_2_Bus_Voltage = convert_voltage(AP::sitl()->state.battery_voltage);
}
if (update_shunt) {
registers.byname.Channel_2_Shunt_Voltage = convert_current(AP::sitl()->state.battery_current);
}
}
if (registers.byname.configuration.bits.ch3_enable) {
// values different from above to test channel switching
if (update_bus) {
registers.byname.Channel_3_Bus_Voltage = convert_voltage(3.14159);
}
if (update_shunt) {
registers.byname.Channel_3_Shunt_Voltage = convert_current(2.71828);
}
}
}
#endif // AP_SIM_INA3221_ENABLED

89
libraries/SITL/SIM_INA3221.h Normal file
View File

@ -0,0 +1,89 @@
#include "SIM_config.h"
#if AP_SIM_INA3221_ENABLED
#include "SIM_I2CDevice.h"
#include <AP_Common/Bitmask.h>
/*
Testing:
param set BATT_MONITOR 30
reboot
param set BATT_I2C_ADDR 0x42
param set BATT_I2C_BUS 1
param set BATT_CHANNEL 2
reboot
*/
namespace SITL {
class INA3221 : public I2CDevice
{
public:
INA3221();
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
void update(const class Aircraft &aircraft) override;
private:
static const uint16_t MANUFACTURER_ID = 0b0101010001001001; // from datasheet p25
static const uint16_t DIE_ID = 0b0011001000100000; // from datasheet p25
// All 16-bit INA3221 registers are two 8-bit bytes via the I2C
// interface. Table 4 shows a register map for the INA3221.
union Registers {
uint16_t word[256];
struct PACKED ByName {
union {
uint16_t word;
struct PACKED {
uint16_t mode : 3;
uint16_t shunt_voltage_conversiontime : 3;
uint16_t bus_voltage_conversiontime : 3;
uint16_t averaging_mode : 3;
uint16_t ch1_enable : 1;
uint16_t ch2_enable : 1;
uint16_t ch3_enable : 1;
uint16_t reset : 1;
} bits;
} configuration;
uint16_t Channel_1_Shunt_Voltage;
uint16_t Channel_1_Bus_Voltage;
uint16_t Channel_2_Shunt_Voltage;
uint16_t Channel_2_Bus_Voltage;
uint16_t Channel_3_Shunt_Voltage;
uint16_t Channel_3_Bus_Voltage;
uint16_t Channel_1_CriticalAlertLimit;
uint16_t Channel_1_WarningAlertLimit;
uint16_t Channel_2_CriticalAlertLimit;
uint16_t Channel_2_WarningAlertLimit;
uint16_t Channel_3_CriticalAlertLimit;
uint16_t Channel_3_WarningAlertLimit;
uint16_t Shunt_VoltageSum;
uint16_t Shunt_VoltageSumLimit;
uint16_t Mask_Enable;
uint16_t Power_ValidUpperLimit;
uint16_t Power_ValidLowerLimit;
uint16_t unused[236];
uint16_t ManufacturerID;
uint16_t Die_ID;
} byname;
} registers;
// 256 2-byte registers:
assert_storage_size<Registers::ByName, 512> assert_storage_size_registers_reg;
Bitmask<256> writable_registers;
void reset();
};
} // namespace SITL
#endif // AP_SIM_INA3221_ENABLED

3
libraries/SITL/SIM_config.h
View File

@ -144,3 +144,6 @@
#define AP_SIM_GIMBAL_ENABLED (AP_SIM_SOLOGIMBAL_ENABLED)
#endif
#ifndef AP_SIM_INA3221_ENABLED
#define AP_SIM_INA3221_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
#endif