记录无人机SITL的GPS数据来源

send a new set of GPS UBLOX packets

void GPS_UBlox::publish(const GPS_Data *d)

$25 = (const SITL::GPS_Data *) 0x7fffffffdb00

(gdb) p *d

$26 = {timestamp_ms = 9779, latitude = -35.3632621, longitude = 149.1652374, altitude = 584.090027, speedN = 0, speedE = 0, speedD = 0,

yaw_deg = 352.99999475479126, roll_deg = 0, pitch_deg = 0, have_lock = true, horizontal_acc = 0.300000012, vertical_acc = 0.300000012, speed_acc = 0,

num_sats = 10 '\n', fix_type = 6 '\006'}

pos.time = gps_tow.ms;

pos.longitude = d->longitude * 1.0e7;

pos.latitude = d->latitude * 1.0e7;

pos.altitude_ellipsoid = d->altitude * 1000.0f;

pos.altitude_msl = d->altitude * 1000.0f;

pos.horizontal_accuracy = d->horizontal_acc*1000;

pos.vertical_accuracy = d->vertical_acc*1000;

/////////////// 这里是读到AP_GPS_UBLOX::read里面m

state.location.lng = _buffer.pvt.lon;

state.location.lat = _buffer.pvt.lat;

p AP_GPS::get_singleton()->state $0$ .location.alt

$55 = 68406

(gdb) p ((HALSITL::Scheduler*)hal.scheduler)._sitlState.sitl_model.smoothing

p &SITL::SIM::_singleton.opos.alt._value

这个来自与参数

SIM_OPOS_ALT,13

SIM_OPOS_HDG,353

SIM_OPOS_LAT,30.22791

SIM_OPOS_LNG,120.2794

设置到home 并且同步到location

location.alt = static_cast<int32_t>(home.alt - position.z * 100.0f);

(gdb) p ((HALSITL::Scheduler*)hal.scheduler)._sitlState.sitl_model.position.x

$10 = 0

void Aircraft::update_dynamics(const Vector3f &rot_accel)

position.z = -(ground_level + frame_height - home.alt * 0.01f + ground_height_difference());

///////

然后 position.z的更新在sim里面变得巨复杂根据电机推力反算模拟.

数据流：从 PWM 到推力

读取 PWM 指令 ：

const float pwm = input.servos[motor_offset+servo];
- 从 input.servos 数组里，找到控制这个电机的 PWM 值（通常是 1000 到 2000 之间的整数）。
转换为归一化指令 ：

float command = pwm_to_command(pwm);
- 把 PWM 值转成一个标准化的油门指令（通常是 0 到 1 或 -1 到 1），方便计算。
电池电压修正 ：

float voltage_scale = voltage / voltage_max;
- 根据当前电池电压修正推力值。电压低了，推力会相应减小，模拟真实电池的掉压效果。
计算推力 (核心) ：

float motor_thrust = calc_thrust(command, air_density, velocity_in, voltage_scale);
- 调用 calc_thrust 函数，结合油门指令、空气密度和入流速度，计算出此时电机产生的实际推力大小（牛顿）。

输出参数

断点打在Motor::calculate_forces

复制代码

void Motor::calculate_forces(..., 
                             Vector3f &torque,   // 输出1
                             Vector3f &thrust,   // 输出2
                             ...)

输出参数	类型	单位	说明
`torque`	`Vector3f&`	牛米 (N·m)	该电机对飞机产生的扭矩向量（机体坐标系）
`thrust`	`Vector3f&`	牛顿 (N)	该电机产生的推力向量（机体坐标系）

一直觉得很多数学公式学了但是实际用不到就忘记了. 但是在无人机里面这里的数学实用性就体现出来了.

回忆下线性代数里面的矩阵叉乘

代码里面这个算法中两处用到了这个

// add velocity of motor about center due to vehicle rotation

motor_vel+= -(position **%**gyro);

// calculate total torque in newton-meters

torque= (position % thrust) + rotor_torque;

这里的%被重载为叉乘

看下物理意义

物理背景：力矩的定义

在物理学中，力矩（Torque） 的计算公式是：

text

复制代码

τ = r × F
  = 力臂 × 力（叉积）

其中：

r：力的作用点相对于重心的位置向量（力臂）
F：力向量
×：叉积

叉积的结果是一个向量，其：

大小 = |r| × |F| × sin(θ)（旋转的"强度"）
方向 = 旋转轴的方向（右手定则）

举个栗子

右前电机（position = {0.2, 0.2, 0}，thrust = {0, 0, -5.0}）

τ=p×f=(−1.0, 1.0, 0)

roll 逆时针

pitch 顺时针

yaw 不变

电机布局（X 型）

索引	位置	方向	说明
0	`{x: 0.247, y: 0.247}`	右前	正桨（`yaw_factor = 1`）
1	`{x: -0.247, y: -0.247}`	左后	正桨（`yaw_factor = 1`）
2	`{x: 0.247, y: -0.247}`	右后	反桨（`yaw_factor = -1`）
3	`{x: -0.247, y: 0.247}`	左前	反桨（`yaw_factor = -1`）

////////// gyro根据角动量加速度更新