diff --git a/command.cpp b/command.cpp index f4448b4..986701f 100644 --- a/command.cpp +++ b/command.cpp @@ -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 @@ -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); } diff --git a/command.h b/command.h index 32bcf6e..7eaf976 100644 --- a/command.h +++ b/command.h @@ -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; diff --git a/config.cpp b/config.cpp index 1794373..abe7bae 100644 --- a/config.cpp +++ b/config.cpp @@ -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; @@ -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 @@ -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) @@ -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 diff --git a/config.h b/config.h index 85c1c79..4999194 100644 --- a/config.h +++ b/config.h @@ -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} diff --git a/control.cpp b/control.cpp index 1c4b80c..9107d9c 100644 --- a/control.cpp +++ b/control.cpp @@ -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(); diff --git a/flybrix-firmware.ino b/flybrix-firmware.ino index f9f8cf9..85bf9da 100644 --- a/flybrix-firmware.ino +++ b/flybrix-firmware.ino @@ -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; @@ -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; } diff --git a/led.cpp b/led.cpp index 5c15945..4978ae4 100644 --- a/led.cpp +++ b/led.cpp @@ -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? diff --git a/state.cpp b/state.cpp index 2145bf5..0e5077e 100644 --- a/state.cpp +++ b/state.cpp @@ -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); diff --git a/version.h b/version.h index e01e2ca..0fa39c5 100644 --- a/version.h +++ b/version.h @@ -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