Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

failsafe requires RC connection loss AND computer connection loss #1

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

failsafe requires RC connection loss AND computer connection loss #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3b02da9
Configured for Twin Fixed Wing
robpneu Jan 13, 2018
9532233
updated commits of breezystm32 and mavlink submodules to newest version
robpneu Jan 25, 2018
9e73f8b
Airspeed variance threshold increased and default params set
austinhurst May 26, 2018
315574b
Merged 'patch to fix parameter streaming'
robpneu May 26, 2018
4014874
Merged 'When calibrating RC trims, add to existing trim values'
robpneu May 26, 2018
8e42d05
failsafe requires RC connection loss AND computer connection loss
austinhurst Jun 10, 2018
8719d94
Increase min PWM value for failsafe to 910
robpneu Jun 10, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion boards/naze/lib/breezystm32
Open in desktop
Submodule breezystm32 updated 8 files
+2 −0 .gitignore
+4 −1 breezystm32.h
+4 −3 drv_mb1242.c
+11 −37 drv_ms4525.c
+0 −2 drv_ms4525.h
+13 −30 drv_ms5611.c
+1 −1 examples/i2csniff/Makefile
+2 −2 main.c
2 changes: 1 addition & 1 deletion comms/mavlink/v1.0
Open in desktop
Submodule v1.0 updated 6 files
+1 −1 common/version.h
+62 −53 message_definitions/rosflight.xml
+234 −0 rosflight/mavlink_msg_rosflight_aux_cmd.h
+16 −3 rosflight/rosflight.h
+45 −0 rosflight/testsuite.h
+1 −1 rosflight/version.h
1 change: 1 addition & 0 deletions include/comm_manager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ class CommManager
void update_status();
void log(CommLink::LogSeverity severity, const char *fmt, ...);

void send_parameter_list();
void send_named_value_float(const char *const name, float value);
};

Expand Down
4 changes: 2 additions & 2 deletions include/mixer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -192,8 +192,8 @@ class Mixer
{
{S, S, M, S, S, M, NONE, NONE},

{ 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // F Mix
{ 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // X Mix
{ 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f}, // F Mix
{ 1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f}, // X Mix
{ 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}, // Y Mix
{ 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f} // Z Mix
};
Expand Down
5 changes: 5 additions & 0 deletions src/comm_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,6 +158,11 @@ void CommManager::param_request_list_callback(uint8_t target_system)
send_params_index_ = 0;
}

void CommManager::send_parameter_list()
{
send_params_index_ = 0;
}

void CommManager::param_request_read_callback(uint8_t target_system, const char* const param_name, int16_t param_index)
{
if (target_system == sysid_)
Expand Down
4 changes: 2 additions & 2 deletions src/command_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -267,11 +267,11 @@ bool CommandManager::run()
bool last_rc_override = rc_override_;

// Check for and apply failsafe command
if (RF_.state_manager_.state().failsafe)
if (RF_.state_manager_.state().failsafe && !offboard_control_active())
{
combined_command_ = failsafe_command_;
}
else if (RF_.rc_.new_command())
else if (RF_.rc_.new_command() || new_command_)
{
// Read RC
interpret_rc();
Expand Down
6 changes: 3 additions & 3 deletions src/controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,9 +155,9 @@ void Controller::calculate_equilbrium_torque_from_rc()
turbomath::Vector pid_output = run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false);

// the output from the controller is going to be the static offsets
RF_.params_.set_param_float(PARAM_X_EQ_TORQUE, pid_output.x);
RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE, pid_output.y);
RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE, pid_output.z);
RF_.params_.set_param_float(PARAM_X_EQ_TORQUE, pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE));
RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE, pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE));
RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE, pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE));

RF_.comm_manager_.log(CommLink::LogSeverity::LOG_WARNING, "Equilibrium torques found and applied.");
RF_.comm_manager_.log(CommLink::LogSeverity::LOG_WARNING, "Please zero out trims on your transmitter");
Expand Down
59 changes: 29 additions & 30 deletions src/param.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,20 +108,20 @@ void Params::set_defaults(void)
init_param_int(PARAM_STREAM_HEARTBEAT_RATE, "STRM_HRTBT", 1); // Rate of heartbeat streaming (Hz) | 0 | 1000
init_param_int(PARAM_STREAM_STATUS_RATE, "STRM_STATUS", 10); // Rate of status streaming (Hz) | 0 | 1000

init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 200); // Rate of attitude stream (Hz) | 0 | 1000
init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 500); // Rate of IMU stream (Hz) | 0 | 1000
init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 50); // Rate of magnetometer stream (Hz) | 0 | 75
init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 50); // Rate of barometer stream (Hz) | 0 | 100
init_param_int(PARAM_STREAM_ATTITUDE_RATE, "STRM_ATTITUDE", 1); // Rate of attitude stream (Hz) | 0 | 1000
init_param_int(PARAM_STREAM_IMU_RATE, "STRM_IMU", 0); // Rate of IMU stream (Hz) | 0 | 1000
init_param_int(PARAM_STREAM_MAG_RATE, "STRM_MAG", 0); // Rate of magnetometer stream (Hz) | 0 | 75
init_param_int(PARAM_STREAM_BARO_RATE, "STRM_BARO", 0); // Rate of barometer stream (Hz) | 0 | 100
init_param_int(PARAM_STREAM_AIRSPEED_RATE, "STRM_AIRSPEED", 20); // Rate of airspeed stream (Hz) | 0 | 50
init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 40); // Rate of sonar stream (Hz) | 0 | 40
init_param_int(PARAM_STREAM_SONAR_RATE, "STRM_SONAR", 0); // Rate of sonar stream (Hz) | 0 | 40

init_param_int(PARAM_STREAM_OUTPUT_RAW_RATE, "STRM_SERVO", 50); // Rate of raw output stream | 0 | 490
init_param_int(PARAM_STREAM_RC_RAW_RATE, "STRM_RC", 50); // Rate of raw RC input stream | 0 | 50

/********************************/
/*** CONTROLLER CONFIGURATION ***/
/********************************/
init_param_float(PARAM_MAX_COMMAND, "PARAM_MAX_CMD", 1.0); // saturation point for PID controller output | 0 | 1.0
init_param_float(PARAM_MAX_COMMAND, "PARAM_MAX_CMD", 1.0); // saturation point for PID controller output | 0.0 | 1.0

init_param_float(PARAM_PID_ROLL_RATE_P, "PID_ROLL_RATE_P", 0.070f); // Roll Rate Proportional Gain | 0.0 | 1000.0
init_param_float(PARAM_PID_ROLL_RATE_I, "PID_ROLL_RATE_I", 0.000f); // Roll Rate Integral Gain | 0.0 | 1000.0
Expand All @@ -137,44 +137,43 @@ void Params::set_defaults(void)

init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f); // Roll Angle Proporitional Gain | 0.0 | 1000.0
init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f); // Roll Angle Integral Gain | 0.0 | 1000.0
init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f); // Roll Angle Derivative Gain | 0.0 | 1000.0

init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.07f); // Roll Angle Derivative Gain | 0.0 | 1000.0
init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f); // Pitch Angle Proporitional Gain | 0.0 | 1000.0
init_param_float(PARAM_PID_PITCH_ANGLE_I, "PID_PITCH_ANG_I", 0.0f); // Pitch Angle Integral Gain | 0.0 | 1000.0
init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.05f); // Pitch Angle Derivative Gain | 0.0 | 1000.0
init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.07f); // Pitch Angle Derivative Gain | 0.0 | 1000.0

init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0
init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0
init_param_float(PARAM_Z_EQ_TORQUE, "Z_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0
init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", -0.082f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0
init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", -0.056f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0
init_param_float(PARAM_Z_EQ_TORQUE, "Z_EQ_TORQUE", -0.1f); // Equilibrium torque added to output of controller on z axis | -1.0 | 1.0

init_param_float(PARAM_PID_TAU, "PID_TAU", 0.05f); // Dirty Derivative time constant - See controller documentation | 0.0 | 1.0


/*************************/
/*** PWM CONFIGURATION ***/
/*************************/
init_param_int(PARAM_MOTOR_PWM_SEND_RATE, "MOTOR_PWM_UPDATE", 490); // Refresh rate of motor commands to motors - See motor documentation | 0 | 1000
init_param_float(PARAM_MOTOR_IDLE_THROTTLE, "MOTOR_IDLE_THR", 0.1); // min throttle command sent to motors when armed (Set above 0.1 to spin when armed) | 0.0 | 1.0
init_param_float(PARAM_FAILSAFE_THROTTLE, "FAILSAFE_THR", 0.3); // Throttle sent to motors in failsafe condition (set just below hover throttle) | 0.0 | 1.0
init_param_int(PARAM_MOTOR_PWM_SEND_RATE, "MOTOR_PWM_UPDATE", 50); // Refresh rate of motor commands to motors - See motor documentation | 0 | 1000
init_param_float(PARAM_MOTOR_IDLE_THROTTLE, "MOTOR_IDLE_THR", 0.0); // min throttle command sent to motors when armed (Set above 0.1 to spin when armed) | 0.0 | 1.0
init_param_float(PARAM_FAILSAFE_THROTTLE, "FAILSAFE_THR", 0.0); // Throttle sent to motors in failsafe condition (set just below hover throttle) | 0.0 | 1.0
init_param_int(PARAM_MOTOR_MIN_PWM, "MOTOR_MIN_PWM", 1000); // PWM value sent to motor ESCs at zero throttle | 1000 | 2000
init_param_int(PARAM_MOTOR_MAX_PWM, "MOTOR_MAX_PWM", 2000); // PWM value sent to motor ESCs at full throttle | 1000 | 2000
init_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED, "ARM_SPIN_MOTORS", true); // Enforce MOTOR_IDLE_THR | 0 | 1
init_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED, "ARM_SPIN_MOTORS", false); // Enforce MOTOR_IDLE_THR | 0 | 1

/*******************************/
/*** ESTIMATOR CONFIGURATION ***/
/*******************************/
init_param_int(PARAM_INIT_TIME, "FILTER_INIT_T", 3000); // Time in ms to initialize estimator | 0 | 100000
init_param_float(PARAM_FILTER_KP, "FILTER_KP", 0.5f); // estimator proportional gain - See estimator documentation | 0 | 10.0
init_param_float(PARAM_FILTER_KI, "FILTER_KI", 0.05f); // estimator integral gain - See estimator documentation | 0 | 1.0
init_param_float(PARAM_FILTER_KP, "FILTER_KP", 1.0f); // estimator proportional gain - See estimator documentation | 0 | 10.0
init_param_float(PARAM_FILTER_KI, "FILTER_KI", 0.1f); // estimator integral gain - See estimator documentation | 0 | 1.0

init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", 1); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation loop on F1 processors) | 0 | 1
init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", 1); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 processors) 0 - use euler integration | 0 | 1
init_param_int(PARAM_FILTER_USE_QUAD_INT, "FILTER_QUAD_INT", 0); // Perform a quadratic averaging of LPF gyro data prior to integration (adds ~20 us to estimation loop on F1 processors) | 0 | 1
init_param_int(PARAM_FILTER_USE_MAT_EXP, "FILTER_MAT_EXP", 0); // 1 - Use matrix exponential to improve gyro integration (adds ~90 us to estimation loop in F1 processors) 0 - use euler integration | 0 | 1
init_param_int(PARAM_FILTER_USE_ACC, "FILTER_USE_ACC", 1); // Use accelerometer to correct gyro integration drift (adds ~70 us to estimation loop) | 0 | 1

init_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, "CAL_GYRO_ARM", false); // True if desired to calibrate gyros on arm | 0 | 1

init_param_float(PARAM_GYRO_ALPHA, "GYRO_LPF_ALPHA", 0.3f); // Low-pass filter constant - See estimator documentation | 0 | 1.0
init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", 0.5f); // Low-pass filter constant - See estimator documentation | 0 | 1.0
init_param_float(PARAM_GYRO_ALPHA, "GYRO_LPF_ALPHA", 0.88f); // Low-pass filter constant - See estimator documentation | 0 | 1.0
init_param_float(PARAM_ACC_ALPHA, "ACC_LPF_ALPHA", 0.88f); // Low-pass filter constant - See estimator documentation | 0 | 1.0

init_param_float(PARAM_GYRO_X_BIAS, "GYRO_X_BIAS", 0.0f); // Constant x-bias of gyroscope readings | -1.0 | 1.0
init_param_float(PARAM_GYRO_Y_BIAS, "GYRO_Y_BIAS", 0.0f); // Constant y-bias of gyroscope readings | -1.0 | 1.0
Expand Down Expand Up @@ -215,8 +214,8 @@ void Params::set_defaults(void)
init_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, "RC_ATT_OVRD_CHN", 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7
init_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, "RC_THR_OVRD_CHN", 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7
init_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL, "RC_ATT_CTRL_CHN", -1); // RC switch channel mapped to attitude control type [0 indexed, -1 to disable] | 4 | 7
init_param_int(PARAM_RC_ARM_CHANNEL, "ARM_CHANNEL", -1); // RC switch channel mapped to arming (only if PARAM_ARM_STICKS is false) [0 indexed, -1 to disable] | 4 | 7
init_param_int(PARAM_RC_NUM_CHANNELS, "RC_NUM_CHN", 6); // number of RC input channels | 1 | 8
init_param_int(PARAM_RC_ARM_CHANNEL, "ARM_CHANNEL", 5); // RC switch channel mapped to arming (only if PARAM_ARM_STICKS is false) [0 indexed, -1 to disable] | 4 | 7
init_param_int(PARAM_RC_NUM_CHANNELS, "RC_NUM_CHN", 8); // number of RC input channels | 1 | 8

init_param_int(PARAM_RC_SWITCH_5_DIRECTION, "SWITCH_5_DIR", 1); // RC switch 5 toggle direction | -1 | 1
init_param_int(PARAM_RC_SWITCH_6_DIRECTION, "SWITCH_6_DIR", 1); // RC switch 6 toggle direction | -1 | 1
Expand All @@ -225,7 +224,7 @@ void Params::set_defaults(void)

init_param_float(PARAM_RC_OVERRIDE_DEVIATION, "RC_OVRD_DEV", 0.1); // RC stick deviation from center for overrride | 0.0 | 1.0
init_param_int(PARAM_OVERRIDE_LAG_TIME, "OVRD_LAG_TIME", 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000
init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", true); // Take minimum throttle between RC and computer at all times | 0 | 1
init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", false); // Take minimum throttle between RC and computer at all times | 0 | 1

init_param_int(PARAM_RC_ATTITUDE_MODE, "RC_ATT_MODE", 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1
init_param_float(PARAM_RC_MAX_ROLL, "RC_MAX_ROLL", 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159
Expand All @@ -237,11 +236,11 @@ void Params::set_defaults(void)
/***************************/
/*** FRAME CONFIGURATION ***/
/***************************/
init_param_int(PARAM_MIXER, "MIXER", Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10
init_param_int(PARAM_MIXER, "MIXER", Mixer::FIXEDWING); // Which mixer to choose - See Mixer documentation | 0 | 10

init_param_int(PARAM_FIXED_WING, "FIXED_WING", false); // switches on passthrough commands for fixedwing operation | 0 | 1
init_param_int(PARAM_ELEVATOR_REVERSE, "ELEVATOR_REV", 0); // reverses elevator servo output | 0 | 1
init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", 0); // reverses aileron servo output | 0 | 1
init_param_int(PARAM_FIXED_WING, "FIXED_WING", true); // switches on passthrough commands for fixedwing operation | 0 | 1
init_param_int(PARAM_ELEVATOR_REVERSE, "ELEVATOR_REV", true); // reverses elevator servo output | 0 | 1
init_param_int(PARAM_AILERON_REVERSE, "AIL_REV", true); // reverses aileron servo output | 0 | 1
init_param_int(PARAM_RUDDER_REVERSE, "RUDDER_REV", 0); // reverses rudder servo output | 0 | 1

/********************/
Expand Down Expand Up @@ -337,7 +336,7 @@ bool Params::set_param_float(uint16_t id, float value)
{
params.values[id].fvalue = value;
change_callback(id);
RF_.comm_manager_.send_param_value(id);
RF_.comm_manager_.send_parameter_list();
return true;
}
return false;
Expand Down
2 changes: 1 addition & 1 deletion src/rc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,7 +174,7 @@ bool RC::check_rc_lost()
// go into failsafe if we get an invalid RC command for any channel
for (int8_t i = 0; i<RF_.params_.get_param_int(PARAM_RC_NUM_CHANNELS); i++)
{
if (RF_.board_.pwm_read(i) < 900 || RF_.board_.pwm_read(i) > 2100)
if (RF_.board_.pwm_read(i) < 910 || RF_.board_.pwm_read(i) > 2100)
{
failsafe = true;
}
Expand Down
2 changes: 1 addition & 1 deletion src/sensors.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ const int Sensors::SENSOR_CAL_DELAY_CYCLES = 128;
const int Sensors::SENSOR_CAL_CYCLES = 127;

const float Sensors::BARO_MAX_CALIBRATION_VARIANCE = 25.0; // standard dev about 0.2 m
const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 100.0; // standard dev about 3 m/s
const float Sensors::DIFF_PRESSURE_MAX_CALIBRATION_VARIANCE = 1000.0; // standard dev about 3 m/s when = to 100.0

Sensors::Sensors(ROSflight& rosflight) :
rf_(rosflight)
Expand Down