void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_STATUSTEXT: { // ignore any statustext messages not from our GCS: if (msg->sysid != rover.g.sysid_my_gcs) { break; } mavlink_statustext_t packet; mavlink_msg_statustext_decode(msg, &packet); char text[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1+4] = { 'G', 'C', 'S', ':'}; memcpy(&text[4], packet.text, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN); rover.DataFlash.Log_Write_Message(text); break; } case MAVLINK_MSG_ID_COMMAND_INT: { // decode packet mavlink_command_int_t packet; mavlink_msg_command_int_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; switch (packet.command) { #if MOUNT == ENABLED case MAV_CMD_DO_SET_ROI: { // param1 : /* Region of interest mode (not used)*/ // param2 : /* MISSION index/ target ID (not used)*/ // param3 : /* ROI index (not used)*/ // param4 : /* empty */ // x : lat // y : lon // z : alt // sanity check location if (!check_latlng(packet.x, packet.y)) { break; } Location roi_loc; roi_loc.lat = packet.x; roi_loc.lng = packet.y; roi_loc.alt = (int32_t)(packet.z * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; } #endif default: result = MAV_RESULT_UNSUPPORTED; break; } // send ACK or NAK mavlink_msg_command_ack_send_buf(msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; // do command switch (packet.command) { case MAV_CMD_START_RX_PAIR: result = handle_rc_bind(packet); break; case MAV_CMD_NAV_RETURN_TO_LAUNCH: rover.set_mode(RTL); result = MAV_RESULT_ACCEPTED; break; #if MOUNT == ENABLED // Sets the region of interest (ROI) for the camera case MAV_CMD_DO_SET_ROI: // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location roi_loc; roi_loc.lat = (int32_t)(packet.param5 * 1.0e7f); roi_loc.lng = (int32_t)(packet.param6 * 1.0e7f); roi_loc.alt = (int32_t)(packet.param7 * 100.0f); if (roi_loc.lat == 0 && roi_loc.lng == 0 && roi_loc.alt == 0) { // switch off the camera tracking if enabled if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { rover.camera_mount.set_mode_to_default(); } } else { // send the command to the camera mount rover.camera_mount.set_roi_target(roi_loc); } result = MAV_RESULT_ACCEPTED; break; #endif #if CAMERA == ENABLED case MAV_CMD_DO_DIGICAM_CONFIGURE: rover.camera.configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_DO_DIGICAM_CONTROL: if (rover.camera.control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6)) { rover.log_picture(); } result = MAV_RESULT_ACCEPTED; break; #endif // CAMERA == ENABLED case MAV_CMD_DO_MOUNT_CONTROL: #if MOUNT == ENABLED rover.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7); result = MAV_RESULT_ACCEPTED; #endif break; case MAV_CMD_MISSION_START: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_CALIBRATION: if (hal.util->get_soft_armed()) { result = MAV_RESULT_FAILED; break; } if (is_equal(packet.param1, 1.0f)) { rover.ins.init_gyro(); if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_equal(packet.param3, 1.0f)) { rover.init_barometer(false); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param4, 1.0f)) { rover.trim_radio(); result = MAV_RESULT_ACCEPTED; } else if (is_equal(packet.param5, 1.0f)) { result = MAV_RESULT_ACCEPTED; // start with gyro calibration rover.ins.init_gyro(); // reset ahrs gyro bias if (rover.ins.gyro_calibrated_ok_all()) { rover.ahrs.reset_gyro_drift(); } else { result = MAV_RESULT_FAILED; } rover.ins.acal_init(); rover.ins.get_acal()->start(this); } else if (is_equal(packet.param5, 2.0f)) { // start with gyro calibration rover.ins.init_gyro(); // accel trim float trim_roll, trim_pitch; if (rover.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine rover.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { send_text(MAV_SEVERITY_WARNING, "Unsupported preflight calibration"); } break; case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: { uint8_t compassNumber = -1; if (is_equal(packet.param1, 2.0f)) { compassNumber = 0; } else if (is_equal(packet.param1, 5.0f)) { compassNumber = 1; } else if (is_equal(packet.param1, 6.0f)) { compassNumber = 2; } if (compassNumber != (uint8_t) -1) { rover.compass.set_and_save_offsets(compassNumber, packet.param2, packet.param3, packet.param4); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: rover.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: rover.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_STABILIZE_DISARMED: case MAV_MODE_STABILIZE_ARMED: rover.set_mode(LEARNING); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (rover.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_SERVO: if (rover.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_SET_RELAY: if (rover.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_DO_REPEAT_RELAY: if (rover.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_equal(packet.param1, 1.0f) || is_equal(packet.param1, 3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1, 3.0f)); result = MAV_RESULT_ACCEPTED; } break; case MAV_CMD_COMPONENT_ARM_DISARM: if (is_equal(packet.param1, 1.0f)) { // run pre_arm_checks and arm_checks and display failures if (rover.arm_motors(AP_Arming::MAVLINK)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (is_zero(packet.param1)) { if (rover.disarm_motors()) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_GET_HOME_POSITION: if (rover.home_is_set != HOME_UNSET) { send_home(rover.ahrs.get_home()); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } break; case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1, 1.0f)) { send_autopilot_version(FIRMWARE_VERSION); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) // param5 : latitude // param6 : longitude // param7 : altitude result = MAV_RESULT_FAILED; // assume failure if (is_equal(packet.param1, 1.0f)) { rover.init_home(); } else { if (is_zero(packet.param5) && is_zero(packet.param6) && is_zero(packet.param7)) { // don't allow the 0,0 position break; } // sanity check location if (!check_latlng(packet.param5, packet.param6)) { break; } Location new_home_loc {}; new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f); new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); rover.ahrs.set_home(new_home_loc); rover.home_is_set = HOME_SET_NOT_LOCKED; rover.Log_Write_Home_And_Origin(); GCS_MAVLINK::send_home_all(new_home_loc); result = MAV_RESULT_ACCEPTED; rover.gcs_send_text_fmt(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", (double)(new_home_loc.lat * 1.0e-7f), (double)(new_home_loc.lng * 1.0e-7f), (uint32_t)(new_home_loc.alt * 0.01f)); } break; } case MAV_CMD_DO_START_MAG_CAL: case MAV_CMD_DO_ACCEPT_MAG_CAL: case MAV_CMD_DO_CANCEL_MAG_CAL: result = rover.compass.handle_mag_cal_command(packet); break; case MAV_CMD_NAV_SET_YAW_SPEED: { // param1 : yaw angle to adjust direction by in centidegress // param2 : Speed - normalized to 0 .. 1 // exit if vehicle is not in Guided mode if (rover.control_mode != GUIDED) { break; } rover.guided_mode = Guided_Angle; rover.guided_yaw_speed.msg_time_ms = AP_HAL::millis(); rover.guided_yaw_speed.turn_angle = packet.param1; rover.guided_yaw_speed.target_speed = constrain_float(packet.param2, 0.0f, 1.0f); rover.nav_set_yaw_speed(); result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_ACCELCAL_VEHICLE_POS: result = MAV_RESULT_FAILED; if (rover.ins.get_acal()->gcs_vehicle_position(packet.param1)) { result = MAV_RESULT_ACCEPTED; } break; default: break; } mavlink_msg_command_ack_send_buf( msg, chan, packet.command, result); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&rover, &Rover::mavlink_set_mode, bool, uint8_t)); break; } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { handle_mission_request_list(rover.mission, msg); break; } // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST_INT: case MAVLINK_MSG_ID_MISSION_REQUEST: { handle_mission_request(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_ACK: { // not used break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { // mark the firmware version in the tlog send_text(MAV_SEVERITY_INFO, FIRMWARE_STRING); #if defined(PX4_GIT_VERSION) && defined(NUTTX_GIT_VERSION) send_text(MAV_SEVERITY_INFO, "PX4: " PX4_GIT_VERSION " NuttX: " NUTTX_GIT_VERSION); #endif handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { handle_mission_clear_all(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { handle_mission_set_current(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_COUNT: { handle_mission_count(rover.mission, msg); break; } case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { handle_mission_write_partial_list(rover.mission, msg); break; } // GCS has sent us a mission item, store to EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_MISSION_ITEM_INT: { if (handle_mission_item(msg, rover.mission)) { rover.DataFlash.Log_Write_EntireMission(rover.mission); } break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, &rover.DataFlash); break; } case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: { // allow override of RC channel values for HIL // or for complete GCS control of switch position // and RC PWM values. if (msg->sysid != rover.g.sysid_my_gcs) { // Only accept control from our gcs break; } mavlink_rc_channels_override_t packet; int16_t v[8]; mavlink_msg_rc_channels_override_decode(msg, &packet); v[0] = packet.chan1_raw; v[1] = packet.chan2_raw; v[2] = packet.chan3_raw; v[3] = packet.chan4_raw; v[4] = packet.chan5_raw; v[5] = packet.chan6_raw; v[6] = packet.chan7_raw; v[7] = packet.chan8_raw; hal.rcin->set_overrides(v, 8); rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_RC, false); break; } case MAVLINK_MSG_ID_HEARTBEAT: { // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes if (msg->sysid != rover.g.sysid_my_gcs) { break; } rover.last_heartbeat_ms = rover.failsafe.rc_override_timer = AP_HAL::millis(); rover.failsafe_trigger(FAILSAFE_EVENT_GCS, false); break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: // MAV ID: 84 { // decode packet mavlink_set_position_target_local_ned_t packet; mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != GUIDED) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED && packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED && packet.coordinate_frame != MAV_FRAME_BODY_NED && packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; // prepare and send target position if (!pos_ignore) { Location loc = rover.current_loc; switch (packet.coordinate_frame) { case MAV_FRAME_BODY_NED: case MAV_FRAME_BODY_OFFSET_NED: { // rotate from body-frame to NE frame float ne_x = packet.x*rover.ahrs.cos_yaw() - packet.y*rover.ahrs.sin_yaw(); float ne_y = packet.x*rover.ahrs.sin_yaw() + packet.y*rover.ahrs.cos_yaw(); // add offset to current location location_offset(loc, ne_x, ne_y); } break; case MAV_FRAME_LOCAL_OFFSET_NED: // add offset to current location location_offset(loc, packet.x, packet.y); break; default: // MAV_FRAME_LOCAL_NED interpret as an offset from home loc = rover.ahrs.get_home(); location_offset(loc, packet.x, packet.y); break; } rover.guided_WP = loc; rover.rtl_complete = false; rover.set_guided_WP(); } break; } case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: // MAV ID: 86 { // decode packet mavlink_set_position_target_global_int_t packet; mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode if (rover.control_mode != GUIDED) { break; } // check for supported coordinate frames if (packet.coordinate_frame != MAV_FRAME_GLOBAL_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT && packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) { break; } bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE; // prepare and send target position if (!pos_ignore) { Location loc = rover.current_loc; loc.lat = packet.lat_int; loc.lng = packet.lon_int; rover.guided_WP = loc; rover.rtl_complete = false; rover.set_guided_WP(); } break; } case MAVLINK_MSG_ID_GPS_RTCM_DATA: case MAVLINK_MSG_ID_GPS_INPUT: case MAVLINK_MSG_ID_HIL_GPS: { rover.gps.handle_msg(msg); break; } #if HIL_MODE != HIL_MODE_DISABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; mavlink_msg_hil_state_decode(msg, &packet); // sanity check location if (!check_latlng(packet.lat, packet.lon)) { break; } // set gps hil sensor Location loc; loc.lat = packet.lat; loc.lng = packet.lon; loc.alt = packet.alt/10; Vector3f vel(packet.vx, packet.vy, packet.vz); vel *= 0.01f; gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D, packet.time_usec/1000, loc, vel, 10, 0); // rad/sec Vector3f gyros; gyros.x = packet.rollspeed; gyros.y = packet.pitchspeed; gyros.z = packet.yawspeed; // m/s/s Vector3f accels; accels.x = packet.xacc * (GRAVITY_MSS/1000.0f); accels.y = packet.yacc * (GRAVITY_MSS/1000.0f); accels.z = packet.zacc * (GRAVITY_MSS/1000.0f); ins.set_gyro(0, gyros); ins.set_accel(0, accels); compass.setHIL(0, packet.roll, packet.pitch, packet.yaw); compass.setHIL(1, packet.roll, packet.pitch, packet.yaw); break; } #endif // HIL_MODE #if CAMERA == ENABLED // deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: { break; } // deprecated. Use MAV_CMD_DO_DIGICAM_CONFIGURE case MAVLINK_MSG_ID_DIGICAM_CONTROL: { rover.camera.control_msg(msg); rover.log_picture(); break; } #endif // CAMERA == ENABLED #if MOUNT == ENABLED // deprecated. Use MAV_CMD_DO_MOUNT_CONFIGURE case MAVLINK_MSG_ID_MOUNT_CONFIGURE: { rover.camera_mount.configure_msg(msg); break; } // deprecated. Use MAV_CMD_DO_MOUNT_CONTROL case MAVLINK_MSG_ID_MOUNT_CONTROL: { rover.camera_mount.control_msg(msg); break; } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: case MAVLINK_MSG_ID_RADIO_STATUS: { handle_radio_status(msg, rover.DataFlash, rover.should_log(MASK_LOG_PM)); break; } case MAVLINK_MSG_ID_LOG_REQUEST_DATA: rover.in_log_download = true; /* no break */ case MAVLINK_MSG_ID_LOG_ERASE: /* no break */ case MAVLINK_MSG_ID_LOG_REQUEST_LIST: if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_LOG_REQUEST_END: rover.in_log_download = false; if (!rover.in_mavlink_delay) { handle_log_message(msg, rover.DataFlash); } break; case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, rover.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, rover.gps); break; case MAVLINK_MSG_ID_DISTANCE_SENSOR: rover.sonar.handle_msg(msg); break; case MAVLINK_MSG_ID_REMOTE_LOG_BLOCK_STATUS: rover.DataFlash.remote_log_block_status_msg(chan, msg); break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: send_autopilot_version(FIRMWARE_VERSION); break; case MAVLINK_MSG_ID_LED_CONTROL: // send message to Notify AP_Notify::handle_led_control(msg); break; case MAVLINK_MSG_ID_PLAY_TUNE: // send message to Notify AP_Notify::handle_play_tune(msg); break; default: handle_common_message(msg); break; } // end switch } // end handle mavlink
void AP_TECS::_update_throttle(void) { // Calculate limits to be applied to potential energy error to prevent over or underspeed occurring due to large height errors float SPE_err_max = 0.5f * _TASmax * _TASmax - _SKE_dem; float SPE_err_min = 0.5f * _TASmin * _TASmin - _SKE_dem; // Calculate total energy error _STE_error = constrain_float((_SPE_dem - _SPE_est), SPE_err_min, SPE_err_max) + _SKE_dem - _SKE_est; float STEdot_dem = constrain_float((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max); float STEdot_error = STEdot_dem - _SPEdot - _SKEdot; // Apply 0.5 second first order filter to STEdot_error // This is required to remove accelerometer noise from the measurement STEdot_error = 0.2f*STEdot_error + 0.8f*_STEdotErrLast; _STEdotErrLast = STEdot_error; // Calculate throttle demand // If underspeed condition is set, then demand full throttle if (_underspeed) { _throttle_dem = 1.0f; } else { // Calculate gain scaler from specific energy error to throttle float K_STE2Thr = 1 / (timeConstant() * (_STEdot_max - _STEdot_min)); // Calculate feed-forward throttle float ff_throttle = 0; float nomThr = aparm.throttle_cruise * 0.01f; const Matrix3f &rotMat = _ahrs.get_dcm_matrix(); // Use the demanded rate of change of total energy as the feed-forward demand, but add // additional component which scales with (1/cos(bank angle) - 1) to compensate for induced // drag increase during turns. float cosPhi = sqrtf((rotMat.a.y*rotMat.a.y) + (rotMat.b.y*rotMat.b.y)); STEdot_dem = STEdot_dem + _rollComp * (1.0f/constrain_float(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f); ff_throttle = nomThr + STEdot_dem / (_STEdot_max - _STEdot_min) * (_THRmaxf - _THRminf); // Calculate PD + FF throttle _throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle; // Constrain throttle demand _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); // Rate limit PD + FF throttle // Calculate the throttle increment from the specified slew time if (aparm.throttle_slewrate != 0) { float thrRateIncr = _DT * (_THRmaxf - _THRminf) * aparm.throttle_slewrate * 0.01f; _throttle_dem = constrain_float(_throttle_dem, _last_throttle_dem - thrRateIncr, _last_throttle_dem + thrRateIncr); _last_throttle_dem = _throttle_dem; } // Calculate integrator state upper and lower limits // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand // Additionally constrain the integrator state amplitude so that the integrator comes off limits faster. float maxAmp = 0.5f*(_THRmaxf - _THRminf); float integ_max = constrain_float((_THRmaxf - _throttle_dem + 0.1f),-maxAmp,maxAmp); float integ_min = constrain_float((_THRminf - _throttle_dem - 0.1f),-maxAmp,maxAmp); // Calculate integrator state, constraining state // Set integrator to a max throttle value during climbout _integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr; if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) { _integ6_state = integ_max; } else { _integ6_state = constrain_float(_integ6_state, integ_min, integ_max); } // Sum the components. // Only use feed-forward component if airspeed is not being used if (_ahrs.airspeed_sensor_enabled()) { _throttle_dem = _throttle_dem + _integ6_state; } else { _throttle_dem = ff_throttle; } } // Constrain throttle demand _throttle_dem = constrain_float(_throttle_dem, _THRminf, _THRmaxf); }
void AC_AttitudeControl::input_euler_angle_roll_pitch_euler_rate_yaw_smooth(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds, float smoothing_gain) { // Convert from centidegrees on public interface to radians float euler_roll_angle_rad = radians(euler_roll_angle_cd*0.01f); float euler_pitch_angle_rad = radians(euler_pitch_angle_cd*0.01f); float euler_yaw_rate_rads = radians(euler_yaw_rate_cds*0.01f); // Sanity check smoothing gain smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f); // Add roll trim to compensate tail rotor thrust in heli (will return zero on multirotors) euler_roll_angle_rad += get_roll_trim_rad(); if ((get_accel_roll_max_radss() > 0.0f) && _rate_bf_ff_enabled) { // When roll acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler roll-axis // angular velocity that will cause the euler roll angle to smoothly stop at the input angle with limited deceleration // and an exponential decay specified by smoothing_gain at the end. float euler_rate_desired_rads = sqrt_controller(euler_roll_angle_rad-_att_target_euler_rad.x, smoothing_gain, get_accel_roll_max_radss()); // Acceleration is limited directly to smooth the beginning of the curve. float rate_change_limit_rads = get_accel_roll_max_radss() * _dt; _att_target_euler_rate_rads.x = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.x-rate_change_limit_rads, _att_target_euler_rate_rads.x+rate_change_limit_rads); // The output rate is used to update the attitude target euler angles and is fed forward into the rate controller. update_att_target_roll(_att_target_euler_rate_rads.x, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD); } else { // When acceleration limiting and feedforward are not enabled, the target roll euler angle is simply set to the // input value and the feedforward rate is zeroed. _att_target_euler_rad.x = euler_roll_angle_rad; _att_target_euler_rate_rads.x = 0; } _att_target_euler_rad.x = constrain_float(_att_target_euler_rad.x, -get_tilt_limit_rad(), get_tilt_limit_rad()); if ((get_accel_pitch_max_radss() > 0.0f) && _rate_bf_ff_enabled) { // When pitch acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler pitch-axis // angular velocity that will cause the euler pitch angle to smoothly stop at the input angle with limited deceleration // and an exponential decay specified by smoothing_gain at the end. float euler_rate_desired_rads = sqrt_controller(euler_pitch_angle_rad-_att_target_euler_rad.y, smoothing_gain, get_accel_pitch_max_radss()); // Acceleration is limited directly to smooth the beginning of the curve. float rate_change_limit_rads = get_accel_pitch_max_radss() * _dt; _att_target_euler_rate_rads.y = constrain_float(euler_rate_desired_rads, _att_target_euler_rate_rads.y-rate_change_limit_rads, _att_target_euler_rate_rads.y+rate_change_limit_rads); // The output rate is used to update the attitude target euler angles and is fed forward into the rate controller. update_att_target_pitch(_att_target_euler_rate_rads.y, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX_RAD); } else { _att_target_euler_rad.y = euler_pitch_angle_rad; _att_target_euler_rate_rads.y = 0; } _att_target_euler_rad.y = constrain_float(_att_target_euler_rad.y, -get_tilt_limit_rad(), get_tilt_limit_rad()); if (get_accel_yaw_max_radss() > 0.0f) { // When yaw acceleration limiting is enabled, the yaw input shaper constrains angular acceleration about the yaw axis, slewing // the output rate towards the input rate. float rate_change_limit_rads = get_accel_yaw_max_radss() * _dt; _att_target_euler_rate_rads.z += constrain_float(euler_yaw_rate_rads - _att_target_euler_rate_rads.z, -rate_change_limit_rads, rate_change_limit_rads); // The output rate is used to update the attitude target euler angles and is fed forward into the rate controller. update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD); } else { // When yaw acceleration limiting is disabled, the attitude target is simply rotated using the input rate and the input rate // is fed forward into the rate controller. _att_target_euler_rate_rads.z = euler_yaw_rate_rads; update_att_target_yaw(_att_target_euler_rate_rads.z, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX_RAD); } // Convert euler angle derivative of desired attitude into a body-frame angular velocity vector for feedforward if (_rate_bf_ff_enabled) { euler_rate_to_ang_vel(_att_target_euler_rad, _att_target_euler_rate_rads, _att_target_ang_vel_rads); } else { euler_rate_to_ang_vel(_att_target_euler_rad, Vector3f(0,0,_att_target_euler_rate_rads.z), _att_target_ang_vel_rads); } // Call attitude controller attitude_controller_run_euler(_att_target_euler_rad, _att_target_ang_vel_rads); }
// angle_ef_roll_pitch_rate_ef_yaw_smooth - attempts to maintain a roll and pitch angle and yaw rate (all earth frame) while smoothing the attitude based on the feel parameter // smoothing_gain : a number from 1 to 50 with 1 being sluggish and 50 being very crisp void AC_AttitudeControl::angle_ef_roll_pitch_rate_ef_yaw_smooth(float roll_angle_ef, float pitch_angle_ef, float yaw_rate_ef, float smoothing_gain) { float rate_ef_desired; float rate_change_limit; Vector3f angle_ef_error; // earth frame angle errors // sanity check smoothing gain smoothing_gain = constrain_float(smoothing_gain,1.0f,50.0f); // if accel limiting and feed forward enabled if ((_accel_roll_max > 0.0f) && _rate_bf_ff_enabled) { rate_change_limit = _accel_roll_max * _dt; // calculate earth-frame feed forward roll rate using linear response when close to the target, sqrt response when we're further away rate_ef_desired = sqrt_controller(roll_angle_ef-_angle_ef_target.x, smoothing_gain, _accel_roll_max); // apply acceleration limit to feed forward roll rate _rate_ef_desired.x = constrain_float(rate_ef_desired, _rate_ef_desired.x-rate_change_limit, _rate_ef_desired.x+rate_change_limit); // update earth-frame roll angle target using desired roll rate update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); } else { // target roll and pitch to desired input roll and pitch _angle_ef_target.x = roll_angle_ef; angle_ef_error.x = wrap_180_cd_float(_angle_ef_target.x - _ahrs.roll_sensor); // set roll and pitch feed forward to zero _rate_ef_desired.x = 0; } // constrain earth-frame angle targets _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max); // if accel limiting and feed forward enabled if ((_accel_pitch_max > 0.0f) && _rate_bf_ff_enabled) { rate_change_limit = _accel_pitch_max * _dt; // calculate earth-frame feed forward pitch rate using linear response when close to the target, sqrt response when we're further away rate_ef_desired = sqrt_controller(pitch_angle_ef-_angle_ef_target.y, smoothing_gain, _accel_pitch_max); // apply acceleration limit to feed forward pitch rate _rate_ef_desired.y = constrain_float(rate_ef_desired, _rate_ef_desired.y-rate_change_limit, _rate_ef_desired.y+rate_change_limit); // update earth-frame pitch angle target using desired pitch rate update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); } else { // target roll and pitch to desired input roll and pitch _angle_ef_target.y = pitch_angle_ef; angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor); // set roll and pitch feed forward to zero _rate_ef_desired.y = 0; } // constrain earth-frame angle targets _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max); if (_accel_yaw_max > 0.0f) { // set earth-frame feed forward rate for yaw rate_change_limit = _accel_yaw_max * _dt; // update yaw rate target with acceleration limit _rate_ef_desired.z += constrain_float(yaw_rate_ef - _rate_ef_desired.z, -rate_change_limit, rate_change_limit); // calculate yaw target angle and angle error update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); } else { // set yaw feed forward to zero _rate_ef_desired.z = yaw_rate_ef; // calculate yaw target angle and angle error update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); } // convert earth-frame angle errors to body-frame angle errors frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); // add body frame rate feed forward if (_rate_bf_ff_enabled) { // convert earth-frame feed forward rates to body-frame feed forward rates frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired); _rate_bf_target += _rate_bf_desired; } else { // convert earth-frame feed forward rates to body-frame feed forward rates frame_conversion_ef_to_bf(Vector3f(0,0,_rate_ef_desired.z), _rate_bf_desired); _rate_bf_target += _rate_bf_desired; } // body-frame to motor outputs should be called separately }
// rate_bf_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all body frame) void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_rate_bf, float yaw_rate_bf) { Vector3f angle_ef_error; float rate_change, rate_change_limit; // update the rate feed forward with angular acceleration limits if (_accel_roll_max > 0.0f) { rate_change_limit = _accel_roll_max * _dt; rate_change = roll_rate_bf - _rate_bf_desired.x; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_bf_desired.x += rate_change; } else { _rate_bf_desired.x = roll_rate_bf; } // update the rate feed forward with angular acceleration limits if (_accel_pitch_max > 0.0f) { rate_change_limit = _accel_pitch_max * _dt; rate_change = pitch_rate_bf - _rate_bf_desired.y; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_bf_desired.y += rate_change; } else { _rate_bf_desired.y = pitch_rate_bf; } if (_accel_yaw_max > 0.0f) { rate_change_limit = _accel_yaw_max * _dt; rate_change = yaw_rate_bf - _rate_bf_desired.z; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_bf_desired.z += rate_change; } else { _rate_bf_desired.z = yaw_rate_bf; } // Update angle error if (labs(_ahrs.pitch_sensor)<_acro_angle_switch) { _acro_angle_switch = 6000; // convert body-frame rates to earth-frame rates frame_conversion_bf_to_ef(_rate_bf_desired, _rate_ef_desired); // update earth frame angle targets and errors update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX); update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX); update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_ACRO_OVERSHOOT_ANGLE_MAX); // convert earth-frame angle errors to body-frame angle errors frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); } else { _acro_angle_switch = 4500; integrate_bf_rate_error_to_angle_errors(); if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) { _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); } if (_angle_ef_target.y > 9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); _angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } if (_angle_ef_target.y < -9000.0f) { _angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f); _angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y); _angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f); } } // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); // body-frame rate commands added _rate_bf_target += _rate_bf_desired; }
// return a steering servo output from -1 to +1 given a // desired yaw rate in radians/sec. Positive yaw is to the right. float AR_AttitudeControl::get_steering_out_rate(float desired_rate, bool skid_steering, bool motor_limit_left, bool motor_limit_right, bool reversed) { // calculate dt const uint32_t now = AP_HAL::millis(); float dt = (now - _steer_turn_last_ms) / 1000.0f; if (_steer_turn_last_ms == 0 || dt > 0.1f) { dt = 0.0f; _steer_rate_pid.reset_filter(); } else { _steer_rate_pid.set_dt(dt); } _steer_turn_last_ms = now; // get speed forward float speed; if (!get_forward_speed(speed)) { // we expect caller will not try to control heading using rate control without a valid speed estimate // on failure to get speed we do not attempt to steer return 0.0f; } // only use positive speed. Use reverse flag instead of negative speeds. speed = fabsf(speed); // enforce minimum speed to stop oscillations when first starting to move bool low_speed = false; if (speed < AR_ATTCONTROL_STEER_SPEED_MIN) { low_speed = true; speed = AR_ATTCONTROL_STEER_SPEED_MIN; } // scaler to linearize output because turn rate increases as vehicle speed increases on non-skid steering vehicles float scaler = 1.0f; if (!skid_steering) { scaler = 1.0f / fabsf(speed); } // Calculate the steering rate error (rad/sec) and apply gain scaler // We do this in earth frame to allow for rover leaning over in hard corners float yaw_rate_earth = _ahrs.get_yaw_rate_earth(); // check if reversing if (reversed) { yaw_rate_earth *= -1.0f; } const float rate_error = (desired_rate - yaw_rate_earth) * scaler; // record desired rate for logging purposes only _steer_rate_pid.set_desired_rate(desired_rate); // pass error to PID controller _steer_rate_pid.set_input_filter_all(rate_error); // get p const float p = _steer_rate_pid.get_p(); // get i unless non-skid-steering rover at low speed or steering output has hit a limit float i = _steer_rate_pid.get_integrator(); if ((!low_speed || skid_steering) && ((is_negative(rate_error) && !motor_limit_left) || (is_positive(rate_error) && !motor_limit_right))) { i = _steer_rate_pid.get_i(); } // get d const float d = _steer_rate_pid.get_d(); // constrain and return final output return constrain_float(p + i + d, -1.0f, 1.0f); }
void Rover::update_current_mode(void) { switch (control_mode){ case AUTO: case RTL: set_reverse(false); calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); break; case GUIDED: set_reverse(false); if (rtl_complete || verify_RTL()) { // we have reached destination so stop where we are if (channel_throttle->get_servo_out() != g.throttle_min.get()) { gcs_send_mission_item_reached_message(0); } channel_throttle->set_servo_out(g.throttle_min.get()); channel_steer->set_servo_out(0); lateral_acceleration = 0; } else { calc_lateral_acceleration(); calc_nav_steer(); calc_throttle(g.speed_cruise); } break; case STEERING: { /* in steering mode we control lateral acceleration directly. We first calculate the maximum lateral acceleration at full steering lock for this speed. That is V^2/R where R is the radius of turn. We get the radius of turn from half the STEER2SRV_P. */ float max_g_force = ground_speed * ground_speed / steerController.get_turn_radius(); // constrain to user set TURN_MAX_G max_g_force = constrain_float(max_g_force, 0.1f, g.turn_max_g * GRAVITY_MSS); lateral_acceleration = max_g_force * (channel_steer->pwm_to_angle()/4500.0f); calc_nav_steer(); // and throttle gives speed in proportion to cruise speed, up // to 50% throttle, then uses nudging above that. float target_speed = channel_throttle->pwm_to_angle() * 0.01f * 2 * g.speed_cruise; set_reverse(target_speed < 0); if (in_reverse) { target_speed = constrain_float(target_speed, -g.speed_cruise, 0); } else { target_speed = constrain_float(target_speed, 0, g.speed_cruise); } calc_throttle(target_speed); break; } case LEARNING: case MANUAL: /* in both MANUAL and LEARNING we pass through the controls. Setting servo_out here actually doesn't matter, as we set the exact value in set_servos(), but it helps for logging */ channel_throttle->set_servo_out(channel_throttle->get_control_in()); channel_steer->set_servo_out(channel_steer->pwm_to_angle()); // mark us as in_reverse when using a negative throttle to // stop AHRS getting off set_reverse(channel_throttle->get_servo_out() < 0); break; case HOLD: // hold position - stop motors and center steering channel_throttle->set_servo_out(0); channel_steer->set_servo_out(0); set_reverse(false); break; case INITIALISING: break; } }
// Update rate_target_ang_vel using attitude_error_rot_vec_rad Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(Vector3f attitude_error_rot_vec_rad) { Vector3f rate_target_ang_vel; // Compute the roll angular velocity demand from the roll angle error if (_use_ff_and_input_shaping) { rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); }else{ rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x; } // Compute the pitch angular velocity demand from the roll angle error if (_use_ff_and_input_shaping) { rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt); }else{ rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y; } // Compute the yaw angular velocity demand from the roll angle error if (_use_ff_and_input_shaping) { rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt); }else{ rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z; } return rate_target_ang_vel; }
void AP_UAVCAN::do_cyclic(void) { if (_initialized) { auto *node = get_node(); const int error = node->spin(uavcan::MonotonicDuration::fromMSec(1)); if (error < 0) { hal.scheduler->delay_microseconds(1000); } else { if (rc_out_sem_take()) { if (_rco_armed) { bool repeat_send; // if we have any Servos in bitmask if (_servo_bm > 0) { uint8_t starting_servo = 0; do { repeat_send = false; uavcan::equipment::actuator::ArrayCommand msg; uint8_t i; // UAVCAN can hold maximum of 15 commands in one frame for (i = 0; starting_servo < UAVCAN_RCO_NUMBER && i < 15; starting_servo++) { uavcan::equipment::actuator::Command cmd; /* * Servo output uses a range of 1000-2000 PWM for scaling. * This converts output PWM from [1000:2000] range to [-1:1] range that * is passed to servo as unitless type via UAVCAN. * This approach allows for MIN/TRIM/MAX values to be used fully on * autopilot side and for servo it should have the setup to provide maximum * physically possible throws at [-1:1] limits. */ if (_rco_conf[starting_servo].active && ((((uint32_t) 1) << starting_servo) & _servo_bm)) { cmd.actuator_id = starting_servo + 1; // TODO: other types cmd.command_type = uavcan::equipment::actuator::Command::COMMAND_TYPE_UNITLESS; // TODO: failsafe, safety cmd.command_value = constrain_float(((float) _rco_conf[starting_servo].pulse - 1000.0) / 500.0 - 1.0, -1.0, 1.0); msg.commands.push_back(cmd); i++; } } if (i > 0) { act_out_array[_uavcan_i]->broadcast(msg); if (i == 15) { repeat_send = true; } } } while (repeat_send); } // if we have any ESC's in bitmask if (_esc_bm > 0) { static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max(); uavcan::equipment::esc::RawCommand esc_msg; uint8_t active_esc_num = 0, max_esc_num = 0; uint8_t k = 0; // find out how many esc we have enabled and if they are active at all for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) { if ((((uint32_t) 1) << i) & _esc_bm) { max_esc_num = i + 1; if (_rco_conf[i].active) { active_esc_num++; } } } // if at least one is active (update) we need to send to all if (active_esc_num > 0) { k = 0; for (uint8_t i = 0; i < max_esc_num && k < 20; i++) { uavcan::equipment::actuator::Command cmd; if ((((uint32_t) 1) << i) & _esc_bm) { // TODO: ESC negative scaling for reverse thrust and reverse rotation float scaled = cmd_max * (hal.rcout->scale_esc_to_unity(_rco_conf[i].pulse) + 1.0) / 2.0; scaled = constrain_float(scaled, 0, cmd_max); esc_msg.cmd.push_back(static_cast<int>(scaled)); } else { esc_msg.cmd.push_back(static_cast<unsigned>(0)); } k++; } esc_raw[_uavcan_i]->broadcast(esc_msg); } } } for (uint8_t i = 0; i < UAVCAN_RCO_NUMBER; i++) { // mark as transmitted _rco_conf[i].active = false; } rc_out_sem_give(); } } } else { hal.scheduler->delay_microseconds(1000); } }
void Replay::loop() { while (true) { char type[5]; if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) { if (!hal.util->get_soft_armed()) { hal.util->set_soft_armed(true); ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis()); } } if (!logreader.update(type)) { ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f); fclose(plotf); break; } read_sensors(type); if (streq(type,"ATT")) { Vector3f ekf_euler; Vector3f velNED; Vector3f posNED; Vector3f gyroBias; float accelWeighting; float accelZBias1; float accelZBias2; Vector3f windVel; Vector3f magNED; Vector3f magXYZ; Vector3f DCM_attitude; Vector3f ekf_relpos; Vector3f velInnov; Vector3f posInnov; Vector3f magInnov; float tasInnov; float velVar; float posVar; float hgtVar; Vector3f magVar; float tasVar; Vector2f offset; uint8_t faultStatus; const Matrix3f &dcm_matrix = _vehicle.ahrs.AP_AHRS_DCM::get_dcm_matrix(); dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z); _vehicle.EKF.getEulerAngles(ekf_euler); _vehicle.EKF.getVelNED(velNED); _vehicle.EKF.getPosNED(posNED); _vehicle.EKF.getGyroBias(gyroBias); _vehicle.EKF.getIMU1Weighting(accelWeighting); _vehicle.EKF.getAccelZBias(accelZBias1, accelZBias2); _vehicle.EKF.getWind(windVel); _vehicle.EKF.getMagNED(magNED); _vehicle.EKF.getMagXYZ(magXYZ); _vehicle.EKF.getInnovations(velInnov, posInnov, magInnov, tasInnov); _vehicle.EKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); _vehicle.EKF.getFilterFaults(faultStatus); _vehicle.EKF.getPosNED(ekf_relpos); Vector3f inav_pos = _vehicle.inertial_nav.get_position() * 0.01f; float temp = degrees(ekf_euler.z); if (temp < 0.0f) temp = temp + 360.0f; fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n", AP_HAL::millis() * 0.001f, logreader.get_sim_attitude().x, logreader.get_sim_attitude().y, logreader.get_sim_attitude().z, _vehicle.barometer.get_altitude(), logreader.get_attitude().x, logreader.get_attitude().y, wrap_180_cd(logreader.get_attitude().z*100)*0.01f, logreader.get_inavpos().x, logreader.get_inavpos().y, logreader.get_relalt(), logreader.get_ahr2_attitude().x, logreader.get_ahr2_attitude().y, wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f, degrees(DCM_attitude.x), degrees(DCM_attitude.y), degrees(DCM_attitude.z), degrees(ekf_euler.x), degrees(ekf_euler.y), degrees(ekf_euler.z), inav_pos.x, inav_pos.y, inav_pos.z, ekf_relpos.x, ekf_relpos.y, -ekf_relpos.z); fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", AP_HAL::millis() * 0.001f, degrees(ekf_euler.x), degrees(ekf_euler.y), temp, velNED.x, velNED.y, velNED.z, posNED.x, posNED.y, posNED.z, 60*degrees(gyroBias.x), 60*degrees(gyroBias.y), 60*degrees(gyroBias.z), windVel.x, windVel.y, magNED.x, magNED.y, magNED.z, magXYZ.x, magXYZ.y, magXYZ.z, logreader.get_attitude().x, logreader.get_attitude().y, logreader.get_attitude().z); // define messages for EKF1 data packet int16_t roll = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg) int16_t pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg) uint16_t yaw = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg) float velN = (float)(velNED.x); // velocity North (m/s) float velE = (float)(velNED.y); // velocity East (m/s) float velD = (float)(velNED.z); // velocity Down (m/s) float posN = (float)(posNED.x); // metres North float posE = (float)(posNED.y); // metres East float posD = (float)(posNED.z); // metres Down float gyrX = (float)(6000*degrees(gyroBias.x)); // centi-deg/min float gyrY = (float)(6000*degrees(gyroBias.y)); // centi-deg/min float gyrZ = (float)(6000*degrees(gyroBias.z)); // centi-deg/min // print EKF1 data packet fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %.0f %.0f %.0f\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), roll, pitch, yaw, velN, velE, velD, posN, posE, posD, gyrX, gyrY, gyrZ); // define messages for EKF2 data packet int8_t accWeight = (int8_t)(100*accelWeighting); int8_t acc1 = (int8_t)(100*accelZBias1); int8_t acc2 = (int8_t)(100*accelZBias2); int16_t windN = (int16_t)(100*windVel.x); int16_t windE = (int16_t)(100*windVel.y); int16_t magN = (int16_t)(magNED.x); int16_t magE = (int16_t)(magNED.y); int16_t magD = (int16_t)(magNED.z); int16_t magX = (int16_t)(magXYZ.x); int16_t magY = (int16_t)(magXYZ.y); int16_t magZ = (int16_t)(magXYZ.z); // print EKF2 data packet fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), accWeight, acc1, acc2, windN, windE, magN, magE, magD, magX, magY, magZ); // define messages for EKF3 data packet int16_t innovVN = (int16_t)(100*velInnov.x); int16_t innovVE = (int16_t)(100*velInnov.y); int16_t innovVD = (int16_t)(100*velInnov.z); int16_t innovPN = (int16_t)(100*posInnov.x); int16_t innovPE = (int16_t)(100*posInnov.y); int16_t innovPD = (int16_t)(100*posInnov.z); int16_t innovMX = (int16_t)(magInnov.x); int16_t innovMY = (int16_t)(magInnov.y); int16_t innovMZ = (int16_t)(magInnov.z); int16_t innovVT = (int16_t)(100*tasInnov); // print EKF3 data packet fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), innovVN, innovVE, innovVD, innovPN, innovPE, innovPD, innovMX, innovMY, innovMZ, innovVT); // define messages for EKF4 data packet int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX)); int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX)); int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX)); int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX)); int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX)); int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX)); // print EKF4 data packet fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, (unsigned)AP_HAL::millis(), (int)sqrtvarV, (int)sqrtvarP, (int)sqrtvarH, (int)sqrtvarMX, (int)sqrtvarMY, (int)sqrtvarMZ, (int)sqrtvarVT, (int)offsetNorth, (int)offsetEast, (int)faultStatus); } } flush_dataflash(); if (check_solution) { report_checks(); } exit(0); }
// loiter_run - runs the loiter controller // should be called at 100hz or more void Copter::ModeLoiter::run() { LoiterModeState loiter_state; float target_yaw_rate = 0.0f; float target_climb_rate = 0.0f; float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); pos_control->set_accel_z(g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe if (!copter.failsafe.radio) { // apply SIMPLE mode transform to pilot inputs update_simple_mode(); // process pilot's roll and pitch input wp_nav->set_pilot_desired_acceleration(channel_roll->get_control_in(), channel_pitch->get_control_in()); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // get pilot desired climb rate target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason wp_nav->clear_pilot_desired_acceleration(); } // relax loiter target if we might be landed if (ap.land_complete_maybe) { wp_nav->loiter_soften_for_landing(); } // Loiter State Machine Determination if (!motors->armed() || !motors->get_interlock()) { loiter_state = Loiter_MotorStopped; } else if (takeoff_state.running || takeoff_triggered(target_climb_rate)) { loiter_state = Loiter_Takeoff; } else if (!ap.auto_armed || ap.land_complete) { loiter_state = Loiter_Landed; } else { loiter_state = Loiter_Flying; } // Loiter State Machine switch (loiter_state) { case Loiter_MotorStopped: motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN); #if FRAME_CONFIG == HELI_FRAME // force descent rate and call position controller pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); #else wp_nav->init_loiter_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero #endif wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); pos_control->update_z_controller(); break; case Loiter_Takeoff: // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // initiate take-off if (!takeoff_state.running) { takeoff_timer_start(constrain_float(g.pilot_takeoff_alt,0.0f,1000.0f)); // indicate we are taking off set_land_complete(false); // clear i term when we're taking off set_throttle_takeoff(); } // get takeoff adjusted pilot and takeoff climb rates takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); pos_control->update_z_controller(); break; case Loiter_Landed: // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } wp_nav->init_loiter_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain()); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; case Loiter_Flying: // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); #if PRECISION_LANDING == ENABLED if (do_precision_loiter()) { precision_loiter_xy(); } #endif // run loiter controller wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); // adjust climb rate using rangefinder if (copter.rangefinder_alt_ok()) { // if rangefinder is ok, use surface tracking target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt); } // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); pos_control->update_z_controller(); break; } }
void AP_TECS::update_pitch_throttle(int32_t hgt_dem_cm, int32_t EAS_dem_cm, enum FlightStage flight_stage, bool is_doing_auto_land, float distance_beyond_land_wp, int32_t ptchMinCO_cd, int16_t throttle_nudge, float hgt_afe, float load_factor) { // Calculate time in seconds since last update uint32_t now = AP_HAL::micros(); _DT = MAX((now - _update_pitch_throttle_last_usec), 0U) * 1.0e-6f; _update_pitch_throttle_last_usec = now; _flags.is_doing_auto_land = is_doing_auto_land; _distance_beyond_land_wp = distance_beyond_land_wp; _flight_stage = flight_stage; // Convert inputs _hgt_dem = hgt_dem_cm * 0.01f; _EAS_dem = EAS_dem_cm * 0.01f; // Update the speed estimate using a 2nd order complementary filter _update_speed(load_factor); if (aparm.takeoff_throttle_max != 0 && (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT)) { _THRmaxf = aparm.takeoff_throttle_max * 0.01f; } else { _THRmaxf = aparm.throttle_max * 0.01f; } _THRminf = aparm.throttle_min * 0.01f; // work out the maximum and minimum pitch // if TECS_PITCH_{MAX,MIN} isn't set then use // LIM_PITCH_{MAX,MIN}. Don't allow TECS_PITCH_{MAX,MIN} to be // larger than LIM_PITCH_{MAX,MIN} if (_pitch_max <= 0) { _PITCHmaxf = aparm.pitch_limit_max_cd * 0.01f; } else { _PITCHmaxf = MIN(_pitch_max, aparm.pitch_limit_max_cd * 0.01f); } // apply temporary pitch limit and clear _PITCHmaxf = constrain_float(_PITCHmaxf, -90, _pitch_max_limit); _pitch_max_limit = 90; if (_pitch_min >= 0) { _PITCHminf = aparm.pitch_limit_min_cd * 0.01f; } else { _PITCHminf = MAX(_pitch_min, aparm.pitch_limit_min_cd * 0.01f); } if (flight_stage == FLIGHT_LAND_FINAL) { // in flare use min pitch from LAND_PITCH_CD _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); // and use max pitch from TECS_LAND_PMAX if (_land_pitch_max != 0) { _PITCHmaxf = MIN(_PITCHmaxf, _land_pitch_max); } // and allow zero throttle _THRminf = 0; } else if ((flight_stage == FLIGHT_LAND_APPROACH || flight_stage == FLIGHT_LAND_PREFLARE) && (-_climb_rate) > _land_sink) { // constrain the pitch in landing as we get close to the flare // point. Use a simple linear limit from 15 meters after the // landing point float time_to_flare = (- hgt_afe / _climb_rate) - aparm.land_flare_sec; if (time_to_flare < 0) { // we should be flaring already _PITCHminf = MAX(_PITCHminf, aparm.land_pitch_cd * 0.01f); } else if (time_to_flare < timeConstant()*2) { // smoothly move the min pitch to the flare min pitch over // twice the time constant float p = time_to_flare/(2*timeConstant()); float pitch_limit_cd = p*aparm.pitch_limit_min_cd + (1-p)*aparm.land_pitch_cd; #if 0 ::printf("ttf=%.1f hgt_afe=%.1f _PITCHminf=%.1f pitch_limit=%.1f climb=%.1f\n", time_to_flare, hgt_afe, _PITCHminf, pitch_limit_cd*0.01f, _climb_rate); #endif _PITCHminf = MAX(_PITCHminf, pitch_limit_cd*0.01f); } } // convert to radians _PITCHmaxf = radians(_PITCHmaxf); _PITCHminf = radians(_PITCHminf); // initialise selected states and variables if DT > 1 second or in climbout _initialise_states(ptchMinCO_cd, hgt_afe); // Calculate Specific Total Energy Rate Limits _update_STE_rate_lim(); // Calculate the speed demand _update_speed_demand(); // Calculate the height demand _update_height_demand(); // Detect underspeed condition _detect_underspeed(); // Calculate specific energy quantitiues _update_energies(); // Calculate throttle demand - use simple pitch to throttle if no airspeed sensor if (_ahrs.airspeed_sensor_enabled()) { _update_throttle(); } else { _update_throttle_option(throttle_nudge); } // Detect bad descent due to demanded airspeed being too high _detect_bad_descent(); // Calculate pitch demand _update_pitch(); // Write internal variables to the log_tuning structure. This // structure will be logged in dataflash at 10Hz log_tuning.hgt_dem = _hgt_dem_adj; log_tuning.hgt = _height; log_tuning.dhgt_dem = _hgt_rate_dem; log_tuning.dhgt = _climb_rate; log_tuning.spd_dem = _TAS_dem_adj; log_tuning.spd = _integ5_state; log_tuning.dspd = _vel_dot; log_tuning.ithr = _integ6_state; log_tuning.iptch = _integ7_state; log_tuning.thr = _throttle_dem; log_tuning.ptch = _pitch_dem; log_tuning.dspd_dem = _TAS_rate_dem; log_tuning.flags = _flags_byte; log_tuning.time_us = AP_HAL::micros64(); }
void AP_TECS::_update_pitch(void) { // Calculate Speed/Height Control Weighting // This is used to determine how the pitch control prioritises speed and height control // A weighting of 1 provides equal priority (this is the normal mode of operation) // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed // rises above the demanded value, the pitch angle will be increased by the TECS controller. float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); if (!_ahrs.airspeed_sensor_enabled()) { SKE_weighting = 0.0f; } else if ( _flags.underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { SKE_weighting = 2.0f; } else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_PREFLARE || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { if (_spdWeightLand < 0) { // use sliding scale from normal weight down to zero at landing float scaled_weight = _spdWeight * (1.0f - _path_proportion); SKE_weighting = constrain_float(scaled_weight, 0.0f, 2.0f); } else { SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f); } } log_tuning.speed_weight = SKE_weighting; float SPE_weighting = 2.0f - SKE_weighting; // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded float integ7_input = SEB_error * _get_i_gain(); if (_pitch_dem > _PITCHmaxf) { integ7_input = MIN(integ7_input, _PITCHmaxf - _pitch_dem); } else if (_pitch_dem < _PITCHminf) { integ7_input = MAX(integ7_input, _PITCHminf - _pitch_dem); } _integ7_state = _integ7_state + integ7_input * _DT; #if 0 if (_flight_stage == FLIGHT_LAND_FINAL && fabsf(_climb_rate) > 0.2f) { ::printf("_hgt_rate_dem=%.1f _hgt_dem_adj=%.1f climb=%.1f _flare_counter=%u _pitch_dem=%.1f SEB_dem=%.2f SEBdot_dem=%.2f SEB_error=%.2f SEBdot_error=%.2f\n", _hgt_rate_dem, _hgt_dem_adj, _climb_rate, _flare_counter, degrees(_pitch_dem), SEB_dem, SEBdot_dem, SEB_error, SEBdot_error); } #endif // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. // During flare a different damping gain is used float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS); float temp = SEB_error + SEBdot_dem * timeConstant(); float pitch_damp = _ptchDamp; if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { pitch_damp = _landDamp; } else if (!is_zero(_land_pitch_damp) && is_on_land_approach(false)) { pitch_damp = _land_pitch_damp; } temp += SEBdot_error * pitch_damp; if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF || _flight_stage == AP_TECS::FLIGHT_LAND_ABORT) { temp += _PITCHminf * gainInv; } _integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; // Constrain pitch demand _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); // Rate limit the pitch demand to comply with specified vertical // acceleration limit float ptchRateIncr = _DT * _vertAccLim / _integ5_state; if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { _pitch_dem = _last_pitch_dem + ptchRateIncr; } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { _pitch_dem = _last_pitch_dem - ptchRateIncr; } // re-constrain pitch demand _pitch_dem = constrain_float(_pitch_dem, _PITCHminf, _PITCHmaxf); _last_pitch_dem = _pitch_dem; }
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; } }
/// advance_target_along_track - move target location along track from origin to destination void AC_WPNav::advance_target_along_track(float dt) { float track_covered; Vector3f track_error; float track_desired_max; float track_desired_temp = _track_desired; float track_extra_max; // get current location Vector3f curr_pos = _inav->get_position(); Vector3f curr_delta = curr_pos - _origin; // calculate how far along the track we are track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z; Vector3f track_covered_pos = _pos_delta_unit * track_covered; track_error = curr_delta - track_covered_pos; // calculate the horizontal error float track_error_xy = safe_sqrt(track_error.x*track_error.x + track_error.y*track_error.y); // calculate the vertical error float track_error_z = fabsf(track_error.z); // calculate how far along the track we could move the intermediate target before reaching the end of the leash track_extra_max = min(_track_leash_length*(_wp_leash_z-track_error_z)/_wp_leash_z, _track_leash_length*(_wp_leash_xy-track_error_xy)/_wp_leash_xy); if(track_extra_max <0) { track_desired_max = track_covered; }else{ track_desired_max = track_covered + track_extra_max; } // get current velocity const Vector3f &curr_vel = _inav->get_velocity(); // get speed along track float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; // calculate point at which velocity switches from linear to sqrt float linear_velocity = _wp_speed_cms; float kP = _pid_pos_lat->kP(); if (kP >= 0.0f) { // avoid divide by zero linear_velocity = _track_accel/kP; } // let the limited_speed_xy_cms be some range above or below current velocity along track if (speed_along_track < -linear_velocity) { // we are travelling fast in the opposite direction of travel to the waypoint so do not move the intermediate point _limited_speed_xy_cms = 0; }else{ // increase intermediate target point's velocity if not yet at target speed (we will limit it below) if(dt > 0) { if(track_desired_max > _track_desired) { _limited_speed_xy_cms += 2.0f * _track_accel * dt; }else{ // do nothing, velocity stays constant _track_desired = track_desired_max; } } // do not go over top speed if(_limited_speed_xy_cms > _track_speed) { _limited_speed_xy_cms = _track_speed; } // if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity if (fabsf(speed_along_track) < linear_velocity) { _limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity); } } // advance the current target track_desired_temp += _limited_speed_xy_cms * dt; // do not let desired point go past the end of the segment track_desired_temp = constrain_float(track_desired_temp, 0, _track_length); _track_desired = max(_track_desired, track_desired_temp); // recalculate the desired position _target = _origin + _pos_delta_unit * _track_desired; // check if we've reached the waypoint if( !_flags.reached_destination ) { if( _track_desired >= _track_length ) { // "fast" waypoints are complete once the intermediate point reaches the destination if (_flags.fast_waypoint) { _flags.reached_destination = true; }else{ // regular waypoints also require the copter to be within the waypoint radius Vector3f dist_to_dest = curr_pos - _destination; if( dist_to_dest.length() <= _wp_radius_cm ) { _flags.reached_destination = true; } } } } }
// set steering as a value from -4500 to +4500 // apply_scaling should be set to false for manual modes where // no scaling by speed or angle should be performed void AP_MotorsUGV::set_steering(float steering, bool apply_scaling) { _steering = constrain_float(steering, -4500.0f, 4500.0f); _scale_steering = apply_scaling; }
bool Compass::consistent() const { Vector3f primary_mag_field = get_field(); Vector3f primary_mag_field_norm; if (!primary_mag_field.is_zero()) { primary_mag_field_norm = primary_mag_field.normalized(); } else { return false; } Vector2f primary_mag_field_xy = Vector2f(primary_mag_field.x,primary_mag_field.y); Vector2f primary_mag_field_xy_norm; if (!primary_mag_field_xy.is_zero()) { primary_mag_field_xy_norm = primary_mag_field_xy.normalized(); } else { return false; } for (uint8_t i=0; i<get_count(); i++) { if (use_for_yaw(i)) { Vector3f mag_field = get_field(i); Vector3f mag_field_norm; if (!mag_field.is_zero()) { mag_field_norm = mag_field.normalized(); } else { return false; } Vector2f mag_field_xy = Vector2f(mag_field.x,mag_field.y); Vector2f mag_field_xy_norm; if (!mag_field_xy.is_zero()) { mag_field_xy_norm = mag_field_xy.normalized(); } else { return false; } float xyz_ang_diff = acosf(constrain_float(mag_field_norm * primary_mag_field_norm,-1.0f,1.0f)); float xy_ang_diff = acosf(constrain_float(mag_field_xy_norm*primary_mag_field_xy_norm,-1.0f,1.0f)); float xy_len_diff = (primary_mag_field_xy-mag_field_xy).length(); // check for gross misalignment on all axes bool xyz_ang_diff_large = xyz_ang_diff > AP_COMPASS_MAX_XYZ_ANG_DIFF; // check for an unacceptable angle difference on the xy plane bool xy_ang_diff_large = xy_ang_diff > AP_COMPASS_MAX_XY_ANG_DIFF; // check for an unacceptable length difference on the xy plane bool xy_length_diff_large = xy_len_diff > AP_COMPASS_MAX_XY_LENGTH_DIFF; // check for inconsistency in the XY plane if (xyz_ang_diff_large || xy_ang_diff_large || xy_length_diff_large) { return false; } } } return true; }
// sanity check parameters void AP_MotorsUGV::sanity_check_parameters() { _throttle_min = constrain_int16(_throttle_min, 0, 20); _throttle_max = constrain_int16(_throttle_max, 30, 100); _vector_throttle_base = constrain_float(_vector_throttle_base, 0.0f, 100.0f); }
// return a throttle output from -1 to +1 given a desired speed in m/s (use negative speeds to travel backwards) // motor_limit should be true if motors have hit their upper or lower limits // cruise speed should be in m/s, cruise throttle should be a number from -1 to +1 float AR_AttitudeControl::get_throttle_out_speed(float desired_speed, bool motor_limit_low, bool motor_limit_high, float cruise_speed, float cruise_throttle) { // get speed forward float speed; if (!get_forward_speed(speed)) { // we expect caller will not try to control heading using rate control without a valid speed estimate // on failure to get speed we do not attempt to steer return 0.0f; } // calculate dt const uint32_t now = AP_HAL::millis(); float dt = (now - _speed_last_ms) / 1000.0f; if (_speed_last_ms == 0 || dt > 0.1f) { dt = 0.0f; _throttle_speed_pid.reset_filter(); } _speed_last_ms = now; // acceleration limit desired speed if (is_positive(_throttle_accel_max)) { // reset desired speed to current speed on first iteration if (!is_positive(dt)) { desired_speed = speed; } else { const float speed_change_max = _throttle_accel_max * dt; desired_speed = constrain_float(desired_speed, _desired_speed - speed_change_max, _desired_speed + speed_change_max); } } // record desired speed for next iteration _desired_speed = desired_speed; // calculate speed error and pass to PID controller const float speed_error = desired_speed - speed; _throttle_speed_pid.set_input_filter_all(speed_error); // record desired speed for logging purposes only _throttle_speed_pid.set_desired_rate(desired_speed); // get p const float p = _throttle_speed_pid.get_p(); // get i unless moving at low speed or motors have hit a limit float i = _throttle_speed_pid.get_integrator(); if ((is_negative(speed_error) && !motor_limit_low && !_throttle_limit_low) || (is_positive(speed_error) && !motor_limit_high && !_throttle_limit_high)) { i = _throttle_speed_pid.get_i(); } // get d const float d = _throttle_speed_pid.get_d(); // calculate base throttle (protect against divide by zero) float throttle_base = 0.0f; if (is_positive(cruise_speed) && is_positive(cruise_throttle)) { throttle_base = desired_speed * (cruise_throttle / cruise_speed); } // calculate final output float throttle_out = (p+i+d+throttle_base); // clear local limit flags used to stop i-term build-up as we stop reversed outputs going to motors _throttle_limit_low = false; _throttle_limit_high = false; // protect against reverse output being sent to the motors unless braking has been enabled if (!_brake_enable) { // if both desired speed and actual speed are positive, do not allow negative values if ((desired_speed >= 0.0f) && (throttle_out <= 0.0f)) { throttle_out = 0.0f; _throttle_limit_low = true; } if ((desired_speed <= 0.0f) && (throttle_out >= 0.0f)) { throttle_out = 0.0f; _throttle_limit_high = true; } } // final output throttle in range -1 to 1 return throttle_out; }
// Set inertial navigation aiding mode void NavEKF2_core::setAidingMode() { // Save the previous status so we can detect when it has changed PV_AidingModePrev = PV_AidingMode; // Determine if we should change aiding mode switch (PV_AidingMode) { case AID_NONE: { // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete // and IMU gyro bias estimates have stabilised bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus(); // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present // GPS aiding is the preferred option unless excluded by the user bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable && !gpsInhibit); bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable; bool canUseExtNav = readyToUseExtNav(); if(canUseGPS || canUseRangeBeacon || canUseExtNav) { PV_AidingMode = AID_ABSOLUTE; } else if (optFlowDataPresent() && filterIsStable) { PV_AidingMode = AID_RELATIVE; } } break; case AID_RELATIVE: { // Check if the optical flow sensor has timed out bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000); // Check if the fusion has timed out (flow measurements have been rejected for too long) bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000); // Enable switch to absolute position mode if GPS is available // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding if((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit) { PV_AidingMode = AID_ABSOLUTE; } else if (flowSensorTimeout || flowFusionTimeout) { PV_AidingMode = AID_NONE; } } break; case AID_ABSOLUTE: { // Find the minimum time without data required to trigger any check uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms)); // Check if optical flow data is being used bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms); // Check if airspeed data is being used bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms); // Check if range beacon data is being used bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms); // Check if GPS is being used bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms); bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms); // Check if attitude drift has been constrained by a measurement source bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed; // check if velocity drift has been constrained by a measurement source bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed; // check if position drift has been constrained by a measurement source bool posAiding = posUsed || rngBcnUsed; // Check if the loss of attitude aiding has become critical bool attAidLossCritical = false; if (!attAiding) { attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) && (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms); } // Check if the loss of position accuracy has become critical bool posAidLossCritical = false; if (!posAiding ) { uint16_t maxLossTime_ms; if (!velAiding) { maxLossTime_ms = frontend->posRetryTimeNoVel_ms; } else { maxLossTime_ms = frontend->posRetryTimeUseVel_ms; } posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) && (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms); } if (attAidLossCritical) { // if the loss of attitude data is critical, then put the filter into a constant position mode PV_AidingMode = AID_NONE; posTimeout = true; velTimeout = true; rngBcnTimeout = true; tasTimeout = true; gpsNotAvailable = true; } else if (posAidLossCritical) { // if the loss of position is critical, declare all sources of position aiding as being timed out posTimeout = true; velTimeout = true; rngBcnTimeout = true; gpsNotAvailable = true; } break; } } // check to see if we are starting or stopping aiding and set states and modes as required if (PV_AidingMode != PV_AidingModePrev) { // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped. switch (PV_AidingMode) { case AID_NONE: // We have ceased aiding gcs().send_text(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index); // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors posTimeout = true; velTimeout = true; // Reset the normalised innovation to avoid false failing bad fusion tests velTestRatio = 0.0f; posTestRatio = 0.0f; // store the current position to be used to keep reporting the last known position lastKnownPositionNE.x = stateStruct.position.x; lastKnownPositionNE.y = stateStruct.position.y; // initialise filtered altitude used to provide a takeoff reference to current baro on disarm // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used meaHgtAtTakeOff = baroDataDelayed.hgt; // reset the vertical position state to faster recover from baro errors experienced during touchdown stateStruct.position.z = -meaHgtAtTakeOff; break; case AID_RELATIVE: // We have commenced aiding, but GPS usage has been prohibited so use optical flow only gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index); posTimeout = true; velTimeout = true; // Reset the last valid flow measurement time flowValidMeaTime_ms = imuSampleTime_ms; // Reset the last valid flow fusion time prevFlowFuseTime_ms = imuSampleTime_ms; break; case AID_ABSOLUTE: { bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && !gpsInhibit); bool canUseRangeBeacon = readyToUseRangeBeacon(); bool canUseExtNav = readyToUseExtNav(); // We have commenced aiding and GPS usage is allowed if (canUseGPS) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index); } posTimeout = false; velTimeout = false; // We have commenced aiding and range beacon usage is allowed if (canUseRangeBeacon) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index); gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)receiverPos.x,(double)receiverPos.y); gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset); } // We have commenced aiding and external nav usage is allowed if (canUseExtNav) { gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index); gcs().send_text(MAV_SEVERITY_INFO, "EKF2 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z); // handle yaw reset as special case extNavYawResetRequest = true; controlMagYawReset(); // handle height reset as special case hgtMea = -extNavDataDelayed.pos.z; posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f)); ResetHeight(); } // reset the last fusion accepted times to prevent unwanted activation of timeout logic lastPosPassTime_ms = imuSampleTime_ms; lastVelPassTime_ms = imuSampleTime_ms; lastRngBcnPassTime_ms = imuSampleTime_ms; } break; } // Always reset the position and velocity when changing mode ResetVelocity(); ResetPosition(); } }
/* Function returns an equivalent elevator deflection in centi-degrees in the range from -4500 to 4500 A positive demand is up Inputs are: 1) demanded pitch rate in degrees/second 2) control gain scaler = scaling_speed / aspeed 3) boolean which is true when stabilise mode is active 4) minimum FBW airspeed (metres/sec) 5) maximum FBW airspeed (metres/sec) */ int32_t AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool disable_integrator, float aspeed) { uint32_t tnow = hal.scheduler->millis(); uint32_t dt = tnow - _last_t; if (_last_t == 0 || dt > 1000) { dt = 0; } _last_t = tnow; float delta_time = (float)dt * 0.001f; // Get body rate vector (radians/sec) float omega_y = _ahrs.get_gyro().y; // Calculate the pitch rate error (deg/sec) and scale float achieved_rate = ToDeg(omega_y); float rate_error = (desired_rate - achieved_rate) * scaler; // Multiply pitch rate error by _ki_rate and integrate // Scaler is applied before integrator so that integrator state relates directly to elevator deflection // This means elevator trim offset doesn't change as the value of scaler changes with airspeed // Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs if (!disable_integrator && gains.I > 0) { float ki_rate = gains.I * gains.tau; //only integrate if gain and time step are positive and airspeed above min value. if (dt > 0 && aspeed > 0.5f*float(aparm.airspeed_min)) { float integrator_delta = rate_error * ki_rate * delta_time * scaler; if (_last_out < -45) { // prevent the integrator from increasing if surface defln demand is above the upper limit integrator_delta = max(integrator_delta , 0); } else if (_last_out > 45) { // prevent the integrator from decreasing if surface defln demand is below the lower limit integrator_delta = min(integrator_delta , 0); } _integrator += integrator_delta; } } else { _integrator = 0; } // Scale the integration limit float intLimScaled = gains.imax * 0.01f; // Constrain the integrator state _integrator = constrain_float(_integrator, -intLimScaled, intLimScaled); // Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law // No conversion is required for K_D float kp_ff = max((gains.P - gains.I * gains.tau) * gains.tau - gains.D , 0) / _ahrs.get_EAS2TAS(); // Calculate the demanded control surface deflection // Note the scaler is applied again. We want a 1/speed scaler applied to the feed-forward // path, but want a 1/speed^2 scaler applied to the rate error path. // This is because acceleration scales with speed^2, but rate scales with speed. _last_out = ( (rate_error * gains.D) + (desired_rate * kp_ff) ) * scaler; if (autotune.running && aspeed > aparm.airspeed_min) { // let autotune have a go at the values // Note that we don't pass the integrator component so we get // a better idea of how much the base PD controller // contributed autotune.update(desired_rate, achieved_rate, _last_out); // set down rate to rate up when auto-tuning _max_rate_neg.set_and_save_ifchanged(gains.rmax); } _last_out += _integrator; // Convert to centi-degrees and constrain return constrain_float(_last_out * 100, -4500, 4500); }
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to // avoid unnecessary operations void NavEKF2_core::setWindMagStateLearningMode() { // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE); if (!inhibitWindStates && setWindInhibit) { inhibitWindStates = true; } else if (inhibitWindStates && !setWindInhibit) { inhibitWindStates = false; // set states and variances if (yawAlignComplete && useAirspeed()) { // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading // which assumes the vehicle has launched into the wind Vector3f tempEuler; stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); float windSpeed = sqrtf(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas; stateStruct.wind_vel.x = windSpeed * cosf(tempEuler.z); stateStruct.wind_vel.y = windSpeed * sinf(tempEuler.z); // set the wind sate variances to the measurement uncertainty for (uint8_t index=22; index<=23; index++) { P[index][index] = sq(constrain_float(frontend->_easNoise, 0.5f, 5.0f) * constrain_float(_ahrs->get_EAS2TAS(), 0.9f, 10.0f)); } } else { // set the variances using a typical wind speed for (uint8_t index=22; index<=23; index++) { P[index][index] = sq(5.0f); } } } // determine if the vehicle is manoevring if (accNavMagHoriz > 0.5f) { manoeuvring = true; } else { manoeuvring = false; } // Determine if learning of magnetic field states has been requested by the user uint8_t magCal = effective_magCal(); bool magCalRequested = ((magCal == 0) && inFlight) || // when flying ((magCal == 1) && manoeuvring) || // when manoeuvring ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete (magCal == 4); // all the time // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user, // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user) bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4); // Inhibit the magnetic field calibration if not requested or denied bool setMagInhibit = !magCalRequested || magCalDenied; if (!inhibitMagStates && setMagInhibit) { inhibitMagStates = true; } else if (inhibitMagStates && !setMagInhibit) { inhibitMagStates = false; if (magFieldLearned) { // if we have already learned the field states, then retain the learned variances P[16][16] = earthMagFieldVar.x; P[17][17] = earthMagFieldVar.y; P[18][18] = earthMagFieldVar.z; P[19][19] = bodyMagFieldVar.x; P[20][20] = bodyMagFieldVar.y; P[21][21] = bodyMagFieldVar.z; } else { // set the variances equal to the observation variances for (uint8_t index=18; index<=21; index++) { P[index][index] = sq(frontend->_magNoise); } // set the NE earth magnetic field states using the published declination // and set the corresponding variances and covariances alignMagStateDeclination(); } // request a reset of the yaw and magnetic field states if not done before if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) { magYawResetRequest = true; } } // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed // because we want it re-done for each takeoff if (onGround) { finalInflightYawInit = false; finalInflightMagInit = false; } // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations // if we are not using those states if (inhibitMagStates && inhibitWindStates) { stateIndexLim = 15; } else if (inhibitWindStates) { stateIndexLim = 21; } else { stateIndexLim = 23; } }
// rate_ef_roll_pitch_yaw - attempts to maintain a roll, pitch and yaw rate (all earth frame) void AC_AttitudeControl::rate_ef_roll_pitch_yaw(float roll_rate_ef, float pitch_rate_ef, float yaw_rate_ef) { Vector3f angle_ef_error; float rate_change_limit, rate_change; if (_accel_roll_max > 0.0f) { rate_change_limit = _accel_roll_max * _dt; // update feed forward roll rate after checking it is within acceleration limits rate_change = roll_rate_ef - _rate_ef_desired.x; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_ef_desired.x += rate_change; } else { _rate_ef_desired.x = roll_rate_ef; } if (_accel_pitch_max > 0.0f) { rate_change_limit = _accel_pitch_max * _dt; // update feed forward pitch rate after checking it is within acceleration limits rate_change = pitch_rate_ef - _rate_ef_desired.y; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_ef_desired.y += rate_change; } else { _rate_ef_desired.y = pitch_rate_ef; } if (_accel_yaw_max > 0.0f) { rate_change_limit = _accel_yaw_max * _dt; // update feed forward yaw rate after checking it is within acceleration limits rate_change = yaw_rate_ef - _rate_ef_desired.z; rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); _rate_ef_desired.z += rate_change; } else { _rate_ef_desired.z = yaw_rate_ef; } // update earth frame angle targets and errors update_ef_roll_angle_and_error(_rate_ef_desired.x, angle_ef_error, AC_ATTITUDE_RATE_STAB_ROLL_OVERSHOOT_ANGLE_MAX); update_ef_pitch_angle_and_error(_rate_ef_desired.y, angle_ef_error, AC_ATTITUDE_RATE_STAB_PITCH_OVERSHOOT_ANGLE_MAX); update_ef_yaw_angle_and_error(_rate_ef_desired.z, angle_ef_error, AC_ATTITUDE_RATE_STAB_YAW_OVERSHOOT_ANGLE_MAX); // constrain earth-frame angle targets _angle_ef_target.x = constrain_float(_angle_ef_target.x, -_aparm.angle_max, _aparm.angle_max); _angle_ef_target.y = constrain_float(_angle_ef_target.y, -_aparm.angle_max, _aparm.angle_max); // convert earth-frame angle errors to body-frame angle errors frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error); // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); // convert earth-frame rates to body-frame rates frame_conversion_ef_to_bf(_rate_ef_desired, _rate_bf_desired); // add body frame rate feed forward _rate_bf_target += _rate_bf_desired; // body-frame to motor outputs should be called separately }
// rate_bf_to_motor_roll_pitch - ask the rate controller to calculate the motor outputs to achieve the target rate in centi-degrees / second void AC_AttitudeControl_Heli::rate_bf_to_motor_roll_pitch(float rate_roll_target_cds, float rate_pitch_target_cds) { float roll_pd, roll_i, roll_ff; // used to capture pid values float pitch_pd, pitch_i, pitch_ff; // used to capture pid values float rate_roll_error, rate_pitch_error; // simply target_rate - current_rate float roll_out, pitch_out; const Vector3f& gyro = _ahrs.get_gyro(); // get current rates // calculate error rate_roll_error = rate_roll_target_cds - gyro.x * AC_ATTITUDE_CONTROL_DEGX100; rate_pitch_error = rate_pitch_target_cds - gyro.y * AC_ATTITUDE_CONTROL_DEGX100; // input to PID controller _pid_rate_roll.set_input_filter_all(rate_roll_error); _pid_rate_roll.set_desired_rate(rate_roll_target_cds); _pid_rate_pitch.set_input_filter_all(rate_pitch_error); _pid_rate_pitch.set_desired_rate(rate_pitch_target_cds); // call p and d controllers roll_pd = _pid_rate_roll.get_p() + _pid_rate_roll.get_d(); pitch_pd = _pid_rate_pitch.get_p() + _pid_rate_pitch.get_d(); // get roll i term roll_i = _pid_rate_roll.get_integrator(); // update i term as long as we haven't breached the limits or the I term will certainly reduce if (!_flags_heli.limit_roll || ((roll_i>0&&rate_roll_error<0)||(roll_i<0&&rate_roll_error>0))){ if (((AP_MotorsHeli&)_motors).has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim if (rate_roll_target_cds > -50 && rate_roll_target_cds < 50){ // Frozen at high rates roll_i = _pid_rate_roll.get_i(); } }else{ if (_flags_heli.leaky_i){ roll_i = ((AC_HELI_PID&)_pid_rate_roll).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); }else{ roll_i = _pid_rate_roll.get_i(); } } } // get pitch i term pitch_i = _pid_rate_pitch.get_integrator(); // update i term as long as we haven't breached the limits or the I term will certainly reduce if (!_flags_heli.limit_pitch || ((pitch_i>0&&rate_pitch_error<0)||(pitch_i<0&&rate_pitch_error>0))){ if (((AP_MotorsHeli&)_motors).has_flybar()) { // Mechanical Flybars get regular integral for rate auto trim if (rate_pitch_target_cds > -50 && rate_pitch_target_cds < 50){ // Frozen at high rates pitch_i = _pid_rate_pitch.get_i(); } }else{ if (_flags_heli.leaky_i) { pitch_i = ((AC_HELI_PID&)_pid_rate_pitch).get_leaky_i(AC_ATTITUDE_HELI_RATE_INTEGRATOR_LEAK_RATE); }else{ pitch_i = _pid_rate_pitch.get_i(); } } } roll_ff = roll_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_roll).get_vff(rate_roll_target_cds), _dt); pitch_ff = pitch_feedforward_filter.apply(((AC_HELI_PID&)_pid_rate_pitch).get_vff(rate_pitch_target_cds), _dt); // add feed forward and final output roll_out = roll_pd + roll_i + roll_ff; pitch_out = pitch_pd + pitch_i + pitch_ff; // constrain output and update limit flags if (fabsf(roll_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) { roll_out = constrain_float(roll_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX); _flags_heli.limit_roll = true; }else{ _flags_heli.limit_roll = false; } if (fabsf(pitch_out) > AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX) { pitch_out = constrain_float(pitch_out,-AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX,AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX); _flags_heli.limit_pitch = true; }else{ _flags_heli.limit_pitch = false; } // output to motors _motors.set_roll(roll_out); _motors.set_pitch(pitch_out); /* #if HELI_CC_COMP == ENABLED static LowPassFilterFloat rate_dynamics_filter; // Rate Dynamics filter #endif #if HELI_CC_COMP == ENABLED rate_dynamics_filter.set_cutoff_frequency(0.01f, 4.0f); #endif #if AC_ATTITUDE_HELI_CC_COMP == ENABLED // Do cross-coupling compensation for low rpm helis // Credit: Jolyon Saunders // Note: This is not widely tested at this time. Will not be used by default yet. float cc_axis_ratio = 2.0f; // Ratio of compensation on pitch vs roll axes. Number >1 means pitch is affected more than roll float cc_kp = 0.0002f; // Compensation p term. Setting this to zero gives h_phang only, while increasing it will increase the p term of correction float cc_kd = 0.127f; // Compensation d term, scaled. This accounts for flexing of the blades, dampers etc. Originally was (motors.ext_gyro_gain * 0.0001) float cc_angle, cc_total_output; uint32_t cc_roll_d, cc_pitch_d, cc_sum_d; int32_t cc_scaled_roll; int32_t cc_roll_output; // Used to temporarily hold output while rotation is being calculated int32_t cc_pitch_output; // Used to temporarily hold output while rotation is being calculated static int32_t last_roll_output = 0; static int32_t last_pitch_output = 0; cc_scaled_roll = roll_output / cc_axis_ratio; // apply axis ratio to roll cc_total_output = safe_sqrt(cc_scaled_roll * cc_scaled_roll + pitch_output * pitch_output) * cc_kp; // find the delta component cc_roll_d = (roll_output - last_roll_output) / cc_axis_ratio; cc_pitch_d = pitch_output - last_pitch_output; cc_sum_d = safe_sqrt(cc_roll_d * cc_roll_d + cc_pitch_d * cc_pitch_d); // do the magic. cc_angle = cc_kd * cc_sum_d * cc_total_output - cc_total_output * motors.get_phase_angle(); // smooth angle variations, apply constraints cc_angle = rate_dynamics_filter.apply(cc_angle); cc_angle = constrain_float(cc_angle, -90.0f, 0.0f); cc_angle = radians(cc_angle); // Make swash rate vector Vector2f swashratevector; swashratevector.x = cosf(cc_angle); swashratevector.y = sinf(cc_angle); swashratevector.normalize(); // rotate the output cc_roll_output = roll_output; cc_pitch_output = pitch_output; roll_output = - (cc_pitch_output * swashratevector.y - cc_roll_output * swashratevector.x); pitch_output = cc_pitch_output * swashratevector.x + cc_roll_output * swashratevector.y; // make current outputs old, for next iteration last_roll_output = cc_roll_output; last_pitch_output = cc_pitch_output; # endif // HELI_CC_COMP #if AC_ATTITUDE_HELI_PIRO_COMP == ENABLED if (control_mode <= ACRO){ int32_t piro_roll_i, piro_pitch_i; // used to hold i term while doing prio comp piro_roll_i = roll_i; piro_pitch_i = pitch_i; Vector2f yawratevector; yawratevector.x = cosf(-omega.z/100.0f); yawratevector.y = sinf(-omega.z/100.0f); yawratevector.normalize(); roll_i = piro_roll_i * yawratevector.x - piro_pitch_i * yawratevector.y; pitch_i = piro_pitch_i * yawratevector.x + piro_roll_i * yawratevector.y; g.pid_rate_pitch.set_integrator(pitch_i); g.pid_rate_roll.set_integrator(roll_i); } #endif //HELI_PIRO_COMP */ }
// perform drift correction. This function aims to update _omega_P and // _omega_I with our best estimate of the short term and long term // gyro error. The _omega_P value is what pulls our attitude solution // back towards the reference vector quickly. The _omega_I term is an // attempt to learn the long term drift rate of the gyros. // // This drift correction implementation is based on a paper // by Bill Premerlani from here: // http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf void AP_AHRS_DCM::drift_correction(float deltat) { Vector3f velocity; uint32_t last_correction_time; // perform yaw drift correction if we have a new yaw reference // vector drift_correction_yaw(); // rotate accelerometer values into the earth frame for (uint8_t i=0; i<_ins.get_accel_count(); i++) { if (_ins.get_accel_health(i)) { /* by using get_delta_velocity() instead of get_accel() the accel value is sampled over the right time delta for each sensor, which prevents an aliasing effect */ Vector3f delta_velocity; float delta_velocity_dt; _ins.get_delta_velocity(i, delta_velocity); delta_velocity_dt = _ins.get_delta_velocity_dt(i); if (delta_velocity_dt > 0) { _accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt); // integrate the accel vector in the earth frame between GPS readings _ra_sum[i] += _accel_ef[i] * deltat; } } } //update _accel_ef_blended #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 if (_ins.get_accel_count() == 2 && _ins.get_accel_health(0) && _ins.get_accel_health(1)) { float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f; // slew _imu1_weight over one second _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat); _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight); } else { _accel_ef_blended = _accel_ef[_ins.get_primary_accel()]; } #else _accel_ef_blended = _accel_ef[_ins.get_primary_accel()]; #endif // HAL_CPU_CLASS >= HAL_CPU_CLASS_75 // keep a sum of the deltat values, so we know how much time // we have integrated over _ra_deltat += deltat; if (!have_gps() || _gps.status() < AP_GPS::GPS_OK_FIX_3D || _gps.num_sats() < _gps_minsats) { // no GPS, or not a good lock. From experience we need at // least 6 satellites to get a really reliable velocity number // from the GPS. // // As a fallback we use the fixed wing acceleration correction // if we have an airspeed estimate (which we only have if // _fly_forward is set), otherwise no correction if (_ra_deltat < 0.2f) { // not enough time has accumulated return; } float airspeed; if (airspeed_sensor_enabled()) { airspeed = _airspeed->get_airspeed(); } else { airspeed = _last_airspeed; } // use airspeed to estimate our ground velocity in // earth frame by subtracting the wind velocity = _dcm_matrix.colx() * airspeed; // add in wind estimate velocity += _wind; last_correction_time = hal.scheduler->millis(); _have_gps_lock = false; } else { if (_gps.last_fix_time_ms() == _ra_sum_start) { // we don't have a new GPS fix - nothing more to do return; } velocity = _gps.velocity(); last_correction_time = _gps.last_fix_time_ms(); if (_have_gps_lock == false) { // if we didn't have GPS lock in the last drift // correction interval then set the velocities equal _last_velocity = velocity; } _have_gps_lock = true; // keep last airspeed estimate for dead-reckoning purposes Vector3f airspeed = velocity - _wind; airspeed.z = 0; _last_airspeed = airspeed.length(); } if (have_gps()) { // use GPS for positioning with any fix, even a 2D fix _last_lat = _gps.location().lat; _last_lng = _gps.location().lng; _position_offset_north = 0; _position_offset_east = 0; // once we have a single GPS lock, we can update using // dead-reckoning from then on _have_position = true; } else { // update dead-reckoning position estimate _position_offset_north += velocity.x * _ra_deltat; _position_offset_east += velocity.y * _ra_deltat; } // see if this is our first time through - in which case we // just setup the start times and return if (_ra_sum_start == 0) { _ra_sum_start = last_correction_time; _last_velocity = velocity; return; } // equation 9: get the corrected acceleration vector in earth frame. Units // are m/s/s Vector3f GA_e; GA_e = Vector3f(0, 0, -1.0f); bool using_gps_corrections = false; float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS); if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) { float v_scale = gps_gain.get() * ra_scale; Vector3f vdelta = (velocity - _last_velocity) * v_scale; GA_e += vdelta; GA_e.normalize(); if (GA_e.is_inf()) { // wait for some non-zero acceleration information _last_failure_ms = hal.scheduler->millis(); return; } using_gps_corrections = true; } // calculate the error term in earth frame. // we do this for each available accelerometer then pick the // accelerometer that leads to the smallest error term. This takes // advantage of the different sample rates on different // accelerometers to dramatically reduce the impact of aliasing // due to harmonics of vibrations that match closely the sampling // rate of our accelerometers. On the Pixhawk we have the LSM303D // running at 800Hz and the MPU6000 running at 1kHz, by combining // the two the effects of aliasing are greatly reduced. Vector3f error[INS_MAX_INSTANCES]; float error_dirn[INS_MAX_INSTANCES]; Vector3f GA_b[INS_MAX_INSTANCES]; int8_t besti = -1; float best_error = 0; for (uint8_t i=0; i<_ins.get_accel_count(); i++) { if (!_ins.get_accel_health(i)) { // only use healthy sensors continue; } _ra_sum[i] *= ra_scale; // get the delayed ra_sum to match the GPS lag if (using_gps_corrections) { GA_b[i] = ra_delayed(i, _ra_sum[i]); } else { GA_b[i] = _ra_sum[i]; } if (GA_b[i].is_zero()) { // wait for some non-zero acceleration information continue; } GA_b[i].normalize(); if (GA_b[i].is_inf()) { // wait for some non-zero acceleration information continue; } error[i] = GA_b[i] % GA_e; // Take dot product to catch case vectors are opposite sign and parallel error_dirn[i] = GA_b[i] * GA_e; float error_length = error[i].length(); if (besti == -1 || error_length < best_error) { besti = i; best_error = error_length; } // Catch case where orientation is 180 degrees out if (error_dirn[besti] < 0.0f) { best_error = 1.0f; } } if (besti == -1) { // no healthy accelerometers! _last_failure_ms = hal.scheduler->millis(); return; } _active_accel_instance = besti; #define YAW_INDEPENDENT_DRIFT_CORRECTION 0 #if YAW_INDEPENDENT_DRIFT_CORRECTION // step 2 calculate earth_error_Z float earth_error_Z = error.z; // equation 10 float tilt = pythagorous2(GA_e.x, GA_e.y); // equation 11 float theta = atan2f(GA_b[besti].y, GA_b[besti].x); // equation 12 Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z); // step 6 error = GA_b[besti] % GA_e2; error.z = earth_error_Z; #endif // YAW_INDEPENDENT_DRIFT_CORRECTION // to reduce the impact of two competing yaw controllers, we // reduce the impact of the gps/accelerometers on yaw when we are // flat, but still allow for yaw correction using the // accelerometers at high roll angles as long as we have a GPS if (AP_AHRS_DCM::use_compass()) { if (have_gps() && is_equal(gps_gain,1.0f)) { error[besti].z *= sinf(fabsf(roll)); } else { error[besti].z = 0; } } // if ins is unhealthy then stop attitude drift correction and // hope the gyros are OK for a while. Just slowly reduce _omega_P // to prevent previous bad accels from throwing us off if (!_ins.healthy()) { error[besti].zero(); } else { // convert the error term to body frame error[besti] = _dcm_matrix.mul_transpose(error[besti]); } if (error[besti].is_nan() || error[besti].is_inf()) { // don't allow bad values check_matrix(); _last_failure_ms = hal.scheduler->millis(); return; } _error_rp = 0.8f * _error_rp + 0.2f * best_error; // base the P gain on the spin rate float spin_rate = _omega.length(); // sanity check _kp value if (_kp < AP_AHRS_RP_P_MIN) { _kp = AP_AHRS_RP_P_MIN; } // we now want to calculate _omega_P and _omega_I. The // _omega_P value is what drags us quickly to the // accelerometer reading. _omega_P = error[besti] * _P_gain(spin_rate) * _kp; if (use_fast_gains()) { _omega_P *= 8; } if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D && _gps.ground_speed() < GPS_SPEED_MIN && _ins.get_accel().x >= 7 && pitch_sensor > -3000 && pitch_sensor < 3000) { // assume we are in a launch acceleration, and reduce the // rp gain by 50% to reduce the impact of GPS lag on // takeoff attitude when using a catapult _omega_P *= 0.5f; } // accumulate some integrator error if (spin_rate < ToRad(SPIN_RATE_LIMIT)) { _omega_I_sum += error[besti] * _ki * _ra_deltat; _omega_I_sum_time += _ra_deltat; } if (_omega_I_sum_time >= 5) { // limit the rate of change of omega_I to the hardware // reported maximum gyro drift rate. This ensures that // short term errors don't cause a buildup of omega_I // beyond the physical limits of the device float change_limit = _gyro_drift_limit * _omega_I_sum_time; _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit); _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit); _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit); _omega_I += _omega_I_sum; _omega_I_sum.zero(); _omega_I_sum_time = 0; } // zero our accumulator ready for the next GPS step memset(&_ra_sum[0], 0, sizeof(_ra_sum)); _ra_deltat = 0; _ra_sum_start = last_correction_time; // remember the velocity for next time _last_velocity = velocity; }
// 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 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 // 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); // 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_avg_max); // 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.0f - (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_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; 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); } } }
void AP_TECS::_update_pitch(void) { // Calculate Speed/Height Control Weighting // This is used to determine how the pitch control prioritises speed and height control // A weighting of 1 provides equal priority (this is the normal mode of operation) // A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available // A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected. In this instance, if airspeed // rises above the demanded value, the pitch angle will be increased by the TECS controller. float SKE_weighting = constrain_float(_spdWeight, 0.0f, 2.0f); if (!_ahrs.airspeed_sensor_enabled()) { SKE_weighting = 0.0f; } else if ( _underspeed || _flight_stage == AP_TECS::FLIGHT_TAKEOFF) { SKE_weighting = 2.0f; } else if (_flight_stage == AP_TECS::FLIGHT_LAND_APPROACH || _flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { SKE_weighting = constrain_float(_spdWeightLand, 0.0f, 2.0f); } float SPE_weighting = 2.0f - SKE_weighting; // Calculate Specific Energy Balance demand, and error float SEB_dem = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting; float SEBdot_dem = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting; float SEB_error = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting); float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting); // Calculate integrator state, constraining input if pitch limits are exceeded float integ7_input = SEB_error * _integGain; if (_pitch_dem_unc > _PITCHmaxf) { integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc); } else if (_pitch_dem_unc < _PITCHminf) { integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc); } _integ7_state = _integ7_state + integ7_input * _DT; // Apply max and min values for integrator state that will allow for no more than // 5deg of saturation. This allows for some pitch variation due to gusts before the // integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence // During climbout/takeoff, bias the demanded pitch angle so that zero speed error produces a pitch angle // demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the // integrator has to catch up before the nose can be raised to reduce speed during climbout. // During flare a different damping gain is used float gainInv = (_integ5_state * timeConstant() * GRAVITY_MSS); float temp = SEB_error + SEBdot_dem * timeConstant(); if (_flight_stage == AP_TECS::FLIGHT_LAND_FINAL) { temp += SEBdot_error * _landDamp; } else { temp += SEBdot_error * _ptchDamp; } if (_flight_stage == AP_TECS::FLIGHT_TAKEOFF) { temp += _PITCHminf * gainInv; } _integ7_state = constrain_float(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp); // Calculate pitch demand from specific energy balance signals _pitch_dem_unc = (temp + _integ7_state) / gainInv; // Constrain pitch demand _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); // Rate limit the pitch demand to comply with specified vertical // acceleration limit float ptchRateIncr = _DT * _vertAccLim / _integ5_state; if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) { _pitch_dem = _last_pitch_dem + ptchRateIncr; } else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) { _pitch_dem = _last_pitch_dem - ptchRateIncr; } _last_pitch_dem = _pitch_dem; }
// drift_run - runs the drift controller // should be called at 100hz or more void Copter::ModeDrift::run() { static float braker = 0.0f; static float roll_input = 0.0f; float target_roll, target_pitch; float target_yaw_rate; float pilot_throttle_scaled; // if landed and throttle at zero, set throttle to zero and exit immediately if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) { zero_throttle_and_relax_ac(); return; } // clear landing flag above zero throttle if (!ap.throttle_zero) { set_land_complete(false); } // convert pilot input to lean angles get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, copter.aparm.angle_max); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in()); // Grab inertial velocity const Vector3f& vel = inertial_nav.get_velocity(); // rotate roll, pitch input from north facing to vehicle's perspective float roll_vel = vel.y * ahrs.cos_yaw() - vel.x * ahrs.sin_yaw(); // body roll vel float pitch_vel = vel.y * ahrs.sin_yaw() + vel.x * ahrs.cos_yaw(); // body pitch vel // gain sceduling for Yaw float pitch_vel2 = MIN(fabsf(pitch_vel), 2000); target_yaw_rate = ((float)target_roll/1.0f) * (1.0f - (pitch_vel2 / 5000.0f)) * g.acro_yaw_p; roll_vel = constrain_float(roll_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); pitch_vel = constrain_float(pitch_vel, -DRIFT_SPEEDLIMIT, DRIFT_SPEEDLIMIT); roll_input = roll_input * .96f + (float)channel_yaw->get_control_in() * .04f; //convert user input into desired roll velocity float roll_vel_error = roll_vel - (roll_input / DRIFT_SPEEDGAIN); // Roll velocity is feed into roll acceleration to minimize slip target_roll = roll_vel_error * -DRIFT_SPEEDGAIN; target_roll = constrain_float(target_roll, -4500.0f, 4500.0f); // If we let go of sticks, bring us to a stop if(is_zero(target_pitch)){ // .14/ (.03 * 100) = 4.6 seconds till full braking braker += .03f; braker = MIN(braker, DRIFT_SPEEDGAIN); target_pitch = pitch_vel * braker; }else{ braker = 0.0f; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // output pilot's throttle with angle boost attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt); }
void AC_AttitudeControl::update_ang_vel_target_from_att_error() { // Compute the roll angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_roll_max > 0.0f) { _ang_vel_target_rads.x = sqrt_controller(_att_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.x = _p_angle_roll.kP() * _att_error_rot_vec_rad.x; } // Compute the pitch angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_pitch_max > 0.0f) { _ang_vel_target_rads.y = sqrt_controller(_att_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss()/2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.y = _p_angle_pitch.kP() * _att_error_rot_vec_rad.y; } // Compute the yaw angular velocity demand from the roll angle error if (_att_ctrl_use_accel_limit && _accel_yaw_max > 0.0f) { _ang_vel_target_rads.z = sqrt_controller(_att_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss()/2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS)); }else{ _ang_vel_target_rads.z = _p_angle_yaw.kP() * _att_error_rot_vec_rad.z; } // Add feedforward term that attempts to ensure that the copter yaws about the reference // Z axis, rather than the vehicle body Z axis. // NOTE: This is a small-angle approximation. _ang_vel_target_rads.x += _att_error_rot_vec_rad.y * _ahrs.get_gyro().z; _ang_vel_target_rads.y += -_att_error_rot_vec_rad.x * _ahrs.get_gyro().z; }
/// advance_wp_target_along_track - move target location along track from origin to destination void AC_WPNav::advance_wp_target_along_track(float dt) { float track_covered; // distance (in cm) along the track that the vehicle has traveled. Measured by drawing a perpendicular line from the track to the vehicle. Vector3f track_error; // distance error (in cm) from the track_covered position (i.e. closest point on the line to the vehicle) and the vehicle float track_desired_max; // the farthest distance (in cm) along the track that the leash will allow float track_desired_temp = _track_desired; float track_leash_slack; // additional distance (in cm) along the track from our track_covered position that our leash will allow bool reached_leash_limit = false; // true when track has reached leash limit and we need to slow down the target point // get current location Vector3f curr_pos = _inav.get_position(); Vector3f curr_delta = curr_pos - _origin; // calculate how far along the track we are track_covered = curr_delta.x * _pos_delta_unit.x + curr_delta.y * _pos_delta_unit.y + curr_delta.z * _pos_delta_unit.z; Vector3f track_covered_pos = _pos_delta_unit * track_covered; track_error = curr_delta - track_covered_pos; // calculate the horizontal error float track_error_xy = pythagorous2(track_error.x, track_error.y); // calculate the vertical error float track_error_z = fabsf(track_error.z); // get position control leash lengths float leash_xy = _pos_control.get_leash_xy(); float leash_z; if (track_error.z >= 0) { leash_z = _pos_control.get_leash_up_z(); }else{ leash_z = _pos_control.get_leash_down_z(); } // calculate how far along the track we could move the intermediate target before reaching the end of the leash track_leash_slack = min(_track_leash_length*(leash_z-track_error_z)/leash_z, _track_leash_length*(leash_xy-track_error_xy)/leash_xy); if (track_leash_slack < 0) { track_desired_max = track_covered; }else{ track_desired_max = track_covered + track_leash_slack; } // check if target is already beyond the leash if (_track_desired > track_desired_max) { reached_leash_limit = true; } // get current velocity const Vector3f &curr_vel = _inav.get_velocity(); // get speed along track float speed_along_track = curr_vel.x * _pos_delta_unit.x + curr_vel.y * _pos_delta_unit.y + curr_vel.z * _pos_delta_unit.z; // calculate point at which velocity switches from linear to sqrt float linear_velocity = _wp_speed_cms; float kP = _pos_control.get_pos_xy_kP(); if (kP >= 0.0f) { // avoid divide by zero linear_velocity = _track_accel/kP; } // let the limited_speed_xy_cms be some range above or below current velocity along track if (speed_along_track < -linear_velocity) { // we are traveling fast in the opposite direction of travel to the waypoint so do not move the intermediate point _limited_speed_xy_cms = 0; }else{ // increase intermediate target point's velocity if not yet at the leash limit if(dt > 0 && !reached_leash_limit) { _limited_speed_xy_cms += 2.0f * _track_accel * dt; } // do not allow speed to be below zero or over top speed _limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms, 0.0f, _track_speed); // check if we should begin slowing down if (!_flags.fast_waypoint) { float dist_to_dest = _track_length - _track_desired; if (!_flags.slowing_down && dist_to_dest <= _slow_down_dist) { _flags.slowing_down = true; } // if target is slowing down, limit the speed if (_flags.slowing_down) { _limited_speed_xy_cms = min(_limited_speed_xy_cms, get_slow_down_speed(dist_to_dest, _track_accel)); } } // if our current velocity is within the linear velocity range limit the intermediate point's velocity to be no more than the linear_velocity above or below our current velocity if (fabsf(speed_along_track) < linear_velocity) { _limited_speed_xy_cms = constrain_float(_limited_speed_xy_cms,speed_along_track-linear_velocity,speed_along_track+linear_velocity); } } // advance the current target if (!reached_leash_limit) { track_desired_temp += _limited_speed_xy_cms * dt; } // do not let desired point go past the end of the segment track_desired_temp = constrain_float(track_desired_temp, 0, _track_length); // set our new desired position on track _track_desired = max(_track_desired, track_desired_temp); // recalculate the desired position _pos_control.set_pos_target(_origin + _pos_delta_unit * _track_desired); // check if we've reached the waypoint if( !_flags.reached_destination ) { if( _track_desired >= _track_length ) { // "fast" waypoints are complete once the intermediate point reaches the destination if (_flags.fast_waypoint) { _flags.reached_destination = true; }else{ // regular waypoints also require the copter to be within the waypoint radius Vector3f dist_to_dest = curr_pos - _destination; if( dist_to_dest.length() <= _wp_radius_cm ) { _flags.reached_destination = true; } } } } }