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