Skip to content

Commit

Permalink
Merge remote-tracking branch 'refs/remotes/origin/v1.1.0'
Browse files Browse the repository at this point in the history
  • Loading branch information
@adnanademovic
adnanademovic committed Mar 9, 2016
2 parents affcbab + 715583f commit e3705a2
Show file tree
Hide file tree
Showing 11 changed files with 167 additions and 119 deletions.
9 changes: 9 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,13 @@ Dependencies:
* teensyduino libraries from https://www.pjrc.com/teensy/teensyduino.html
* teensy loader from https://www.pjrc.com/teensy/loader.html


Tips:

If you can't compile because it appears that you're missing the ADC or i2c_t3 libraries,
be sure that you have set up the Arduino IDE to target the correct board ("Teensy 3.2 / 3.1")!

The Arduino IDE defaults to expand tabs with 2 spaces. To change that edit your preferences file.
https://www.arduino.cc/en/Hacking/Preferences -- Change “editor.tabs.size=” to 4 and restart arduino

If you run into problems, send us a note!
25 changes: 10 additions & 15 deletions command.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,21 +93,6 @@ void PilotCommand::processCommands(void) {
}
}

// look for trim adjustment
if (throttle.isExtraLow()) {
static const int16_t trimGain = 3; //consider exposing in CONFIG
state->Tx_trim = trimGain * (1-2*((CONFIG.data.commandInversion >> 0) & 1)) * ((int16_t)pitch.untrimmed_mid - (int16_t)pitch.val);
state->Ty_trim = trimGain * (1-2*((CONFIG.data.commandInversion >> 1) & 1)) * ((int16_t)roll.untrimmed_mid - (int16_t)roll.val);
state->Tz_trim = trimGain * (1-2*((CONFIG.data.commandInversion >> 2) & 1)) * ((int16_t)yaw.untrimmed_mid - (int16_t)yaw.val);
pitch.mid = pitch.val;
roll.mid = roll.val;
yaw.mid = yaw.val;
//copy the midpoint values to state for logging purposes
state->PPMchannel_pitch_mid = pitch.mid;
state->PPMchannel_roll_mid = roll.mid;
state->PPMchannel_yaw_mid = yaw.mid;
}

if (recentlyEnabled || throttle.isLow()) {
*throttle_command = 0;
*pitch_command = 0;
Expand All @@ -130,6 +115,16 @@ void PilotCommand::processCommands(void) {
*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);

//
// 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);

}

if (state->is(STATUS_OVERRIDE))
Expand Down
6 changes: 1 addition & 5 deletions command.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,18 +23,14 @@ class PPMchannel {
PPMchannel(){};

uint16_t val = 1500;
uint16_t mid = 1500; // we allow the midpoint to be overwritten using trim
static const uint16_t untrimmed_mid = 1500;
static const uint16_t mid = 1500;
static const uint16_t min = 1100;
static const uint16_t max = 1900;

void update(uint16_t newVal) {
val = newVal;
};

boolean isExtraLow() {
return ((val - min) < (max - min) / 5);
};
boolean isLow() {
return ((val - min) < (max - min) / 10);
};
Expand Down
29 changes: 13 additions & 16 deletions control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,37 +52,34 @@ void Control::parseConfig(CONFIG_struct& config) {

void Control::calculateControlVectors() {
thrust_pid.setMasterInput(state->kinematicsAltitude);
thrust_pid.setSlaveInput(0.0f); // TODO: where would be get this data from?
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.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);

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

// compute new output levels for state
uint32_t now = micros();

// compute new slave setpoints in the master PIDs
// and compute state control torques
state->Fz = thrust_pid.Compute(now, pidEnabled[THRUST_MASTER], pidEnabled[THRUST_SLAVE]);
state->Tx = pitch_pid.Compute(now, pidEnabled[PITCH_MASTER], pidEnabled[PITCH_SLAVE]);
state->Ty = roll_pid.Compute(now, pidEnabled[ROLL_MASTER], pidEnabled[ROLL_SLAVE]);
state->Tz = yaw_pid.Compute(now, pidEnabled[YAW_MASTER], pidEnabled[YAW_SLAVE]);

// add in the feedforward torques
state->Tx += state->Tx_trim;
state->Ty += state->Ty_trim;
state->Tz += state->Tz_trim;

// use the updated pid outputs to calculate the motor drive terms
if (state->Fz == 0) { // throttle is in low condition
state->Tx = 0;
state->Ty = 0;
state->Tz = 0;

thrust_pid.IntegralReset();
pitch_pid.IntegralReset();
roll_pid.IntegralReset();
yaw_pid.IntegralReset();
}
}
2 changes: 1 addition & 1 deletion flybrix-firmware.ino
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,7 @@ Systems::Systems()
airframe{&state},
pilot{&state},
control{&state, CONFIG.data},
conf{&state, RX, &control, &CONFIG} // listen for configuration inputs
conf{&state, RX, &control, &CONFIG, &led} // listen for configuration inputs
{
}

Expand Down
137 changes: 84 additions & 53 deletions led.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,34 +23,56 @@ LED::LED(State* __state) : state(__state) {
pinMode(RED_LED, OUTPUT);
}

void LED::set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator) {
override = pattern != LED::NO_OVERRIDE;
if (!override)
return;
oldStatus = 0;
red_indicator ? indicatorRedOn() : indicatorRedOff();
green_indicator ? indicatorGreenOn() : indicatorGreenOff();
use(pattern, red_a, green_a, blue_a, red_b, green_b, blue_b);
}

void LED::update() {
cycleIndex++; // 500Hz update
cycleIndex++; // 500Hz update
cycleIndex &= 4095; // ~8 second period

if (oldStatus != state->status) {
if (!override && oldStatus != state->status) {
oldStatus = state->status;
changeLights();
}

if (lightType == 1) {
updateFlash();
} else if (lightType == 2) {
updateBeacon();
switch (lightType) {
case LED::FLASH:
updateFlash();
break;
case LED::BEACON:
updateBeacon();
break;
case LED::BREATHE:
updateBreathe();
break;
case LED::ALTERNATE:
updateAlternate();
break;
case LED::SOLID:
updateSolid();
break;
}
}

void LED::beacon(uint8_t red, uint8_t green, uint8_t blue) {
lightType = 2;
this->red = red;
this->green = green;
this->blue = blue;
void LED::use(Pattern pattern, uint8_t red, uint8_t green, uint8_t blue) {
use(pattern, red, green, blue, red, green, blue);
}

void LED::flash(uint8_t red, uint8_t green, uint8_t blue) {
lightType = 1;
this->red = red;
this->green = green;
this->blue = blue;
void LED::use(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) {
lightType = pattern;
this->red_a_ = red_a;
this->green_a_ = green_a;
this->blue_a_ = blue_a;
this->red_b_ = red_b;
this->green_b_ = green_b;
this->blue_b_ = blue_b;
}

void LED::changeLights() {
Expand All @@ -69,30 +91,30 @@ void LED::changeLights() {
digitalWriteFast(LED_A_GRN, LOW);
digitalWriteFast(LED_B_GRN, LOW);
} else if (state->is(STATUS_UNPAIRED)) {
// flash(255,180,20); //orange
flash(255, 255, 255); // for pcba testing purposes -- a "good" board will end up in this state
indicatorGreenOn(); // for pcba testing purposes -- a "good" board will end up in this state
// use(LED::FLASH, 255,180,20); //orange
use(LED::FLASH, 255, 255, 255); // for pcba testing purposes -- a "good" board will end up in this state
indicatorGreenOn(); // for pcba testing purposes -- a "good" board will end up in this state
} else if (state->is(STATUS_RX_FAIL)) {
flash(255, 0, 0); // red
use(LED::FLASH, 255, 0, 0); // red
} else if (state->is(STATUS_FAIL_STABILITY) || state->is(STATUS_FAIL_ANGLE)) {
flash(100, 110, 250); // yellow
use(LED::FLASH, 100, 110, 250); // yellow
} else if (state->is(STATUS_OVERRIDE)) {
beacon(255, 0, 0); // red
use(LED::BEACON, 255, 0, 0); // red
} else if (state->is(STATUS_ENABLING)) {
flash(0, 0, 255); // blue
use(LED::FLASH, 0, 0, 255); // blue
} else if (state->is(STATUS_ENABLED)) {
beacon(0, 0, 255); // blue for enable
use(LED::BEACON, 0, 0, 255); // blue for enable
}

else if (state->is(STATUS_TEMP_WARNING)) {
flash(100, 150, 250); // yellow
use(LED::FLASH, 100, 150, 250); // yellow
} else if (state->is(STATUS_LOG_FULL)) {
flash(0, 0, 250);
use(LED::FLASH, 0, 0, 250);
} else if (state->is(STATUS_BATTERY_LOW)) {
beacon(255, 180, 20);
use(LED::BEACON, 255, 180, 20);
} else if (state->is(STATUS_IDLE)) {
indicatorRedOff(); // clear boot test
beacon(0, 255, 0); // breathe instead?
indicatorRedOff(); // clear boot test
use(LED::BEACON, 0, 255, 0); // breathe instead?
} else {
// ERROR: ("NO STATUS BITS SET???");
}
Expand All @@ -106,31 +128,17 @@ uint8_t LED::getLightThreshold() {
}

void LED::rgb() {
uint8_t threshold = getLightThreshold();

if (red > threshold) {
digitalWriteFast(LED_A_RED, LOW);
digitalWriteFast(LED_B_RED, LOW);
} else {
digitalWriteFast(LED_A_RED, HIGH);
digitalWriteFast(LED_B_RED, HIGH);
}

if (green > threshold) {
digitalWriteFast(LED_A_GRN, LOW);
digitalWriteFast(LED_B_GRN, LOW);
} else {
digitalWriteFast(LED_A_GRN, HIGH);
digitalWriteFast(LED_B_GRN, HIGH);
}
rgb(red_a_, green_a_, blue_a_, red_b_, green_b_, blue_b_);
}

if (blue > threshold) {
digitalWriteFast(LED_A_BLU, LOW);
digitalWriteFast(LED_B_BLU, LOW);
} else {
digitalWriteFast(LED_A_BLU, HIGH);
digitalWriteFast(LED_B_BLU, HIGH);
}
void LED::rgb(uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b) {
uint8_t threshold = getLightThreshold();
digitalWriteFast(LED_A_RED, (red_a > threshold) ? LOW : HIGH);
digitalWriteFast(LED_B_RED, (red_b > threshold) ? LOW : HIGH);
digitalWriteFast(LED_A_GRN, (green_a > threshold) ? LOW : HIGH);
digitalWriteFast(LED_B_GRN, (green_b > threshold) ? LOW : HIGH);
digitalWriteFast(LED_A_BLU, (blue_a > threshold) ? LOW : HIGH);
digitalWriteFast(LED_B_BLU, (blue_b > threshold) ? LOW : HIGH);
}

void LED::updateFlash() {
Expand Down Expand Up @@ -159,6 +167,29 @@ void LED::updateBeacon() {
}
}

void LED::updateBreathe() {
uint16_t multiplier = cycleIndex & 2047;
if (cycleIndex > 511) {
allOff();
return;
}
if (multiplier > 255)
multiplier = 512 - multiplier;

rgb((multiplier * red_a_) >> 8, (multiplier * green_a_) >> 8, (multiplier * blue_a_) >> 8, (multiplier * red_b_) >> 8, (multiplier * green_b_) >> 8, (multiplier * blue_b_) >> 8);
}

void LED::updateAlternate() {
if (cycleIndex & 128)
rgb(red_a_, green_a_, blue_a_, 0, 0, 0);
else
rgb(0, 0, 0, red_b_, green_b_, blue_b_);
}

void LED::updateSolid() {
rgb();
}

void LED::allOff() {
digitalWriteFast(LED_A_RED, HIGH);
digitalWriteFast(LED_A_GRN, HIGH);
Expand Down
33 changes: 26 additions & 7 deletions led.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,24 +20,43 @@ class LED {
public:
LED(State *state);

enum Pattern : uint8_t {
NO_OVERRIDE = 0,
FLASH = 1,
BEACON = 2,
BREATHE = 3,
ALTERNATE = 4,
SOLID = 5,
};

void set(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b, bool red_indicator, bool green_indicator);

void update(); // update using the state STATUS bitfield

private:
State *state;
uint16_t oldStatus{0};
uint8_t lightType{0};
uint8_t red{0}, green{0}, blue{0};
uint8_t red_a_{0}, green_a_{0}, blue_a_{0};
uint8_t red_b_{0}, green_b_{0}, blue_b_{0};

uint16_t cycleIndex{0}; // incremented at 500Hz to support dithering, resets every ~8 seconds

bool override{false};

uint8_t getLightThreshold();

void changeLights();

void updateBeacon();
void updateFlash();
void rgb(); // dithered color
void beacon(uint8_t red, uint8_t green, uint8_t blue); // 2sec periodic double pulse
void flash(uint8_t red, uint8_t green, uint8_t blue); //~3Hz flasher
void updateBeacon(); // 2sec periodic double pulse
void updateFlash(); //~3Hz flasher
void updateBreathe(); // 2sec periodic breathe
void updateAlternate(); //~4Hz left/right alternating
void updateSolid(); // maintain constant light level
void rgb(); // dithered color
void rgb(uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b);
void use(Pattern pattern, uint8_t red_a, uint8_t green_a, uint8_t blue_a, uint8_t red_b, uint8_t green_b, uint8_t blue_b);
void use(Pattern pattern, uint8_t red, uint8_t green, uint8_t blue);

void allOff();

Expand All @@ -59,6 +78,6 @@ class LED {
#define LED_A_RED 28 // 53

#define GREEN_LED 13 // 50
#define RED_LED 27 // 54
#define RED_LED 27 // 54

#endif
Loading

0 comments on commit e3705a2

Please sign in to comment.