void ModeStabilize_Run(void) { float target_roll, target_pitch; float target_yaw_rate; float pilot_throttle_scaled; // if not armed set throttle to zero and exit immediately如果没有武装,将油门设置为零并立即退出 if (!ACMotors_armed_state() || tACFlags.throttle_zero) { zero_throttle_and_relax_ac(); return; } // clear landing flag LandDetector_SetLandComplete(false); ACMotors_set_desired_spool_state(DESIRED_THROTTLE_UNLIMITED); // apply SIMPLE mode transform to pilot inputs // update_simple_mode(); // convert pilot input to lean angles将飞行员输入转换为倾斜角度 get_pilot_desired_lean_angles(&target_roll, &target_pitch, tAttitudeContorl.tParam.angle_max, tAttitudeContorl.tParam.angle_max); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(ApmData_GetControlIn_Yaw()); // get pilot's desired throttle pilot_throttle_scaled = get_pilot_desired_throttle(ApmData_GetControlIn_Throttle()); // call attitude controller AttCtrl_input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); // body-frame rate controller is run directly from 100hz loop // output pilot's throttle AttCtrl_set_throttle_out(pilot_throttle_scaled, true, 0.0f); }
时间: 2024-04-21 15:29:15 浏览: 274
DCM.zip_IMU_PITCH_roll_dcm_pitch roll
这段代码是一个姿态控制器的函数,主要功能是控制飞行器姿态稳定。具体来看:
- target_roll、target_pitch、target_yaw_rate和pilot_throttle_scaled都是float类型的变量,用于存储目标倾斜角、目标偏航速率和飞行员期望油门值。
- 如果飞行器没有武装或者油门值为零,则将油门值设置为零并立即退出函数。
- LandDetector_SetLandComplete(false)用于清除降落标志。
- ACMotors_set_desired_spool_state(DESIRED_THROTTLE_UNLIMITED)将电机的期望油门值设置为不限制。
- get_pilot_desired_lean_angles()函数用于将飞行员的输入转换为目标倾斜角度。
- get_pilot_desired_yaw_rate()函数用于获取飞行员的期望偏航速率。
- get_pilot_desired_throttle()函数用于获取飞行员的期望油门值。
- AttCtrl_input_euler_angle_roll_pitch_euler_rate_yaw()函数用于调用姿态控制器,传入目标倾斜角、目标偏航速率等参数。
- AttCtrl_set_throttle_out()函数用于输出期望的油门值。
总的来说,这段代码实现了飞行器的姿态稳定控制,将飞行员的输入转换为目标倾斜角度、期望偏航速率和期望油门值,并通过姿态控制器控制飞行器姿态,将期望油门值输出。
阅读全文