Skip to content

Commit

Permalink
Merge pull request #12 from Flybrix/radio-config-adjustment
Browse files Browse the repository at this point in the history 
Radio config adjustment
  • Loading branch information
@rjwalters
rjwalters committed Mar 25, 2016
2 parents b43d945 + 3931b4a commit e37b55b
Show file tree
Hide file tree
Showing 9 changed files with 100 additions and 45 deletions.
23 changes: 15 additions & 8 deletions command.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,14 @@ void PilotCommand::loadRxData() {
yaw.update(RX[CONFIG.data.assignedChannel[3]]);
AUX1.update(RX[CONFIG.data.assignedChannel[4]]);
AUX2.update(RX[CONFIG.data.assignedChannel[5]]);

// update midpoints form config
throttle.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[0]];
pitch.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[1]];
roll.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[2]];
yaw.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[3]];
AUX1.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[4]];
AUX2.mid = CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[5]];
}

sei(); // enable interrupts
Expand Down Expand Up @@ -109,21 +117,20 @@ void PilotCommand::processCommands(void) {
// users can modify the controller's output using the trim settings in a way that defeats this throttle "off" range!
// this gives us a range of ~1080... we're scaling this received ppm range to 4095 because that's our full scale motor resolution
// commandInversion is separate from the scaling factor in the controller because we would apply a negative scale factor twice in a cascade

uint16_t throttle_threshold = ((throttle.max - throttle.min) / 10) + throttle.min;
*throttle_command = constrain((throttle.val - throttle_threshold) * 4095 / (throttle.max - throttle_threshold), 0, 4095);
*pitch_command = constrain((1-2*((CONFIG.data.commandInversion >> 0) & 1))*(pitch.val - pitch.mid) * 4095 / (pitch.max - pitch.min), -2047, 2047);
*roll_command = constrain((1-2*((CONFIG.data.commandInversion >> 1) & 1))*( roll.val - roll.mid ) * 4095 / (roll.max - roll.min), -2047, 2047);
*yaw_command = constrain((1-2*((CONFIG.data.commandInversion >> 2) & 1))*( yaw.val - yaw.mid ) * 4095 / (yaw.max - yaw.min), -2047, 2047);
*pitch_command = constrain((1-2*((CONFIG.data.commandInversion >> 0) & 1))*(pitch.val - CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[1]]) * 4095 / (pitch.max - pitch.min), -2047, 2047);
*roll_command = constrain((1-2*((CONFIG.data.commandInversion >> 1) & 1))*( roll.val - CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[2]]) * 4095 / (roll.max - roll.min), -2047, 2047);
*yaw_command = constrain((1-2*((CONFIG.data.commandInversion >> 2) & 1))*( yaw.val - CONFIG.data.channelMidpoint[CONFIG.data.assignedChannel[3]]) * 4095 / (yaw.max - yaw.min), -2047, 2047);

//
// in some cases it is impossible to get a ppm channel to be exactly 1500 usec because the controller trim is too coarse to correct a small error
// we can get around by creating a small dead zone on the commands that are potentially effected

int16_t dead_zone_half_width = 30;
*pitch_command = *pitch_command > 0 ? max(0, *pitch_command - dead_zone_half_width) : min(*pitch_command + dead_zone_half_width, 0);
*roll_command = *roll_command > 0 ? max(0, *roll_command - dead_zone_half_width) : min(*roll_command + dead_zone_half_width, 0);
*yaw_command = *yaw_command > 0 ? max(0, *yaw_command - dead_zone_half_width) : min(*yaw_command + dead_zone_half_width, 0);
*pitch_command = *pitch_command > 0 ? max(0, *pitch_command - (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[1]]) : min(*pitch_command + (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[1]], 0);
*roll_command = *roll_command > 0 ? max(0, *roll_command - (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[2]]) : min(*roll_command + (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[2]], 0);
*yaw_command = *yaw_command > 0 ? max(0, *yaw_command - (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[3]]) : min(*yaw_command + (2047.0f/400.0f)*CONFIG.data.channelDeadzone[CONFIG.data.assignedChannel[3]], 0);

}

Expand Down
3 changes: 2 additions & 1 deletion command.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,8 @@ class PPMchannel {
PPMchannel(){};

uint16_t val = 1500;
static const uint16_t mid = 1500;

uint16_t mid = 1500;
static const uint16_t min = 1100;
static const uint16_t max = 1900;

Expand Down
57 changes: 40 additions & 17 deletions config.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,15 @@ void(*config_handler)(CONFIG_struct&) {nullptr};

CONFIG_union CONFIG; // global CONFIG variable

#define BYPASS_THRUST_MASTER 1<<0
#define BYPASS_PITCH_MASTER 1<<1
#define BYPASS_ROLL_MASTER 1<<2
#define BYPASS_YAW_MASTER 1<<3
#define BYPASS_THRUST_SLAVE 1<<4
#define BYPASS_PITCH_SLAVE 1<<5
#define BYPASS_ROLL_SLAVE 1<<6
#define BYPASS_YAW_SLAVE 1<<7

void initializeEEPROM(void) { // Default Settings

CONFIG.data.version[0] = FIRMWARE_VERSION_A;
Expand Down Expand Up @@ -57,6 +66,20 @@ void initializeEEPROM(void) { // Default Settings
CONFIG.data.assignedChannel[5] = 5; // map AUX2 to LHS click

CONFIG.data.commandInversion = 3; //invert pitch and roll

CONFIG.data.channelMidpoint[0] = 1515;
CONFIG.data.channelMidpoint[1] = 1515;
CONFIG.data.channelMidpoint[2] = 1500;
CONFIG.data.channelMidpoint[3] = 1520;
CONFIG.data.channelMidpoint[4] = 1500;
CONFIG.data.channelMidpoint[5] = 1500;

CONFIG.data.channelDeadzone[0] = 20;
CONFIG.data.channelDeadzone[1] = 20;
CONFIG.data.channelDeadzone[2] = 20;
CONFIG.data.channelDeadzone[3] = 40;
CONFIG.data.channelDeadzone[4] = 20;
CONFIG.data.channelDeadzone[5] = 20;

// PID Parameters

Expand All @@ -68,24 +91,24 @@ void initializeEEPROM(void) { // Default Settings
CONFIG.data.thrustMasterPIDParameters[5] = 0.005f; // setpoint filter usec (30Hz)
CONFIG.data.thrustMasterPIDParameters[6] = 1.0f; // (meters / full stick action)

CONFIG.data.pitchMasterPIDParameters[0] = 12.0f; // P
CONFIG.data.pitchMasterPIDParameters[0] = 5.0f; // P
CONFIG.data.pitchMasterPIDParameters[1] = 1.0f; // I
CONFIG.data.pitchMasterPIDParameters[2] = 0.2f; // D
CONFIG.data.pitchMasterPIDParameters[2] = 0.0f; // D
CONFIG.data.pitchMasterPIDParameters[3] = 10.0f; // Windup guard
CONFIG.data.pitchMasterPIDParameters[4] = 0.005f; // D filter usec (15Hz)
CONFIG.data.pitchMasterPIDParameters[5] = 0.005f; // setpoint filter usec (30Hz)
CONFIG.data.pitchMasterPIDParameters[6] = 10.0f; // (degrees / full stick action)

CONFIG.data.rollMasterPIDParameters[0] = 12.0f; // P
CONFIG.data.rollMasterPIDParameters[0] = 5.0f; // P
CONFIG.data.rollMasterPIDParameters[1] = 1.0f; // I
CONFIG.data.rollMasterPIDParameters[2] = 0.2f; // D
CONFIG.data.rollMasterPIDParameters[2] = 0.0f; // D
CONFIG.data.rollMasterPIDParameters[3] = 10.0f; // Windup guard
CONFIG.data.rollMasterPIDParameters[4] = 0.005f; // D filter usec (15Hz)
CONFIG.data.rollMasterPIDParameters[5] = 0.005f; // setpoint filter usec (30Hz)
CONFIG.data.rollMasterPIDParameters[6] = 10.0f; // (degrees / full stick action)

CONFIG.data.yawMasterPIDParameters[0] = 1.0f; // P
CONFIG.data.yawMasterPIDParameters[1] = 0.0f; // I
CONFIG.data.yawMasterPIDParameters[0] = 5.0f; // P
CONFIG.data.yawMasterPIDParameters[1] = 1.0f; // I
CONFIG.data.yawMasterPIDParameters[2] = 0.0f; // D
CONFIG.data.yawMasterPIDParameters[3] = 10.0f; // Windup guard
CONFIG.data.yawMasterPIDParameters[4] = 0.005f; // D filter usec (15Hz)
Expand All @@ -100,31 +123,31 @@ void initializeEEPROM(void) { // Default Settings
CONFIG.data.thrustSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.thrustSlavePIDParameters[6] = 0.3f; // (meters/sec / full stick action)

CONFIG.data.pitchSlavePIDParameters[0] = 40.0f; // P
CONFIG.data.pitchSlavePIDParameters[1] = 5.0f; // I
CONFIG.data.pitchSlavePIDParameters[2] = 4.0f; // D
CONFIG.data.pitchSlavePIDParameters[0] = 20.0f; // P
CONFIG.data.pitchSlavePIDParameters[1] = 8.0f; // I
CONFIG.data.pitchSlavePIDParameters[2] = 0.0f; // D
CONFIG.data.pitchSlavePIDParameters[3] = 30.0f; // Windup guard
CONFIG.data.pitchSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz)
CONFIG.data.pitchSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.pitchSlavePIDParameters[6] = 30.0f; // (deg/sec / full stick action)

CONFIG.data.rollSlavePIDParameters[0] = 40.0f; // P
CONFIG.data.rollSlavePIDParameters[1] = 5.0f; // I
CONFIG.data.rollSlavePIDParameters[2] = 4.0f; // D
CONFIG.data.rollSlavePIDParameters[0] = 20.0f; // P
CONFIG.data.rollSlavePIDParameters[1] = 8.0f; // I
CONFIG.data.rollSlavePIDParameters[2] = 0.0f; // D
CONFIG.data.rollSlavePIDParameters[3] = 30.0f; // Windup guard
CONFIG.data.rollSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz)
CONFIG.data.rollSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.rollSlavePIDParameters[6] = 30.0f; // (deg/sec / full stick action)

CONFIG.data.yawSlavePIDParameters[0] = 20.0f; // P
CONFIG.data.yawSlavePIDParameters[1] = 1.0f; // I
CONFIG.data.yawSlavePIDParameters[0] = 30.0f; // P
CONFIG.data.yawSlavePIDParameters[1] = 5.0f; // I
CONFIG.data.yawSlavePIDParameters[2] = 0.0f; // D
CONFIG.data.yawSlavePIDParameters[3] = 20.0f; // Windup guard
CONFIG.data.yawSlavePIDParameters[4] = 0.001f; // D filter usec (150Hz)
CONFIG.data.yawSlavePIDParameters[5] = 0.001f; // setpoint filter usec (300Hz)
CONFIG.data.yawSlavePIDParameters[6] = 120.0f; // (deg/sec / full stick action)

CONFIG.data.pidBypass = 25; //AHRS/Horizon mode
CONFIG.data.yawSlavePIDParameters[6] = 240.0f; // (deg/sec / full stick action)
CONFIG.data.pidBypass = BYPASS_THRUST_MASTER | BYPASS_THRUST_SLAVE | BYPASS_YAW_MASTER; //AHRS/Horizon mode

CONFIG.data.stateEstimationParameters[0] = 1.00f; // 2*kp or BETA
CONFIG.data.stateEstimationParameters[1] = 0.01f; // 2*ki
Expand Down
2 changes: 2 additions & 0 deletions config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,8 @@ struct __attribute__((packed)) CONFIG_struct {
// RX channel mapping
uint8_t assignedChannel[6];
uint8_t commandInversion; //bitfield order is {pitch_command, roll_command, yaw_command, x, x, x, x, x} (LSB-->MSB)
uint16_t channelMidpoint[6]; // ideally 1500usec
uint16_t channelDeadzone[6]; // usec units

// tunable control parameters
float thrustMasterPIDParameters[7]; //parameters are {P,I,D,integral windup guard, D filter delay sec, setpoint filter delay sec, command scaling factor}
Expand Down
20 changes: 10 additions & 10 deletions control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,17 +53,17 @@ void Control::parseConfig(CONFIG_struct& config) {
void Control::calculateControlVectors() {
thrust_pid.setMasterInput(state->kinematicsAltitude);
thrust_pid.setSlaveInput(0.0f); //state->kinematicsClimbRate
pitch_pid.setMasterInput(state->kinematicsAngle[0] * 57.2957795);
pitch_pid.setSlaveInput(state->kinematicsRate[0] * 57.2957795);
roll_pid.setMasterInput(state->kinematicsAngle[1] * 57.2957795);
roll_pid.setSlaveInput(state->kinematicsRate[1] * 57.2957795);
yaw_pid.setMasterInput(state->kinematicsAngle[2] * 57.2957795);
yaw_pid.setSlaveInput(state->kinematicsRate[2] * 57.2957795);
pitch_pid.setMasterInput(state->kinematicsAngle[0] * 57.2957795f);
pitch_pid.setSlaveInput(state->kinematicsRate[0] * 57.2957795f);
roll_pid.setMasterInput(state->kinematicsAngle[1] * 57.2957795f);
roll_pid.setSlaveInput(state->kinematicsRate[1] * 57.2957795f);
yaw_pid.setMasterInput(state->kinematicsAngle[2] * 57.2957795f);
yaw_pid.setSlaveInput(state->kinematicsRate[2] * 57.2957795f);

thrust_pid.setSetpoint(state->command_throttle * (1.0f/4095.0f) * thrust_pid.getScalingFactor(pidEnabled[THRUST_MASTER], pidEnabled[THRUST_SLAVE], 4096.0));
pitch_pid.setSetpoint(state->command_pitch * (1.0f/2047.0f) * pitch_pid.getScalingFactor(pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE], 2048.0));
roll_pid.setSetpoint(state->command_roll * (1.0f/2047.0f) * roll_pid.getScalingFactor(pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE], 2048.0));
yaw_pid.setSetpoint(state->command_yaw * (1.0f/2047.0f) * yaw_pid.getScalingFactor(pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE], 2048.0));
thrust_pid.setSetpoint(state->command_throttle * (1.0f/4095.0f) * thrust_pid.getScalingFactor(pidEnabled[THRUST_MASTER], pidEnabled[THRUST_SLAVE], 4095.0f));
pitch_pid.setSetpoint(state->command_pitch * (1.0f/2047.0f) * pitch_pid.getScalingFactor(pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE], 2047.0f));
roll_pid.setSetpoint(state->command_roll * (1.0f/2047.0f) * roll_pid.getScalingFactor(pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE], 2047.0f));
yaw_pid.setSetpoint(state->command_yaw * (1.0f/2047.0f) * yaw_pid.getScalingFactor(pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE], 2047.0f));

// compute new output levels for state
uint32_t now = micros();
Expand Down
25 changes: 25 additions & 0 deletions flybrix-firmware.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,7 @@ uint32_t mag_reads = 0;
uint32_t bmp_reads = 0;
uint32_t pwr_reads = 0;

uint32_t low_battery_counter = 0;

#define DEF_PROCESS_VARIABLES(F) uint32_t iterations_at_##F##Hz = 0;

Expand Down Expand Up @@ -268,6 +269,30 @@ bool ProcessTask<40>() {
sys.pwr.measureRawLevels(); // read all ADCs
pwr_reads++;

// check for low voltage condition
if ( ((1/50)/0.003*1.2/65536 * sys.state.I1_raw ) > 1.0f ){ //if total battery current > 1A
if ( ((20.5+226)/20.5*1.2/65536 * sys.state.V0_raw) < 2.8f ) {
low_battery_counter++;
if ( low_battery_counter > 40 ){
sys.state.set(STATUS_BATTERY_LOW);
}
}
else {
low_battery_counter = 0;
}
}
else {
if ( ((20.5+226)/20.5*1.2/65536 * sys.state.V0_raw) < 3.63f ) {
low_battery_counter++;
if ( low_battery_counter > 40 ){
sys.state.set(STATUS_BATTERY_LOW);
}
}
else {
low_battery_counter = 0;
}
}

return true;
}

Expand Down
12 changes: 4 additions & 8 deletions led.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,18 +100,14 @@ void LED::changeLights() {
use(LED::FLASH, 100, 110, 250); // yellow
} else if (state->is(STATUS_OVERRIDE)) {
use(LED::BEACON, 255, 0, 0); // red
} else if (state->is(STATUS_TEMP_WARNING)) {
use(LED::FLASH, 100, 150, 250); // yellow
} else if (state->is(STATUS_BATTERY_LOW)) {
use(LED::BEACON, 255, 180, 20);
} else if (state->is(STATUS_ENABLING)) {
use(LED::FLASH, 0, 0, 255); // blue
} else if (state->is(STATUS_ENABLED)) {
use(LED::BEACON, 0, 0, 255); // blue for enable
}

else if (state->is(STATUS_TEMP_WARNING)) {
use(LED::FLASH, 100, 150, 250); // yellow
} else if (state->is(STATUS_LOG_FULL)) {
use(LED::FLASH, 0, 0, 250);
} else if (state->is(STATUS_BATTERY_LOW)) {
use(LED::BEACON, 255, 180, 20);
} else if (state->is(STATUS_IDLE)) {
indicatorRedOff(); // clear boot test
use(LED::BEACON, 0, 255, 0); // breathe instead?
Expand Down
1 change: 1 addition & 0 deletions state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ void State::processMotorEnablingIteration(void) {
}

void State::disableMotors(void) {
clear(STATUS_BATTERY_LOW);
clear(STATUS_ENABLED);
clear(STATUS_FAIL_STABILITY);
clear(STATUS_FAIL_ANGLE);
Expand Down
2 changes: 1 addition & 1 deletion version.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
#define version_h

#define FIRMWARE_VERSION_A 1
#define FIRMWARE_VERSION_B 1
#define FIRMWARE_VERSION_B 2
#define FIRMWARE_VERSION_C 0

#endif

0 comments on commit e37b55b

Please sign in to comment.