// output booster throttle, if any
void AP_MotorsMulticopter::output_boost_throttle(void)
{
if (_boost_scale > 0) {
float throttle = constrain_float(get_throttle() * _boost_scale, 0, 1);
SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, throttle*1000);
}
}
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void AP_MotorsMulticopter::update_battery_resistance()
{
// if disarmed reset resting voltage and current
if (!_flags.armed) {
_batt_voltage_resting = _batt_voltage;
_batt_current_resting = _batt_current;
_batt_timer = 0;
} else if(_batt_voltage_resting < _batt_voltage && _batt_current_resting > _batt_current) {
// update battery resistance when throttle is over hover throttle
float batt_resistance = (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting);
if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
if (get_throttle() >= get_throttle_hover()) {
// filter reaches 90% in 1/4 the test time
_batt_resistance += 0.05f*(batt_resistance - _batt_resistance);
_batt_timer += 1;
} else {
// initialize battery resistance to prevent change in resting voltage estimate
_batt_resistance = batt_resistance;
}
}
// make sure battery resistance value doesn't result in the predicted battery voltage exceeding the resting voltage
if(batt_resistance < _batt_resistance){
_batt_resistance = batt_resistance;
}
}
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
if (_servo_test_cycle_counter > 0){
// perform boot-up servo test cycle if enabled
servo_test();
} else {
// manual override (i.e. when setting up swash)
switch (_servo_mode) {
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
// pass pilot commands straight through to swash
_roll_in = _roll_radio_passthrough;
_pitch_in = _pitch_radio_passthrough;
_throttle_filter.reset(_throttle_radio_passthrough);
_yaw_in = _yaw_radio_passthrough;
break;
case SERVO_CONTROL_MODE_MANUAL_CENTER:
// fixate mid collective
_roll_in = 0.0f;
_pitch_in = 0.0f;
_throttle_filter.reset(_collective_mid_pct);
_yaw_in = 0.0f;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
// fixate max collective
_roll_in = 0.0f;
_pitch_in = 0.0f;
_throttle_filter.reset(1.0f);
_yaw_in = 1.0f;
break;
case SERVO_CONTROL_MODE_MANUAL_MIN:
// fixate min collective
_roll_in = 0.0f;
_pitch_in = 0.0f;
_throttle_filter.reset(0.0f);
_yaw_in = -1.0f;
break;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
// use servo_test function from child classes
servo_test();
break;
default:
// no manual override
break;
}
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
// helicopters always run stabilizing flight controls
move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
update_motor_control(ROTOR_CONTROL_STOP);
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli::output_armed_zero_throttle()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) {
reset_flight_controls();
}
move_actuators(_roll_in, _pitch_in, get_throttle(), _yaw_in);
update_motor_control(ROTOR_CONTROL_IDLE);
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrixTS::output_armed_stabilizing()
{
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float thrust_max = 0.0f; // highest motor value
float thr_adj = 0.0f; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
thrust_max = 0.0f;
for (int i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
// calculate the thrust outputs for roll and pitch
_thrust_rpyt_out[i] = throttle_thrust + roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
if (thrust_max < _thrust_rpyt_out[i]) {
thrust_max = _thrust_rpyt_out[i];
}
}
}
// if max thrust is more than one reduce average throttle
if (thrust_max > 1.0f) {
thr_adj = 1.0f - thrust_max;
limit.throttle_upper = true;
limit.roll_pitch = true;
for (int i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
// calculate the thrust outputs for roll and pitch
_thrust_rpyt_out[i] += thr_adj;
}
}
}
}
// return true if motors are moving
bool AP_MotorsUGV::active() const
{
// if soft disarmed, motors not active
if (!hal.util->get_soft_armed()) {
return false;
}
// check throttle is active
if (!is_zero(get_throttle())) {
return true;
}
// skid-steering vehicles active when steering
if (have_skid_steering() && !is_zero(get_steering())) {
return true;
}
return false;
}
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void AP_MotorsMulticopter::update_battery_resistance()
{
// if disarmed reset resting voltage and current
if (!_flags.armed) {
_batt_voltage_resting = _batt_voltage;
_batt_current_resting = _batt_current;
_batt_timer = 0;
} else {
// update battery resistance when throttle is over hover throttle
if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
if (get_throttle() >= _hover_out) {
// filter reaches 90% in 1/4 the test time
_batt_resistance += 0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance);
_batt_timer += 1;
} else {
// initialize battery resistance to prevent change in resting voltage estimate
_batt_resistance = ((_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting));
}
}
}
}
// sends commands to the motors
void AP_MotorsSingle::output_armed_stabilizing()
{
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in
float actuator[NUM_ACTUATORS]; // combined roll, pitch and yaw thrusts for each actuator
float actuator_max = 0.0f; // maximum actuator value
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain();
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
// calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw
if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f;
} else {
rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), (float)_yaw_headroom/1000.0f)) / rp_thrust_max, 0.0f, 1.0f);
if (rp_scale < 1.0f) {
limit.roll_pitch = true;
}
}
actuator_allowed = 1.0f - rp_scale * rp_thrust_max;
if (fabsf(yaw_thrust) > actuator_allowed) {
yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed);
limit.yaw = true;
}
// combine roll, pitch and yaw on each actuator
// front servo
actuator[0] = rp_scale * roll_thrust - yaw_thrust;
// right servo
actuator[1] = rp_scale * pitch_thrust - yaw_thrust;
// rear servo
actuator[2] = -rp_scale * roll_thrust - yaw_thrust;
// left servo
actuator[3] = -rp_scale * pitch_thrust - yaw_thrust;
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3])));
thr_adj = throttle_thrust - throttle_avg_max;
if (thr_adj < (thrust_min_rpy - throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rpy, throttle_avg_max) - throttle_avg_max;
}
// calculate the throttle setting for the lift fan
_thrust_out = throttle_avg_max + thr_adj;
if (is_zero(_thrust_out)) {
limit.roll_pitch = true;
limit.yaw = true;
}
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,_thrust_out), 0.5f, 1.0f);
// calculate the maximum allowed actuator output and maximum requested actuator output
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
if (actuator_max > fabsf(actuator[i])) {
actuator_max = fabsf(actuator[i]);
}
}
if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) {
// roll, pitch and yaw request can not be achieved at full servo defection
// reduce roll, pitch and yaw to reduce the requested defection to maximum
limit.roll_pitch = true;
limit.yaw = true;
rp_scale = thrust_out_actuator/actuator_max;
} else {
rp_scale = 1.0f;
}
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {
_actuator_out[i] = constrain_float(rp_scale*actuator[i]/thrust_out_actuator, -1.0f, 1.0f);
}
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsTri::output_armed_stabilizing()
{
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float rpy_low = 0.0f; // lowest motor value
float rpy_high = 0.0f; // highest motor value
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
_yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX);
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain()*sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
throttle_thrust = get_throttle() * get_compensation_gain();
// calculate angle of yaw pivot
_pivot_angle = safe_asin(yaw_thrust);
if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
limit.yaw = true;
_pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg));
}
float pivot_thrust_max = cosf(_pivot_angle);
float thrust_max = 1.0f;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
// The following mix may be offer less coupling between axis but needs testing
//_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
//_thrust_left = roll_thrust * 0.5f + pitch_thrust * 1.0f;
//_thrust_rear = 0;
_thrust_right = roll_thrust * -0.5f + pitch_thrust * 0.5f;
_thrust_left = roll_thrust * 0.5f + pitch_thrust * 0.5f;
_thrust_rear = pitch_thrust * -0.5f;
// calculate roll and pitch for each motor
// set rpy_low and rpy_high to the lowest and highest values of the motors
// record lowest roll pitch command
rpy_low = MIN(_thrust_right,_thrust_left);
rpy_high = MAX(_thrust_right,_thrust_left);
if (rpy_low > _thrust_rear){
rpy_low = _thrust_rear;
}
// check to see if the rear motor will reach maximum thrust before the front two motors
if ((1.0f - rpy_high) > (pivot_thrust_max - _thrust_rear)){
thrust_max = pivot_thrust_max;
rpy_high = _thrust_rear;
}
// calculate throttle that gives most possible room for yaw (range 1000 ~ 2000) which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible room margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, _throttle_avg_max);
if(is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
}
// calculate how close the motors can come to the desired throttle
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if(rpy_scale < 1.0f){
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
if (thr_adj > 0.0f){
limit.throttle_upper = true;
}
thr_adj = 0.0f;
}else{
if(thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
// Throttle can't be reduced to desired value
thr_adj = -(throttle_thrust_best_rpy+rpy_low);
}else if(thr_adj > thrust_max - (throttle_thrust_best_rpy+rpy_high)){
// Throttle can't be increased to desired value
thr_adj = thrust_max - (throttle_thrust_best_rpy+rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
_thrust_right = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_right;
_thrust_left = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_left;
_thrust_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear;
// scale pivot thrust to account for pivot angle
// we should not need to check for divide by zero as _pivot_angle is constrained to the 5deg ~ 80 deg range
_thrust_rear = _thrust_rear/cosf(_pivot_angle);
// constrain all outputs to 0.0f to 1.0f
// test code should be run with these lines commented out as they should not do anything
_thrust_right = constrain_float(_thrust_right, 0.0f, 1.0f);
_thrust_left = constrain_float(_thrust_left, 0.0f, 1.0f);
_thrust_rear = constrain_float(_thrust_rear, 0.0f, 1.0f);
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed_stabilizing()
{
uint8_t i; // general purpose counter
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float rpy_low = 0.0f; // lowest motor value
float rpy_high = 0.0f; // highest motor value
float yaw_allowed = 1.0f; // amount of yaw we can fit in
float unused_range; // amount of yaw we can fit in the current channel
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
// calculate throttle that gives most possible room for yaw which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
throttle_thrust_best_rpy = MIN(0.5f, throttle_thrust_rpy_mix);
// calculate roll and pitch for each motor
// calculate the amount of yaw input that each motor can accept
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
if (!is_zero(_yaw_factor[i])){
if (yaw_thrust * _yaw_factor[i] > 0.0f) {
unused_range = fabsf((1.0 - (throttle_thrust_best_rpy + _thrust_rpyt_out[i]))/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
} else {
unused_range = fabsf((throttle_thrust_best_rpy + _thrust_rpyt_out[i])/_yaw_factor[i]);
if (yaw_allowed > unused_range) {
yaw_allowed = unused_range;
}
}
}
}
}
// todo: make _yaw_headroom 0 to 1
yaw_allowed = MAX(yaw_allowed, (float)_yaw_headroom/1000.0f);
if (fabsf(yaw_thrust) > yaw_allowed) {
yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
limit.yaw = true;
}
// add yaw to intermediate numbers for each motor
rpy_low = 0.0f;
rpy_high = 0.0f;
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll+pitch+yaw command
if (_thrust_rpyt_out[i] > rpy_high) {
rpy_high = _thrust_rpyt_out[i];
}
}
}
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_thrust_rpy_mix);
if (is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {
rpy_scale = constrain_float(-throttle_thrust_best_rpy/rpy_low, 0.0f, 1.0f);
}
// calculate how close the motors can come to the desired throttle
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f){
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
thr_adj = 0.0f;
} else {
if (thr_adj < -(throttle_thrust_best_rpy+rpy_low)){
// Throttle can't be reduced to desired value
thr_adj = -(throttle_thrust_best_rpy+rpy_low);
} else if (thr_adj > 1.0f - (throttle_thrust_best_rpy+rpy_high)){
// Throttle can't be increased to desired value
thr_adj = 1.0f - (throttle_thrust_best_rpy+rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + rpy_scale*_thrust_rpyt_out[i];
}
}
// constrain all outputs to 0.0f to 1.0f
// test code should be run with these lines commented out as they should not do anything
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = constrain_float(_thrust_rpyt_out[i], 0.0f, 1.0f);
}
}
}
int main(void){
clock_switch32M();
uart_init();
bus_init(&bus);
PORTC.DIR = 0xFF;
//Setup a timer, just for counting. Used for bus message and timeouts.
TCC1_CTRLA = TC_CLKSEL_DIV1024_gc;
//Set the interrupts we want to listen to.
PMIC.CTRL = PMIC_MEDLVLEN_bm |PMIC_HILVLEN_bm | PMIC_RREN_bm;
//Make sure to move the reset vectors to application flash.
CCP = CCP_IOREG_gc;
PMIC.CTRL &= ~PMIC_IVSEL_bm;
//Enable interrupts.
sei();
//Read temperature calibration:
read_calibration();
uint8_t cnt_tm = 0; //Used by TCC1
uint8_t poll_cnt = 0; //Used for polling the display.
//uint16_t pwm_lim = 0; //PWM limit may change at higher speed.
while(1){
continue;
get_measurements();
if (get_throttle()){
motor.throttle = throttle;
}else{
motor.throttle = 0;
}
if (display.online == false){
motor.mode = 0;
}
//Set display data
display.voltage = motor.voltage;
display.current = motor.current;
//display.power = power_av;
display.throttle = throttle;
display.error = status | motor.status;
/*if (motor.status){
display.error = motor.status;
}*/
//display.speed = speed% 1000; //999 max
if (TCC1.CNT > 500){
cnt_tm++;
TCC1.CNT = 0;
//Last character in message
if (wait_for_last_char){
wait_for_last_char = false;
}else{
bus_endmessage(&bus);
}
//Send timer tick
bus_tick(&bus);
if (bus_check_for_message()){
green_led_on();
}else{
green_led_off();
//Increase offline count.
display.offline_cnt++;
if (display.offline_cnt > 10){
uart_rate_find();
PORTC.OUTSET = PIN6_bm;
}else{
PORTC.OUTCLR = PIN6_bm;
}
//If offline too long:
if (display.offline_cnt > 50){
//You need to make it unsafe again.
display.road_legal = true;
display.online = false;
throttle_cruise = false;
throttle = 0;
use_brake = false;
brake = 0;
}
if (motor.offline_cnt > 20){
motor.online = false;
}
poll_cnt++;
if (poll_cnt == 2){
bus_display_poll(&bus);
}else if (poll_cnt == 4){
bus_display_update(&bus);
}else if (poll_cnt == 6){
bus_motor_poll(&bus);
motor.offline_cnt++;
}else if (poll_cnt == 8){
poll_cnt = 0;
}
}
}
}
}
// sends commands to the motors
void AP_MotorsCoax::output_armed_stabilizing()
{
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float thrust_min_rpy; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float thrust_out; //
float rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= _throttle_thrust_max) {
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
// calculate how much roll and pitch must be scaled to leave enough range for the minimum yaw
if (is_zero(rp_thrust_max)) {
rp_scale = 1.0f;
} else {
rp_scale = constrain_float((1.0f - MIN(fabsf(yaw_thrust), 0.5f*(float)_yaw_headroom/1000.0f)) / rp_thrust_max, 0.0f, 1.0f);
if (rp_scale < 1.0f) {
limit.roll_pitch = true;
}
}
actuator_allowed = 2.0f * (1.0f - rp_scale * rp_thrust_max);
if (fabsf(yaw_thrust) > actuator_allowed) {
yaw_thrust = constrain_float(yaw_thrust, -actuator_allowed, actuator_allowed);
limit.yaw = true;
}
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(fabsf(rp_scale * rp_thrust_max), fabsf(yaw_thrust));
thr_adj = throttle_thrust - _throttle_avg_max;
if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rpy, _throttle_avg_max) - _throttle_avg_max;
}
// calculate the throttle setting for the lift fan
thrust_out = _throttle_avg_max + thr_adj;
if (fabsf(yaw_thrust) > thrust_out) {
yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out);
limit.yaw = true;
}
_thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust;
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f);
if (is_zero(thrust_out)) {
limit.roll_pitch = true;
}
// force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared
// static thrust is proportional to the airflow velocity squared
// therefore the torque of the roll and pitch actuators should be approximately proportional to
// the angle of attack multiplied by the static thrust.
_actuator_out[0] = roll_thrust/thrust_out_actuator;
_actuator_out[1] = pitch_thrust/thrust_out_actuator;
if (fabsf(_actuator_out[0]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f);
}
if (fabsf(_actuator_out[1]) > 1.0f) {
limit.roll_pitch = true;
_actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f);
}
_actuator_out[2] = -_actuator_out[0];
_actuator_out[3] = -_actuator_out[1];
}
// update the throttle input filter. should be called at 100hz
void AP_MotorsMulticopter::update_throttle_hover(float dt)
{
if (_throttle_hover_learn != HOVER_LEARN_DISABLED) {
_throttle_hover = _throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(get_throttle()-_throttle_hover);
}
}
// output_armed - sends commands to the motors
// includes new scaling stability patch
void AP_MotorsMatrix::output_armed_stabilizing()
{
uint8_t i; // general purpose counter
float roll_thrust; // roll thrust input value, +/- 1.0
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float throttle_thrust_max; // throttle thrust maximum value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float yaw_allowed = 1.0f; // amount of yaw we can fit in
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
// apply voltage and air pressure compensation
const float compensation_gain = get_compensation_gain(); // compensation for battery voltage and altitude
roll_thrust = _roll_in * compensation_gain;
pitch_thrust = _pitch_in * compensation_gain;
yaw_thrust = _yaw_in * compensation_gain;
throttle_thrust = get_throttle() * compensation_gain;
throttle_avg_max = _throttle_avg_max * compensation_gain;
throttle_thrust_max = _thrust_boost_ratio + (1.0f - _thrust_boost_ratio) * _throttle_thrust_max;
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
throttle_thrust = 0.0f;
limit.throttle_lower = true;
}
if (throttle_thrust >= throttle_thrust_max) {
throttle_thrust = throttle_thrust_max;
limit.throttle_upper = true;
}
// ensure that throttle_avg_max is between the input throttle and the maximum throttle
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, throttle_thrust_max);
// calculate throttle that gives most possible room for yaw which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
// 2. the higher of:
// a) the pilot's throttle input
// b) the point _throttle_rpy_mix between the pilot's input throttle and hover-throttle
// Situation #2 ensure we never increase the throttle above hover throttle unless the pilot has commanded this.
// Situation #2b allows us to raise the throttle above what the pilot commanded but not so far that it would actually cause the copter to rise.
// We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favor reducing throttle *because* it provides better yaw control)
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// Under the motor lost condition we remove the highest motor output from our calculations and let that motor go greater than 1.0
// To ensure control and maximum righting performance Hex and Octo have some optimal settings that should be used
// Y6 : MOT_YAW_HEADROOM = 350, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.375, ATC_RAT_PIT_IMAX = 0.375, ATC_RAT_YAW_IMAX = 0.375
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.75, ATC_RAT_PIT_IMAX = 0.75, ATC_RAT_YAW_IMAX = 0.375
// Usable minimums below may result in attitude offsets when motors are lost. Hex aircraft are only marginal and must be handles with care
// Hex : MOT_YAW_HEADROOM = 0, ATC_RAT_RLL_IMAX = 1.0, ATC_RAT_PIT_IMAX = 1.0, ATC_RAT_YAW_IMAX = 0.5
// Octo-Quad (x8) x : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.25, ATC_RAT_PIT_IMAX = 0.25, ATC_RAT_YAW_IMAX = 0.25
// Octo-Quad (x8) + : MOT_YAW_HEADROOM = 300, ATC_RAT_RLL_IMAX = 0.5, ATC_RAT_PIT_IMAX = 0.5, ATC_RAT_YAW_IMAX = 0.25
// Quads cannot make use of motor loss handling because it doesn't have enough degrees of freedom.
// calculate amount of yaw we can fit into the throttle range
// this is always equal to or less than the requested yaw from the pilot or rate controller
float rp_low = 1.0f; // lowest thrust value
float rp_high = -1.0f; // highest thrust value
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
// calculate the thrust outputs for roll and pitch
_thrust_rpyt_out[i] = roll_thrust * _roll_factor[i] + pitch_thrust * _pitch_factor[i];
// record lowest roll+pitch command
if (_thrust_rpyt_out[i] < rp_low) {
rp_low = _thrust_rpyt_out[i];
}
// record highest roll+pitch command
if (_thrust_rpyt_out[i] > rp_high && (!_thrust_boost || i != _motor_lost_index)) {
rp_high = _thrust_rpyt_out[i];
}
}
}
// include the lost motor scaled by _thrust_boost_ratio
if (_thrust_boost && motor_enabled[_motor_lost_index]) {
// record highest roll+pitch command
if (_thrust_rpyt_out[_motor_lost_index] > rp_high) {
rp_high = _thrust_boost_ratio*rp_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index];
}
}
// check for roll and pitch saturation
if (rp_high-rp_low > 1.0f || throttle_avg_max < -rp_low) {
// Full range is being used by roll and pitch.
limit.roll_pitch = true;
}
// calculate the highest allowed average thrust that will provide maximum control range
throttle_thrust_best_rpy = MIN(0.5f, throttle_avg_max);
// calculate the maximum yaw control that can be used
// todo: make _yaw_headroom 0 to 1
yaw_allowed = (float)_yaw_headroom / 1000.0f;
yaw_allowed = _thrust_boost_ratio*0.5f + (1.0f - _thrust_boost_ratio) * yaw_allowed;
yaw_allowed = MAX(MIN(throttle_thrust_best_rpy+rp_low, 1.0f - (throttle_thrust_best_rpy + rp_high)), yaw_allowed);
if (fabsf(yaw_thrust) > yaw_allowed) {
// not all commanded yaw can be used
yaw_thrust = constrain_float(yaw_thrust, -yaw_allowed, yaw_allowed);
limit.yaw = true;
}
// add yaw control to thrust outputs
float rpy_low = 1.0f; // lowest thrust value
float rpy_high = -1.0f; // highest thrust value
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = _thrust_rpyt_out[i] + yaw_thrust * _yaw_factor[i];
// record lowest roll+pitch+yaw command
if (_thrust_rpyt_out[i] < rpy_low) {
rpy_low = _thrust_rpyt_out[i];
}
// record highest roll+pitch+yaw command
if (_thrust_rpyt_out[i] > rpy_high && (!_thrust_boost || i != _motor_lost_index)) {
rpy_high = _thrust_rpyt_out[i];
}
}
}
// include the lost motor scaled by _thrust_boost_ratio
if (_thrust_boost) {
// record highest roll+pitch+yaw command
if (_thrust_rpyt_out[_motor_lost_index] > rpy_high && motor_enabled[_motor_lost_index]) {
rpy_high = _thrust_boost_ratio*rpy_high + (1.0f-_thrust_boost_ratio)*_thrust_rpyt_out[_motor_lost_index];
}
}
// calculate any scaling needed to make the combined thrust outputs fit within the output range
if (rpy_high-rpy_low > 1.0f) {
rpy_scale = 1.0f / (rpy_high-rpy_low);
}
if (is_negative(rpy_low)) {
rpy_scale = MIN(rpy_scale, -throttle_avg_max / rpy_low);
}
// calculate how close the motors can come to the desired throttle
rpy_high *= rpy_scale;
rpy_low *= rpy_scale;
throttle_thrust_best_rpy = -rpy_low;
thr_adj = throttle_thrust - throttle_thrust_best_rpy;
if (rpy_scale < 1.0f) {
// Full range is being used by roll, pitch, and yaw.
limit.roll_pitch = true;
limit.yaw = true;
if (thr_adj > 0.0f) {
limit.throttle_upper = true;
}
thr_adj = 0.0f;
} else {
if (thr_adj < 0.0f) {
// Throttle can't be reduced to desired value
// todo: add lower limit flag and ensure it is handled correctly in altitude controller
thr_adj = 0.0f;
} else if (thr_adj > 1.0f - (throttle_thrust_best_rpy + rpy_high)) {
// Throttle can't be increased to desired value
thr_adj = 1.0f - (throttle_thrust_best_rpy + rpy_high);
limit.throttle_upper = true;
}
}
// add scaled roll, pitch, constrained yaw and throttle for each motor
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
_thrust_rpyt_out[i] = throttle_thrust_best_rpy + thr_adj + (rpy_scale * _thrust_rpyt_out[i]);
}
}
// check for failed motor
check_for_failed_motor(throttle_thrust_best_rpy + thr_adj);
}
void AP_MotorsMulticopter::output_logic()
{
// force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 0.0f;
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
break;
case SPIN_WHEN_ARMED: {
// Motors should be stationary or at spin when armed.
// Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set and increment ramp variables
float spool_step = 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
if (_spool_desired == DESIRED_SHUT_DOWN){
_throttle_low_end_pct -= spool_step;
// constrain ramp value and update mode
if (_throttle_low_end_pct <= 0.0f) {
_throttle_low_end_pct = 0.0f;
_multicopter_flags.spool_mode = SHUT_DOWN;
}
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_throttle_low_end_pct += spool_step;
// constrain ramp value and update mode
if (_throttle_low_end_pct >= 1.0f) {
_throttle_low_end_pct = 1.0f;
_multicopter_flags.spool_mode = SPOOL_UP;
}
} else { // _spool_desired == SPIN_WHEN_ARMED
float spin_when_armed_low_end_pct = 0.0f;
if (_min_throttle > 0) {
spin_when_armed_low_end_pct = (float)_spin_when_armed / _min_throttle;
}
_throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct-_throttle_low_end_pct, -spool_step, spool_step);
}
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
break;
}
case SPOOL_UP:
// Maximum throttle should move from minimum to maximum.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
// constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
_throttle_thrust_max = get_current_limit_max_throttle();
_multicopter_flags.spool_mode = THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
break;
case THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
update_throttle_rpy_mix();
break;
case SPOOL_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servoes should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_multicopter_flags.spool_mode = SPOOL_UP;
break;
}
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix_desired = _throttle_rpy_mix;
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_rpy_mix <= 0.0f){
_throttle_rpy_mix = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max) && is_zero(_throttle_rpy_mix)) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
}
break;
}
}
// update the throttle input filter. should be called at 100hz
void AP_MotorsMulticopter::update_throttle_hover(float dt)
{
if (_throttle_hover_learn != HOVER_LEARN_DISABLED) {
// we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
_throttle_hover = constrain_float(_throttle_hover + (dt/(dt+AP_MOTORS_THST_HOVER_TC))*(get_throttle()-_throttle_hover), AP_MOTORS_THST_HOVER_MIN, AP_MOTORS_THST_HOVER_MAX);
}
}
// run spool logic
void AP_MotorsMulticopter::output_logic()
{
if (_flags.armed) {
_disarm_safety_timer = 100;
} else if (_disarm_safety_timer != 0) {
_disarm_safety_timer--;
}
// force desired and current spool mode if disarmed or not interlocked
if (!_flags.armed || !_flags.interlock) {
_spool_desired = DESIRED_SHUT_DOWN;
_spool_mode = SHUT_DOWN;
}
if (_spool_up_time < 0.05) {
// prevent float exception
_spool_up_time.set(0.05);
}
switch (_spool_mode) {
case SHUT_DOWN:
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_SHUT_DOWN) {
_spool_mode = SPIN_WHEN_ARMED;
break;
}
// set and increment ramp variables
_spin_up_ratio = 0.0f;
_throttle_thrust_max = 0.0f;
break;
case SPIN_WHEN_ARMED: {
// Motors should be stationary or at spin when armed.
// Servos should be moving to correct the current attitude.
// set limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = true;
// set and increment ramp variables
float spool_step = 1.0f/(_spool_up_time*_loop_rate);
if (_spool_desired == DESIRED_SHUT_DOWN){
_spin_up_ratio -= spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio <= 0.0f) {
_spin_up_ratio = 0.0f;
_spool_mode = SHUT_DOWN;
}
} else if(_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_spin_up_ratio += spool_step;
// constrain ramp value and update mode
if (_spin_up_ratio >= 1.0f) {
_spin_up_ratio = 1.0f;
_spool_mode = SPOOL_UP;
}
} else { // _spool_desired == SPIN_WHEN_ARMED
float spin_up_armed_ratio = 0.0f;
if (_spin_min > 0.0f) {
spin_up_armed_ratio = _spin_arm / _spin_min;
}
_spin_up_ratio += constrain_float(spin_up_armed_ratio-_spin_up_ratio, -spool_step, spool_step);
}
_throttle_thrust_max = 0.0f;
break;
}
case SPOOL_UP:
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED ){
_spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max += 1.0f/(_spool_up_time*_loop_rate);
// constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
_throttle_thrust_max = get_current_limit_max_throttle();
_spool_mode = THROTTLE_UNLIMITED;
} else if (_throttle_thrust_max < 0.0f) {
_throttle_thrust_max = 0.0f;
}
break;
case THROTTLE_UNLIMITED:
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired != DESIRED_THROTTLE_UNLIMITED) {
_spool_mode = SPOOL_DOWN;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
break;
case SPOOL_DOWN:
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
// make sure the motors are spooling in the correct direction
if (_spool_desired == DESIRED_THROTTLE_UNLIMITED) {
_spool_mode = SPOOL_UP;
break;
}
// set and increment ramp variables
_spin_up_ratio = 1.0f;
_throttle_thrust_max -= 1.0f/(_spool_up_time*_loop_rate);
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max)) {
_spool_mode = SPIN_WHEN_ARMED;
}
break;
}
}
/*
void read_calibration(void){
//Get the factory temperature value, at 85 Deg. C.
ad_temp_ref = SP_ReadCalibrationByte(PRODSIGNATURES_ADCACAL0) ;
ad_temp_ref |= SP_ReadCalibrationByte(PRODSIGNATURES_ADCACAL1) <<8;
}
*/
int main(void){
clock_switch32M();
init_pwm();
uart_init();
bus_init(&bus);
//Clear the AD settings.
PORTA.DIR = 0;
PORTA.OUT = 0;
ADCA.CTRLA = ADC_FLUSH_bm;
ADCA.CTRLB = 0;
ADCA.REFCTRL = 0;
ADCA.EVCTRL = 0;
ADCA.PRESCALER = 0;
//Set PORTB as AD input
PORTB.DIR = 0;
PORTB.OUT = 0;
//Setup a timer, just for counting. Used for bus message and timeouts.
TCC1_CTRLA = TC_CLKSEL_DIV1024_gc;
//Set up the RTC for speed timing.
CLK.RTCCTRL = CLK_RTCSRC_RCOSC32_gc | CLK_RTCEN_bm;
RTC.CTRL = RTC_PRESCALER_DIV1_gc;
RTC.COMP = 32768;
//A counter for 100% PWM timing.
TCD0.CTRLA = TC_CLKSEL_DIV1024_gc;
TCD0.PER = 320;
TCD0.INTCTRLA = TC_OVFINTLVL_HI_gc;
//Make sure to move the reset vectors to application flash.
//Set the interrupts we want to listen to.
CCP = CCP_IOREG_gc;
PMIC.CTRL = PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm |PMIC_HILVLEN_bm | PMIC_RREN_bm;
//Enable interrupts.
sei();
//Set defaults:
eepsettings.straincal = 200;
eemem_read_block(EEMEM_MAGIC_HEADER_SETTINGS,(uint8_t*)&eepsettings, sizeof(eepsettings), EEBLOCK_SETTINGS1);
//Apply settings...
strain_threshhold = eepsettings.straincal;
//Startup hall state.
hall_state = 0x04;
get_hall();
//Startup PWM value.
pwm = PWM_SET_MIN;
uint8_t cnt_tm = 0; //Used by TCC1
uint8_t poll_cnt = 0; //Used for polling the display.
uint16_t pwm_lim = 0; //PWM limit may change at higher speed.
display.strain_th = strain_threshhold;
//Two flags that can only be set.
bool should_freewheel = true;
bool force_commute = false;
while(1){
//Double check the PWM hasn't gone crazy:
if (pwm < pwm_set_min){
pwm = pwm_set_min;
}
if (pwm > pwm_set_max){
pwm = pwm_set_max;
}
//Testing
if (testvalue > pwm_set_max){
testvalue = pwm_set_max;
}
if (testvalue < pwm_set_min){
testvalue = pwm_set_min;
}
//Apply PWM settings.
TCC0.CCA = pwm;
TCC0.CCB = pwm;
TCC0.CCC = pwm;
//Get halls sensors.
if (!get_hall()){
should_freewheel = true;
status_set(STAT_HALL_FAULT);
}
//Commute or freewheel:
if (should_freewheel){
pwm_freewheel();
}else if (force_commute){
if (direction == FORWARD){
commute_forward();
}else{
commute_backward();
}
}else if (flag_hall_changed){
if (direction == FORWARD){
commute_forward();
}else{
commute_backward();
}
}else{
//pwm_freewheel();
}
//Remeber...
pwm_prev = pwm;
//Reset
should_freewheel = false;
//force_commute = false;
if (startup == 10){ //Just started
//Setup PWM
commute_start();
pwm = estimate_pwm_byspeed();
//Apply PWM settings.
TCC0.CCA = pwm;
TCC0.CCB = pwm;
TCC0.CCC = pwm;
}
if (display.online == false){
should_freewheel = true;
}
if (motor.mode == 0){
should_freewheel = true;
}
//Get voltage/current.
get_measurements();
//Hard under and over voltage.
if (ad_voltage > 3700){ //3900
//Freewheel as fast as possible.
pwm_freewheel();
should_freewheel = true;
status_set(STAT_OVER_VOLTAGE);
}else if (ad_voltage < 1800){ //About 19 volts.
//Freewheel as fast as possible.
pwm_freewheel();
should_freewheel = true;
status_set(STAT_UNDER_VOLTAGE);
pwm_inc(200);pwm_inc(200);
}
//Regenning too hard:
if (ad_current_regav < -1300){
//Freewheel as fast as possible.
//pwm_freewheel();
//should_freewheel = true;
//status_set(STAT_REGEN_HIGH);
//startup = 3;
}
#if (HARDWARE_HAS_THROTTLE)
if (use_throttle){
//This gas pedal is low on disconnect.
if (!get_throttle()){
status_set(STAT_THROTTLE_FAULT);
should_freewheel = true;
}
//Use the brake too:
if (get_brake()){
if (use_brake){
throttle_cruise = false;
throttle_tick = 0;
}else{
if (throttle == 0){ //< 5
should_freewheel = true;
}
}
}else{
status_set(STAT_BRAKE_FAULT);
should_freewheel = true;
}
}
#endif
if (usehall){
//Timer:
if (flag_hall_changed){
//Toggle blue led:
PORTC.OUTTGL = PIN7_bm;
//compute time:
time_now = RTC.CNT;
time_comm = time_now;
//Running average.
time_comm_av += time_comm;
time_comm_av >>= 1;
commtime_sum+= time_comm;
commtime_cnt++;
cnt_commutations++;
//Reset timer.
RTC.CNT = 0;
//Computer average current in this time:
if (ad_current_regcnt > 0){
ad_current_regav = ad_current_regsum / (int32_t)ad_current_regcnt;
}else{
ad_current_regav = ad_current;
}
//Values reset in next if-statement.
}
//Allowed to go faster if the commutation time is guaranteed to switch fast enough.
//TCD0 is used in case the motor suddenly stops. Capacitors are 100uF/16V
if (time_comm_av < 250){
//Round REV0 does not support 100% duty cycle.
pwm_set_min = 0;
}
//Limit for braking
pwm_lim = pwm_set_max - (display.function_val3 * 10);
if (pwm_lim < pwm_set_min){
pwm_lim = pwm_set_min;
}else if (pwm_lim > pwm_set_max){
pwm_lim = pwm_set_max;
}
//That's really slow.
if (RTC.CNT> 5000){
speed = 0;
}
//Minimum response time
if (flag_hall_changed) {
//Clear current sum:
ad_current_regcnt = 0;
ad_current_regsum = 0;
RTC.CNT = 0;
}
//Control loop
if ((flag_hall_changed) || (TCC1.CNT > 450)){
//Clear hall flag if it was set.
flag_hall_changed = false;
if (!off){
//If brake is enabled:
if (use_brake){
if (brake > 5){
//Regenning is a bit harder. The amount of strength decreases if PWM is too high.
//Brake is current limited.
//At low speed, ovverride PWM settings
if (speed < 150){
pwm_lim = pwm_set_max;
}
if ((ad_current_regav + 169) > (((int16_t)brake)*-12)){ //100 * 10 -> current of about -8 Amp
//More power: inc PWM
//Use the speed average as a voltage estimate for the motor.
//24V will give an unloaded speed of 350 (35.0kmh)
//If we're doing 10km/h, the motor is at 7V approx, 30% duty cycle.
if (pwm < pwm_lim){
pwm_inc(slope_brake);
}
}else{
//Back off
pwm_dec(slope_brake);
}
//Voltage limits
if (ad_voltage > 3600){
pwm_dec(slope_brake/2);
}
if (ad_voltage > 3700){
pwm_dec(slope_brake/2);
}
}
}
if (use_throttle){
//Undo brake :
if (flag_brake_lowrpm){
off = false;
tie = false;
status_clear();
flag_brake_lowrpm = false;
}
if (throttle > 5){
/*
//Throttle is current limited.
if ((ad_current_regav + 169) < (((int16_t)throttle)*8)){
//More power
pwm_dec(slope_throttle*2);
}else if ((ad_current_regav + 169) < (((int16_t)throttle)*16)){ //100 * 16 -> current of about 12 Amp
//More power
pwm_dec(slope_throttle);
}else{
//Back off
pwm_inc(slope_throttle);
}*/
//Voltage limits
if (ad_voltage < 1950){
pwm_inc(slope_throttle*2);
}
if (ad_voltage < 1900){
pwm_inc(slope_throttle*2);
}
//Cal current reg:
//Current calibration:
#if(HARDWARE_VER == HW_CTRL_REV0)
int32_t regualtion = ad_current_regav;
regualtion += 1749;
regualtion *= 1000;
regualtion /= 286;
regualtion -= 34;
#endif
//Current regulation
if (speed){
if (regualtion < ((int16_t)throttle*(int16_t)motor.mode)){ //200 * 0-5
//More power
pwm_dec(slope_throttle*2);
}else if (regualtion < ((int16_t)throttle*(int16_t)motor.mode)){
//More power
pwm_dec(slope_throttle);
}else{
//Back off
pwm_inc(slope_throttle);
}
}
}
}
if (use_pedalassist) {
if (ad_strain_av > highestforce){
highestforce = ad_strain_av;
}
if (ad_strain_av > strain_threshhold){
strain_cnt = 75UL + (2UL*speed);
if (ad_strain_av-strain_threshhold < 100){
pedal_signal = ad_strain_av-strain_threshhold;
}else{
pedal_signal = 100;
}
}
if (strain_cnt){
strain_cnt--;
}else{
pedal_signal = 0;
}
if (pedal_signal && strain_cnt){
//Same control loop as in throttle:
//Voltage limits
if (ad_voltage < 1950){
pwm_inc(slope_throttle*2);
}
if (ad_voltage < 1900){
pwm_inc(slope_throttle*2);
}
//Cal current reg:
//Current calibration:
#if(HARDWARE_VER == HW_CTRL_REV0)
int32_t regualtion = ad_current_regav;
regualtion += 1749;
regualtion *= 1000;
regualtion /= 286;
regualtion -= 34;
#endif
//Current regulation
if (speed){
if (regualtion < ((int16_t)pedal_signal*2*(int16_t)motor.mode)){ //200 * 0-5
//More power
pwm_dec(slope_throttle*2);
}else if (regualtion < ((int16_t)pedal_signal*2*(int16_t)motor.mode)){
//More power
pwm_dec(slope_throttle);
}else{
//Back off
pwm_inc(slope_throttle);
}
}
}
}
}else{ //if (off)
//Undo braking at low RPM
if (flag_brake_lowrpm){
off = false;
tie = false;
status_clear();
flag_brake_lowrpm = false;
}
}
}
}else{ //Hall hasn't changed.
//Don't use hall.
if (RTC.CNT > time_comm_av_last){
//Reset timer.
RTC.CNT = 0;
}
}
//Act on a fault?
if (isfaultset()){
if (tie){
tie_ground();
}else{
should_freewheel = true;
}
}
if (use_pedalassist && (!use_throttle)){
if (!pedal_signal){
should_freewheel = true;
}
}else if ((!use_pedalassist) && (use_throttle && (throttle < 5))){
should_freewheel = true;
}else if ((use_throttle)&& (use_pedalassist)){
if ((throttle < 5) && (!pedal_signal)){
should_freewheel = true;
}
}
//Current min/max
if (ad_current < ad_currentmin){
ad_currentmin = ad_current;
}
if (ad_current > ad_currentmax){
ad_currentmax = ad_current;
}
if (flag_keep_pwm){
pwm = pwm_prev;
}
cnt_rotations = cnt_commutations / 48;
//Set display data
display.voltage = ad_voltage_av;
display.current = ad_current_av;
display.power = power_av;
display.error = status;
display.speed = speed% 1000; //999 max
motor.status = status;
//display.speed = pwm;
//display.current = pwm_lim;
if (use_brake){
display.throttle = brake;
}else{
if (use_pedalassist){
if (pedal_signal > throttle){
display.throttle = pedal_signal;
}else{
display.throttle = throttle;
}
}else{
display.throttle = throttle;
}
}
display.cruise = throttle_cruise;
//Throttle test
//display.speed = pwm;
//display.current = status;
if (TCC1.CNT > 450){
cnt_tm++;
TCC1.CNT = 0;
//Testing
/*
if (display.button_state & BUTT_MASK_FRONT){
if (testvalue < (PWM_SET_MAX -5)){
testvalue+=5;
}
}else if (display.button_state & BUTT_MASK_TOP){
if (testvalue > (PWM_SET_MIN+5)){
testvalue-=5;
}
}*/
//Apply speed limit:
pwm_set_max = (display.function_val2 +1) * 50;
if (pwm_set_max > PWM_SET_MAX){
pwm_set_max = PWM_SET_MAX;
}
//Startup from standstill
uint8_t throttle_was = throttle;
throttle = motor.throttle;
if ((throttle_was == 0) && throttle && (speed == 0)){
force_commute = true;
pwm_inc(20);
}else{
force_commute = false;
}
if (!motor.mode){
pwm_inc(200);
}
//Testing
//Last character in message
if (wait_for_last_char){
wait_for_last_char = false;
}else{
bus_endmessage(&bus);
}
//Estimate speed /pwm. Only when there is no throttle signal or brake.
if (!use_brake){
if ((use_throttle && (throttle < 5)) && ((use_pedalassist)&&(!pedal_signal))){
pwm = estimate_pwm_byspeed();
}
}
//Send timer tick
bus_tick(&bus);
if (bus_check_for_message()){
green_led_on();
}else{
green_led_off();
//Increase offline count.
display.offline_cnt++;
if (display.offline_cnt > 10){
uart_rate_find();
PORTC.OUTSET = PIN6_bm;
}else{
PORTC.OUTCLR = PIN6_bm;
}
//If offline too long:
if (display.offline_cnt > 50){
//You need to make it unsafe again.
display.road_legal = true;
display.online = false;
throttle_cruise = false;
should_freewheel = true;
throttle = 0;
use_brake = false;
brake = 0;
pedal_signal = 0;
strain_cnt = 0;
motor.mode = 0;
}
poll_cnt++;
if (poll_cnt == 2){
#if (OPT_DISPLAY== FW_DISPLAY_MASTER)
bus_display_poll(&bus);
#endif
}else if (poll_cnt == 4){
//Update the display, and it's variables:
//Speed is now the amount of commutations per seconds.
if (commtime_sum){
speed = (commtime_cnt * 32768UL) / commtime_sum;
//With a 26" wheel:
speed = (speed * 10000UL) / 6487UL;
}else{
speed=0;
}
commtime_sum = 0;
commtime_cnt = 0 ;
//Disaplay average voltage:
ad_voltage_av = ad_voltage_sum / meas_cnt;
ad_voltage_sum = 0;
//And current.
ad_current_av = ad_current_sum / meas_cnt;
ad_current_sum = 0;
//Strain, if we have it:
#ifdef HARDWARE_HAS_STRAIN
//Running average:
int32_t strain_val = ad_strain_sum / meas_cnt;
strain_val /= 100;
ad_strain_av += strain_val;
ad_strain_av /= 2;
//ad_strain_av = meas_cnt;
ad_strain_sum = 0;
//Strain calibration in Menu F8/F3:
if ((display.func == 8) && (display.menu_downcnt > 100)){
strain_threshhold = 200;
display.strain_th = strain_threshhold;
}
if ((display.func == 3) && (display.menu_downcnt > 100)){
if (ad_strain_av > strain_threshhold){
strain_threshhold++;
}
display.strain_th = strain_threshhold;
flagsave = true;
}
if (display.menu_timeout == 0){
if (flagsave){
flagsave = false;
//Save
eepsettings.straincal = strain_threshhold;
eemem_write_block(EEMEM_MAGIC_HEADER_SETTINGS,(uint8_t*)&eepsettings, sizeof(eepsettings), EEBLOCK_SETTINGS1);
}
}
//Set test value
//uint16_t s;
//adc_init_single_ended(REF_3V3,ADC_CH_MUXPOS_PIN8_gc); //Strain Sensor.
//s = adc_getsample();
//2180 average, over if you help.
/*
if (s > 2183){
display.value1 = 200;
testvalue+=1;
}else if (s < 2177){
display.value1 = 000;
testvalue-=1;
}else{
display.value1 = 100;
}*/
//motor.current = ad_strain_av;
display.value1 = ad_strain_av;// ad_strain_av - 2000;
display.value2 = highestforce;// ad_strain_av - 2000;
display.value3 = pwm;
display.value4 = should_freewheel;
#endif
//Temperature
ad_temp_av = ad_temp_sum / meas_cnt;
ad_temp_sum = 0;
//Set test value
//display.value2 = ad_temp_av / 2;
//Temperature about 20 Deg.C is 1866 counts. CKDIV16 2020 CKDIV512
//Counts per kelvin.
/*
uint32_t cpk = ad_temp_ref*10 / 358;
ad_temp_av = (ad_temp_av * 10) / cpk - 273;
ad_temp_av = ad_temp_ref;
*/
meas_cnt = 0;
//Calibration:
ad_voltage_av -= 175;
ad_voltage_av *= 1000;
ad_voltage_av /= 886;
//Current calibration:
#if(HARDWARE_VER == HW_CTRL_REV0)
ad_current_av += 1749;
ad_current_av *= 1000;
ad_current_av /= 286;
ad_current_av -= 34;
#else
ad_current_av += 169;
ad_current_av *= 100;
ad_current_av /= 75;
#endif
power_av = (ad_current_av * ad_voltage_av) / 10000;
poll_cnt = 0;
#if(OPT_DISPLAY == FW_DISPLAY_MASTER)
bus_display_update(&bus);
#endif
//motor.mode = 1;
}
}
}
//Counter from TCC1
if (cnt_tm > 15){
cnt_tm = 0;
//Increase throttle tick, for cruise control.
if ((use_throttle)&& (!use_brake)){
if ((display.online)&&(throttle > 10)){
if (throttle_tick < 20){
throttle_tick++;
}else{
throttle_cruise = true;
}
}
}
//test
//pwm_dec(5);
testvalue-=5;
ad_currentmin = 9999;
ad_currentmax = -9999;
if (startup){
startup--;
if (startup == 0){
//Clear any status flag:
//status_clear();
//Give it a nudge.
//commute_allowed = true;
}
}else{
status_clear();
}
}
//Set fault flags:
if (off){
status_set(STAT_STOPPED);
}
if (use_throttle){
//status_set(STAT_USETHROTTLE);
}
}