// provide system state estimation for controller
// alt[0-2] : altitude, climb_rate, acceleration
// sonar: sonar reading ,NAN for invalid reading(no echo or other errors), controller is responsible for filtering and switching between sonar and baro
// throttle_realized: throttle delivered last tick by motor mixer.
// motor_state: a combination of one or more of MOTOR_LIMIT_MIN, MOTOR_LIMIT_MAX.
// MOTOR_LIMIT_NONE : the total power of motor matrix didn't reached any limitation
// MOTOR_LIMIT_MAX : the total power of motor matrix has reached maximum
// MOTOR_LIMIT_MAX : the total power of motor matrix has reached minimum
// airborne: if the aircraft has liftoff
int altitude_controller::provide_states(float *alt, float sonar, float *attitude, float throttle_realized, int motor_state, bool airborne)
{
m_baro_states[0] = alt[0];
m_baro_states[1] = alt[1];
m_baro_states[2] = alt[2];
m_attitude[0] = attitude[0];
m_attitude[1] = attitude[1];
m_attitude[2] = attitude[2];
m_throttle_realized = throttle_realized;
// m_motor_state = motor_state;
m_airborne = airborne;
m_sonar = sonar;
if (!isnan(sonar))
{
// step response
// 0.1f : sonar update interval
float sonar_update_inteval = 0.1f;
float step_change_max = sonar_step_threshold * (fabs(m_baro_states[1]) * sonar_update_inteval + 0.5f * fabs(m_baro_states[2]) * sonar_update_inteval * sonar_update_inteval);
step_change_max = fmax(step_change_max, 0.2f);
if (isnan(climb_rate_override) && !isnan(m_sonar_target) && fabs(sonar - m_last_valid_sonar) > step_change_max) // note: only do step response with no climb rate override
{
m_sonar_target += sonar - m_last_valid_sonar;
start_braking();
}
m_last_valid_sonar = sonar;
}
return 0;
}
int8_t set_motor_levels(int16_t motor_speed_level,
uint16_t motor_direction_level)
{
motor_levels.speed_channel_level = motor_speed_level;
motor_levels.direction_channel_level = motor_direction_level;
motor_timestamp = get_current_time();
if (motor_speed_level == MOTOR_LEVEL_BRAKE)
{
start_braking();
}
else if (motor_speed_level > 0) // if want to go forward
{
// if already going forward or stopped
if ((motor_direction == MOTOR_DIRECTION_FORWARD) || (motor_direction
== MOTOR_DIRECTION_STOPPED))
{
set_speed_motor_pwm_level(motor_speed_level);
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_FORWARD);
}
else // if trying to change directions
{
start_brake_coasting();
post_error(ERR_MOTOR_FAULT);
}
}
else if (motor_speed_level < 0) // if want to go backward
{
if ((motor_direction == MOTOR_DIRECTION_REVERSE) || (motor_direction
== MOTOR_DIRECTION_STOPPED))
{
set_speed_motor_pwm_level(motor_speed_level);
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_REVERSE);
}
else // if trying to changed directions
{
start_brake_coasting();
post_error(ERR_MOTOR_FAULT);
}
}
else // if stopping, but not changing directions, then just coast
{
set_speed_motor_pwm_level(motor_speed_level);
motor_state = MOTOR_STATE_COASTING;
motor_coast_timestamp = get_current_time();
}
return ERR_NONE;
}
void motor_run_handler(uint32_t current_time)
{
if (get_nvm_error_flag() != ERR_NONE)
return;
switch (motor_state)
{
case MOTOR_STATE_IDLE:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
set_sensor_waiting_for_measurement(true);
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
break;
case MOTOR_STATE_BRAKING:
if (check_for_timeout(current_time, motor_brake_timestamp,
MOTOR_BRAKE_TIME))
{
start_brake_coasting();
}
break;
case MOTOR_STATE_BRAKE_COAST:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
debug_P(PSTR("Stop brake coasting @ %lu\n"), current_time);
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_BRAKE_READ_BACK_EMF;
motor_read_back_emf_count = 0;
}
break;
case MOTOR_STATE_BRAKE_READ_BACK_EMF:
if (!get_sensor_waiting_for_measurement())
{
handle_back_emf_measurement();
// if we have receive a brake command we just keep braking
if (motor_levels.speed_channel_level == MOTOR_LEVEL_BRAKE)
{
if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else
{
post_error(ERR_MOTOR_FAULT);
}
}
else if (motor_levels.speed_channel_level > 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_FORWARD))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_FORWARD);
}
}
else if (motor_levels.speed_channel_level < 0)
{
if ((motor_direction == MOTOR_DIRECTION_STOPPED)
|| (motor_direction == MOTOR_DIRECTION_REVERSE))
{
motor_state = MOTOR_STATE_ACTIVE;
set_motor_direction(MOTOR_DIRECTION_REVERSE);
}
}
else if (motor_direction == MOTOR_DIRECTION_STOPPED)
{
motor_state = MOTOR_STATE_IDLE;
set_motor_direction(MOTOR_DIRECTION_STOPPED);
}
// motor fault is done
if (motor_state != MOTOR_STATE_BRAKE_READ_BACK_EMF)
{
if (get_last_error(NULL) == ERR_MOTOR_FAULT)
clear_last_error();
}
else if (++motor_read_back_emf_count == BACK_EMF_READ_COUNT)
{
start_braking();
}
else
{
set_sensor_waiting_for_measurement(true);
}
}
break;
case MOTOR_STATE_COASTING:
if (check_for_timeout(current_time, motor_coast_timestamp,
MOTOR_COAST_TIME))
{
set_sensor_waiting_for_measurement(true);
motor_state = MOTOR_STATE_IDLE;
}
break;
case MOTOR_STATE_ACTIVE:
if (check_for_timeout(current_time, motor_timestamp, motor_timeout))
{
motor_levels.speed_channel_level = 0;
start_braking();
debug_P(PSTR("MOTOR Timeout @ %lu\n"), get_current_time());
post_error(ERR_MOTOR_TIMEOUT);
}
else
{
set_speed_motor_pwm_level(motor_levels.speed_channel_level);
set_direction_motor_pwm_level(motor_levels.direction_channel_level);
}
break;
}
}
