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

Merge branch 'master' into lua_utc_time

This commit is contained in:
Andras Schaffer 2024-09-26 14:33:19 +02:00 committed by GitHub
parent 94d1cb3dcf beb4226740
commit 7767dc093e
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
17 changed files with 206 additions and 99 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Attitude.cpp
View File

@ -15,7 +15,9 @@ void Copter::run_rate_controller()
pos_control->set_dt(last_loop_time_s);
// run low level rate controllers that only require IMU data
attitude_control->rate_controller_run();
attitude_control->rate_controller_run();
// reset sysid and other temporary inputs
attitude_control->rate_controller_target_reset();
}
/*************************************************************

2
ArduPlane/quadplane.cpp
View File

@ -1968,6 +1968,8 @@ void QuadPlane::motors_output(bool run_rate_controller)
attitude_control->set_dt(last_loop_time_s);
pos_control->set_dt(last_loop_time_s);
attitude_control->rate_controller_run();
// reset sysid and other temporary inputs
attitude_control->rate_controller_target_reset();
last_att_control_ms = now;
}

17
Tools/autotest/arduplane.py
View File

@ -6206,6 +6206,22 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
'''ensure we don't die with a bad Roll channel defined'''
self.set_parameter("RCMAP_ROLL", 17)
def MAV_CMD_NAV_LOITER_TO_ALT(self):
'''test loiter to alt mission item'''
self.upload_simple_relhome_mission([
(mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
(mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, 0, 0, 500),
(mavutil.mavlink.MAV_CMD_NAV_LOITER_TO_ALT, 0, 0, 100),
(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 800, 0, 0),
])
self.change_mode('AUTO')
self.wait_ready_to_arm()
self.arm_vehicle()
self.wait_altitude(450, 475, relative=True, timeout=600)
self.wait_altitude(75, 125, relative=True, timeout=600)
self.wait_current_waypoint(4)
self.fly_home_land_and_disarm()
def tests(self):
'''return list of all tests'''
ret = super(AutoTestPlane, self).tests()
@ -6258,6 +6274,7 @@ class AutoTestPlane(vehicle_test_suite.TestSuite):
self.AdvancedFailsafe,
self.LOITER,
self.MAV_CMD_NAV_LOITER_TURNS,
self.MAV_CMD_NAV_LOITER_TO_ALT,
self.DeepStall,
self.WatchdogHome,
self.LargeMissions,

88
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -9,10 +9,12 @@ extern const AP_HAL::HAL& hal;
// default gains for Plane
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 5.0 // Min lean angle so that vehicle can maintain limited control
#define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 0
#else
// default gains for Copter and Sub
# define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0 // Min lean angle so that vehicle can maintain limited control
#define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 1
#endif
AC_AttitudeControl *AC_AttitudeControl::_singleton;
@ -185,18 +187,45 @@ float AC_AttitudeControl::get_slew_yaw_max_degs() const
return MIN(_ang_vel_yaw_max, _slew_yaw * 0.01);
}
// get the latest gyro for the purposes of attitude control
// Counter-inuitively the lowest latency for rate control is achieved by running rate control
// *before* attitude control. This is because you want rate control to run as close as possible
// to the time that a gyro sample was read to minimise jitter and control errors. Running rate
// control after attitude control might makes sense logically, but the overhead of attitude
// control calculations (and other updates) compromises the actual rate control.
//
// In the case of running rate control in a separate thread, the ordering between rate and attitude
// updates is less important, except that gyro sample used should be the latest
//
// Currently quadplane runs rate control after attitude control, necessitating the following code
// to minimise latency.
// However this code can be removed once quadplane updates it's structure to run the rate loops before
// the Attitude controller.
const Vector3f AC_AttitudeControl::get_latest_gyro() const
{
#if AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL
// rate updates happen before attitude updates so the last gyro value is the last rate gyro value
// this also allows a separate rate thread to be the source of gyro data
return _rate_gyro;
#else
// rate updates happen after attitude updates so the AHRS must be consulted for the last gyro value
return _ahrs.get_gyro_latest();
#endif
}
// Ensure attitude controller have zero errors to relax rate controller output
void AC_AttitudeControl::relax_attitude_controllers()
{
// take a copy of the last gyro used by the rate controller before using it
Vector3f gyro = get_latest_gyro();
// Initialize the attitude variables to the current attitude
_ahrs.get_quat_body_to_ned(_attitude_target);
_attitude_target.to_euler(_euler_angle_target);
_attitude_ang_error.initialise();
// Initialize the angular rate variables to the current rate
_ang_vel_target = _ahrs.get_gyro();
_ang_vel_target = gyro;
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
_ang_vel_body = _ahrs.get_gyro();
// Initialize remaining variables
_thrust_error_angle = 0.0f;
@ -208,6 +237,8 @@ void AC_AttitudeControl::relax_attitude_controllers()
// Reset the I terms
reset_rate_controller_I_terms();
// finally update the attitude target
_ang_vel_body = gyro;
}
void AC_AttitudeControl::reset_rate_controller_I_terms()
@ -471,6 +502,7 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c
attitude_controller_run_quat();
}
// Fully stabilized acro
// Command an angular velocity with angular velocity feedforward and smoothing
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
@ -511,6 +543,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl
attitude_controller_run_quat();
}
// Rate-only acro with no attitude feedback - used only by Copter rate-only acro
// Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
@ -531,9 +564,12 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds,
_attitude_target.to_euler(_euler_angle_target);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target);
// finally update the attitude target
_ang_vel_body = _ang_vel_target;
}
// Acro with attitude feedback that does not rely on attitude - used only by Plane acro
// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds)
{
@ -554,7 +590,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
_attitude_ang_error.from_axis_angle(attitude_error);
}
Vector3f gyro_latest = _ahrs.get_gyro_latest();
Vector3f gyro_latest = get_latest_gyro();
attitude_ang_error_update_quat.from_axis_angle((_ang_vel_target - gyro_latest) * _dt);
_attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error;
_attitude_ang_error.normalize();
@ -582,8 +618,11 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds,
// Compute the angular velocity target from the integrated rate error
_attitude_ang_error.to_axis_angle(attitude_error);
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
_ang_vel_body += _ang_vel_target;
Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
ang_vel_body += _ang_vel_target;
// finally update the attitude target
_ang_vel_body = ang_vel_body;
}
// Command an angular step (i.e change) in body frame angle
@ -612,6 +651,25 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste
attitude_controller_run_quat();
}
// Command an rate step (i.e change) in body frame rate
// Used to command a step in rate without exciting the orthogonal axis during autotune
// Done as a single thread-safe function to avoid intermediate zero values being seen by the attitude controller
void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd)
{
// Update the unused targets attitude based on current attitude to condition mode change
_ahrs.get_quat_body_to_ned(_attitude_target);
_attitude_target.to_euler(_euler_angle_target);
// Set the target angular velocity to be zero to minimize target overshoot after the rate step finishes
_ang_vel_target.zero();
// Convert body-frame angular velocity into euler angle derivative of desired attitude
_euler_rate_target.zero();
Vector3f ang_vel_body {roll_rate_step_bf_cd, pitch_rate_step_bf_cd, yaw_rate_step_bf_cd};
ang_vel_body = ang_vel_body * 0.01f * DEG_TO_RAD;
// finally update the attitude target
_ang_vel_body = ang_vel_body;
}
// Command a thrust vector and heading rate
void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw)
{
@ -788,33 +846,35 @@ void AC_AttitudeControl::attitude_controller_run_quat()
thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle);
// Compute the angular velocity corrections in the body frame from the attitude error
_ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error);
// ensure angular velocity does not go over configured limits
ang_vel_limit(_ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
ang_vel_limit(ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// rotation from the target frame to the body frame
Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target;
// target angle velocity vector in the body frame
Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target;
Vector3f gyro = get_latest_gyro();
// Correct the thrust vector and smoothly add feedforward and yaw input
_feedforward_scalar = 1.0f;
if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
_ang_vel_body.z = _ahrs.get_gyro().z;
ang_vel_body.z = gyro.z;
} else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
_feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE);
_ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar;
_ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar;
_ang_vel_body.z += ang_vel_body_feedforward.z;
_ang_vel_body.z = _ahrs.get_gyro().z * (1.0 - _feedforward_scalar) + _ang_vel_body.z * _feedforward_scalar;
ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar;
ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar;
ang_vel_body.z += ang_vel_body_feedforward.z;
ang_vel_body.z = gyro.z * (1.0 - _feedforward_scalar) + ang_vel_body.z * _feedforward_scalar;
} else {
_ang_vel_body += ang_vel_body_feedforward;
ang_vel_body += ang_vel_body_feedforward;
}
// Record error to handle EKF resets
_attitude_ang_error = attitude_body.inverse() * _attitude_target;
// finally update the attitude target
_ang_vel_body = ang_vel_body;
}
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.

56
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -11,6 +11,7 @@
#include <AC_PID/AC_PID.h>
#include <AC_PID/AC_P.h>
#include <AP_Vehicle/AP_MultiCopter.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw
@ -151,6 +152,7 @@ public:
// set the rate control input smoothing time constant
void set_input_tc(float input_tc) { _input_tc.set(constrain_float(input_tc, 0.0f, 1.0f)); }
// rate loop visible functions
// Ensure attitude controller have zero errors to relax rate controller output
void relax_attitude_controllers();
@ -177,6 +179,24 @@ public:
// handle reset of attitude from EKF since the last iteration
void inertial_frame_reset();
// Command euler yaw rate and pitch angle with roll angle specified in body frame
// (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes)
virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd,
float euler_pitch_angle_cd, float euler_yaw_rate_cds) {}
////// begin rate update functions //////
// These functions all update _ang_vel_body which is used as the rate target by the rate controller.
// Since _ang_vel_body can be seen by the rate controller thread all these functions only set it
// at the end once all of the calculations have been performed. This avoids intermediate results being
// used by the rate controller when running concurrently. _ang_vel_body is accessed so commonly that
// locking proves to be moderately expensive, however since this is changing incrementally values combining
// previous and current elements are safe and do not have an impact on control.
// Any additional functions that are added to manipulate _ang_vel_body should follow this pattern.
// Calculates the body frame angular velocities to follow the target attitude
// This is used by most of the subsequent functions
void attitude_controller_run_quat();
// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity
virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body);
@ -186,37 +206,47 @@ public:
// Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
virtual void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw);
// Command euler yaw rate and pitch angle with roll angle specified in body frame
// (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes)
virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd,
float euler_pitch_angle_cd, float euler_yaw_rate_cds) {}
// Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing
virtual void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds);
// Fully stabilized acro
// Command an angular velocity with angular velocity feedforward and smoothing
virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
// Command an angular velocity with angular velocity feedforward and smoothing
// Rate-only acro with no attitude feedback - used only by Copter rate-only acro
// Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization
virtual void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
// Acro with attitude feedback that does not rely on attitude - used only by Plane acro
// Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization
virtual void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds);
// Command an angular step (i.e change) in body frame angle
virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd);
// Command an angular rate step (i.e change) in body frame rate
virtual void input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd);
// Command a thrust vector in the earth frame and a heading angle and/or rate
virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true);
virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds);
void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);}
////// end rate update functions //////
// Converts thrust vector and heading angle to quaternion rotation in the earth frame
Quaternion attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const;
// Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run() = 0;
// reset target loop rate modifications
virtual void rate_controller_target_reset() {}
// optional variant to allow running with different dt
virtual void rate_controller_run_dt(const Vector3f& gyro, float dt) { AP_BoardConfig::config_error("rate_controller_run_dt() must be defined"); };
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void euler_rate_to_ang_vel(const Quaternion& att, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads);
@ -244,12 +274,6 @@ public:
// Return the angle between the target thrust vector and the current thrust vector.
float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); }
// Set x-axis angular velocity in centidegrees/s
void rate_bf_roll_target(float rate_cds) { _ang_vel_body.x = radians(rate_cds * 0.01f); }
// Set y-axis angular velocity in centidegrees/s
void rate_bf_pitch_target(float rate_cds) { _ang_vel_body.y = radians(rate_cds * 0.01f); }
// Set z-axis angular velocity in centidegrees/s
void rate_bf_yaw_target(float rate_cds) { _ang_vel_body.z = radians(rate_cds * 0.01f); }
@ -349,9 +373,6 @@ public:
// Calculates the body frame angular velocities to follow the target attitude
void update_attitude_target();
// Calculates the body frame angular velocities to follow the target attitude
void attitude_controller_run_quat();
// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
void thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const;
@ -454,6 +475,9 @@ protected:
// Return the yaw slew rate limit in radians/s
float get_slew_yaw_max_rads() const { return radians(get_slew_yaw_max_degs()); }
// get the latest gyro for the purposes of attitude control
const Vector3f get_latest_gyro() const;
// Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
AP_Float _slew_yaw;
@ -520,7 +544,7 @@ protected:
Vector3f _ang_vel_target;
// This represents the angular velocity in radians per second in the body frame, used in the angular
// velocity controller.
// velocity controller and most importantly the rate controller.
Vector3f _ang_vel_body;
// This is the angular velocity in radians per second in the body frame, added to the output angular

16
libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp
View File

@ -30,20 +30,20 @@ void AC_AttitudeControl::Write_ANG() const
// Write a rate packet
void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const
{
const Vector3f &rate_targets = rate_bf_targets();
const Vector3f rate_targets = rate_bf_targets() * RAD_TO_DEG;
const Vector3f &accel_target = pos_control.get_accel_target_cmss();
const Vector3f &gyro_rate = _rate_gyro;
const Vector3f gyro_rate = _rate_gyro * RAD_TO_DEG;
const struct log_Rate pkt_rate{
LOG_PACKET_HEADER_INIT(LOG_RATE_MSG),
time_us : _rate_gyro_time_us,
control_roll : degrees(rate_targets.x),
roll : degrees(gyro_rate.x),
control_roll : rate_targets.x,
roll : gyro_rate.x,
roll_out : _motors.get_roll()+_motors.get_roll_ff(),
control_pitch : degrees(rate_targets.y),
pitch : degrees(gyro_rate.y),
control_pitch : rate_targets.y,
pitch : gyro_rate.y,
pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(),
control_yaw : degrees(rate_targets.z),
yaw : degrees(gyro_rate.z),
control_yaw : rate_targets.z,
yaw : gyro_rate.z,
yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(),
control_accel : (float)accel_target.z,
accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f),

34
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -439,37 +439,51 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
_throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX);
}
void AC_AttitudeControl_Multi::rate_controller_run()
void AC_AttitudeControl_Multi::rate_controller_run_dt(const Vector3f& gyro, float dt)
{
// take a copy of the target so that it can't be changed from under us.
Vector3f ang_vel_body = _ang_vel_body;
// boost angle_p/pd each cycle on high throttle slew
update_throttle_gain_boost();
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
_ang_vel_body += _sysid_ang_vel_body;
ang_vel_body += _sysid_ang_vel_body;
_rate_gyro = _ahrs.get_gyro_latest();
_rate_gyro = gyro;
_rate_gyro_time_us = AP_HAL::micros64();
_motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll(get_rate_roll_pid().update_all(ang_vel_body.x, gyro.x, dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x);
_motors.set_roll_ff(get_rate_roll_pid().get_ff());
_motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
_motors.set_pitch(get_rate_pitch_pid().update_all(ang_vel_body.y, gyro.y, dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y);
_motors.set_pitch_ff(get_rate_pitch_pid().get_ff());
_motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
_motors.set_yaw(get_rate_yaw_pid().update_all(ang_vel_body.z, gyro.z, dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z);
_motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar);
_sysid_ang_vel_body.zero();
_actuator_sysid.zero();
_pd_scale_used = _pd_scale;
_pd_scale = VECTORF_111;
control_monitor_update();
}
// reset the rate controller target loop updates
void AC_AttitudeControl_Multi::rate_controller_target_reset()
{
_sysid_ang_vel_body.zero();
_actuator_sysid.zero();
_pd_scale = VECTORF_111;
}
// run the rate controller using the configured _dt and latest gyro
void AC_AttitudeControl_Multi::rate_controller_run()
{
Vector3f gyro_latest = _ahrs.get_gyro_latest();
rate_controller_run_dt(gyro_latest, _dt);
}
// sanity check parameters. should be called once before takeoff
void AC_AttitudeControl_Multi::parameter_sanity_check()
{

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -76,6 +76,8 @@ public:
bool is_throttle_mix_min() const override { return (_throttle_rpy_mix < 1.25f * _thr_mix_min); }
// run lowest level body-frame rate controller and send outputs to the motors
void rate_controller_run_dt(const Vector3f& gyro, float dt) override;
void rate_controller_target_reset() override;
void rate_controller_run() override;
// sanity check parameters. should be called once before take-off

7
libraries/AC_AutoTune/AC_AutoTune_Multi.cpp
View File

@ -1254,23 +1254,22 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign
attitude_control->input_rate_bf_roll_pitch_yaw(0.0, 0.0, 0.0);
}
} else {
attitude_control->input_rate_bf_roll_pitch_yaw_2(0.0, 0.0, 0.0);
// Testing rate P and D gains so will set body-frame rate targets.
// Rate controller will use existing body-frame rates and convert to motor outputs
// for all axes except the one we override here.
switch (test_axis) {
case AxisType::ROLL:
// override body-frame roll rate
attitude_control->rate_bf_roll_target(dir_sign * target_rate + start_rate);
attitude_control->input_rate_step_bf_roll_pitch_yaw(dir_sign * target_rate + start_rate, 0.0f, 0.0f);
break;
case AxisType::PITCH:
// override body-frame pitch rate
attitude_control->rate_bf_pitch_target(dir_sign * target_rate + start_rate);
attitude_control->input_rate_step_bf_roll_pitch_yaw(0.0f, dir_sign * target_rate + start_rate, 0.0f);
break;
case AxisType::YAW:
case AxisType::YAW_D:
// override body-frame yaw rate
attitude_control->rate_bf_yaw_target(dir_sign * target_rate + start_rate);
attitude_control->input_rate_step_bf_roll_pitch_yaw(0.0f, 0.0f, dir_sign * target_rate + start_rate);
break;
}
}

22
libraries/AP_Camera/AP_Camera_Backend.cpp
View File

@ -217,7 +217,6 @@ void AP_Camera_Backend::send_camera_information(mavlink_channel_t chan) const
const uint8_t model_name[32] {};
const char cam_definition_uri[140] {};
const uint32_t cap_flags = CAMERA_CAP_FLAGS_CAPTURE_IMAGE;
const float NaN = nanf("0x4152");
// send CAMERA_INFORMATION message
mavlink_msg_camera_information_send(
@ -226,9 +225,9 @@ void AP_Camera_Backend::send_camera_information(mavlink_channel_t chan) const
vendor_name, // vendor_name uint8_t[32]
model_name, // model_name uint8_t[32]
0, // firmware version uint32_t
NaN, // focal_length float (mm)
NaN, // sensor_size_h float (mm)
NaN, // sensor_size_v float (mm)
NaNf, // focal_length float (mm)
NaNf, // sensor_size_h float (mm)
NaNf, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
0, // lens_id, uint8_t
@ -241,15 +240,13 @@ void AP_Camera_Backend::send_camera_information(mavlink_channel_t chan) const
// send camera settings message to GCS
void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
{
const float NaN = nanf("0x4152");
// send CAMERA_SETTINGS message
mavlink_msg_camera_settings_send(
chan,
AP_HAL::millis(), // time_boot_ms
CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
NaN, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
@ -268,7 +265,6 @@ void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
return;
}
// send camera fov status message only if the last calculated values aren't stale
const float NaN = nanf("0x4152");
const float quat_array[4] = {
quat.q1,
quat.q2,
@ -285,8 +281,8 @@ void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
poi_loc.lng,
poi_loc.alt * 10,
quat_array,
horizontal_fov() > 0 ? horizontal_fov() : NaN,
vertical_fov() > 0 ? vertical_fov() : NaN
horizontal_fov() > 0 ? horizontal_fov() : NaNf,
vertical_fov() > 0 ? vertical_fov() : NaNf
);
}
#endif
@ -294,8 +290,6 @@ void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
// send camera capture status message to GCS
void AP_Camera_Backend::send_camera_capture_status(mavlink_channel_t chan) const
{
const float NaN = nanf("0x4152");
// Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress)
const uint8_t image_status = (time_interval_settings.num_remaining > 0) ? 2 : 0;
@ -307,7 +301,7 @@ void AP_Camera_Backend::send_camera_capture_status(mavlink_channel_t chan) const
0, // current status of video capturing (0: idle, 1: capture in progress)
static_cast<float>(time_interval_settings.time_interval_ms) / 1000.0, // image capture interval (s)
0, // elapsed time since recording started (ms)
NaN, // available storage capacity (ms)
NaNf, // available storage capacity (ms)
image_index); // total number of images captured
}

2
libraries/AP_Logger/AP_Logger.h
View File

@ -372,7 +372,7 @@ public:
void handle_log_send();
bool in_log_download() const;
float quiet_nanf() const { return nanf("0x4152"); } // "AR"
float quiet_nanf() const { return NaNf; } // "AR"
double quiet_nan() const { return nan("0x4152445550490a"); } // "ARDUPI"
// returns true if msg_type is associated with a message

2
libraries/AP_Math/AP_Math.h
View File

@ -20,6 +20,8 @@
#include "location.h"
#include "control.h"
static const float NaNf = nanf("0x4152");
#if HAL_WITH_EKF_DOUBLE
typedef Vector2<double> Vector2F;
typedef Vector3<double> Vector3F;

20
libraries/AP_Mount/AP_Mount_Siyi.cpp
View File

@ -1092,8 +1092,8 @@ void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
model_name, // model_name uint8_t[32]
fw_version, // firmware version uint32_t
focal_length_mm, // focal_length float (mm)
0, // sensor_size_h float (mm)
0, // sensor_size_v float (mm)
NaNf, // sensor_size_h float (mm)
NaNf, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
0, // lens_id uint8_t
@ -1107,7 +1107,6 @@ void AP_Mount_Siyi::send_camera_information(mavlink_channel_t chan) const
void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const
{
const uint8_t mode_id = (_config_info.record_status == RecordingStatus::ON) ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE;
const float NaN = nanf("0x4152");
const float zoom_mult_max = get_zoom_mult_max();
float zoom_pct = 0.0;
if (is_positive(zoom_mult_max)) {
@ -1120,14 +1119,13 @@ void AP_Mount_Siyi::send_camera_settings(mavlink_channel_t chan) const
AP_HAL::millis(), // time_boot_ms
mode_id, // camera mode (0:image, 1:video, 2:image survey)
zoom_pct, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
// send camera thermal range message to GCS
void AP_Mount_Siyi::send_camera_thermal_range(mavlink_channel_t chan) const
{
const float NaN = nanf("0x4152");
const uint32_t now_ms = AP_HAL::millis();
bool timeout = now_ms - _thermal.last_update_ms > AP_MOUNT_SIYI_THERM_TIMEOUT_MS;
@ -1137,12 +1135,12 @@ void AP_Mount_Siyi::send_camera_thermal_range(mavlink_channel_t chan) const
now_ms, // time_boot_ms
_instance + 1, // video stream id (assume one-to-one mapping with camera id)
_instance + 1, // camera device id
timeout ? NaN : _thermal.max_C, // max in degC
timeout ? NaN : _thermal.max_pos.x, // max x position
timeout ? NaN : _thermal.max_pos.y, // max y position
timeout ? NaN : _thermal.min_C, // min in degC
timeout ? NaN : _thermal.min_pos.x, // min x position
timeout ? NaN : _thermal.min_pos.y);// min y position
timeout ? NaNf : _thermal.max_C, // max in degC
timeout ? NaNf : _thermal.max_pos.x, // max x position
timeout ? NaNf : _thermal.max_pos.y, // max y position
timeout ? NaNf : _thermal.min_C, // min in degC
timeout ? NaNf : _thermal.min_pos.x, // min x position
timeout ? NaNf : _thermal.min_pos.y);// min y position
}
#endif

10
libraries/AP_Mount/AP_Mount_Topotek.cpp
View File

@ -547,8 +547,8 @@ void AP_Mount_Topotek::send_camera_information(mavlink_channel_t chan) const
model_name, // model_name uint8_t[32]
_firmware_ver, // firmware version uint32_t
0, // focal_length float (mm)
0, // sensor_size_h float (mm)
0, // sensor_size_v float (mm)
NaNf, // sensor_size_h float (mm)
NaNf, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
0, // lens_id uint8_t
@ -566,15 +566,13 @@ void AP_Mount_Topotek::send_camera_settings(mavlink_channel_t chan) const
return;
}
const float NaN = nanf("0x4152");
// send CAMERA_SETTINGS message
mavlink_msg_camera_settings_send(
chan,
AP_HAL::millis(), // time_boot_ms
_recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
NaN, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
NaNf, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
// get rangefinder distance. Returns true on success

8
libraries/AP_Mount/AP_Mount_Viewpro.cpp
View File

@ -920,8 +920,8 @@ void AP_Mount_Viewpro::send_camera_information(mavlink_channel_t chan) const
vendor_name, // vendor_name uint8_t[32]
_model_name, // model_name uint8_t[32]
_firmware_version, // firmware version uint32_t
0, // focal_length float (mm)
0, // sensor_size_h float (mm)
NaNf, // sensor_size_h float (mm)
NaNf, // sensor_size_v float (mm)
0, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
@ -940,8 +940,6 @@ void AP_Mount_Viewpro::send_camera_settings(mavlink_channel_t chan) const
return;
}
const float NaN = nanf("0x4152");
// convert zoom times (e.g. 1x ~ 20x) to target zoom level (e.g. 0 to 100)
const float zoom_level = linear_interpolate(0, 100, _zoom_times, 1, AP_MOUNT_VIEWPRO_ZOOM_MAX);
@ -951,7 +949,7 @@ void AP_Mount_Viewpro::send_camera_settings(mavlink_channel_t chan) const
AP_HAL::millis(), // time_boot_ms
_recording ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
zoom_level, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
// get rangefinder distance. Returns true on success

11
libraries/AP_Mount/AP_Mount_Xacti.cpp
View File

@ -361,7 +361,6 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const
static const uint8_t vendor_name[32] = "Xacti";
static uint8_t model_name[32] = "CX-GB100";
const char cam_definition_uri[140] {};
const float NaN = nanf("0x4152");
// capability flags
const uint32_t flags = CAMERA_CAP_FLAGS_CAPTURE_VIDEO |
@ -375,9 +374,9 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const
vendor_name, // vendor_name uint8_t[32]
model_name, // model_name uint8_t[32]
_firmware_version.received ? _firmware_version.mav_ver : 0, // firmware version uint32_t
NaN, // focal_length float (mm)
NaN, // sensor_size_h float (mm)
NaN, // sensor_size_v float (mm)
NaNf, // focal_length float (mm)
NaNf, // sensor_size_h float (mm)
NaNf, // sensor_size_v float (mm)
0, // resolution_h uint16_t (pix)
0, // resolution_v uint16_t (pix)
0, // lens_id uint8_t
@ -390,15 +389,13 @@ void AP_Mount_Xacti::send_camera_information(mavlink_channel_t chan) const
// send camera settings message to GCS
void AP_Mount_Xacti::send_camera_settings(mavlink_channel_t chan) const
{
const float NaN = nanf("0x4152");
// send CAMERA_SETTINGS message
mavlink_msg_camera_settings_send(
chan,
AP_HAL::millis(), // time_boot_ms
_recording_video ? CAMERA_MODE_VIDEO : CAMERA_MODE_IMAGE, // camera mode (0:image, 1:video, 2:image survey)
0, // zoomLevel float, percentage from 0 to 100, NaN if unknown
NaN); // focusLevel float, percentage from 0 to 100, NaN if unknown
NaNf); // focusLevel float, percentage from 0 to 100, NaN if unknown
}
// get attitude as a quaternion. returns true on success

4
libraries/AP_Param/AP_Param.cpp
View File

@ -2945,14 +2945,14 @@ void AP_Param::show_all(AP_HAL::BetterStream *port, bool showKeyValues)
ParamToken token;
AP_Param *ap;
enum ap_var_type type;
float default_value = nanf("0x4152"); // from logger quiet_nanf
float default_value = NaNf; // from logger quiet_nanf
for (ap=AP_Param::first(&token, &type, &default_value);
ap;
ap=AP_Param::next_scalar(&token, &type, &default_value)) {
if (showKeyValues) {
::printf("Key %u: Index %u: GroupElement %u : Default %f :", (unsigned)var_info(token.key).key, (unsigned)token.idx, (unsigned)token.group_element, default_value);
default_value = nanf("0x4152");
default_value = NaNf;
}
show(ap, token, type, port);
hal.scheduler->delay(1);