Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AntennaTracker: changes for more common mavlink code

This commit is contained in:
Andrew Tridgell 2014-07-13 15:37:40 +10:00
parent 3143e192d3
commit 527de3f2e4
1 changed files with 4 additions and 100 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

104
Tools/AntennaTracker/GCS_Mavlink.pde
View File

@ -117,102 +117,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan)
hal.rcout->read(7));
}
static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{
const Vector3f &accel = ins.get_accel();
const Vector3f &gyro = ins.get_gyro();
const Vector3f &mag = compass.get_field();
mavlink_msg_raw_imu_send(
chan,
hal.scheduler->micros(),
accel.x * 1000.0 / GRAVITY_MSS,
accel.y * 1000.0 / GRAVITY_MSS,
accel.z * 1000.0 / GRAVITY_MSS,
gyro.x * 1000.0,
gyro.y * 1000.0,
gyro.z * 1000.0,
mag.x,
mag.y,
mag.z);
if (ins.get_gyro_count() <= 1 &&
ins.get_accel_count() <= 1 &&
compass.get_count() <= 1) {
return;
}
const Vector3f &accel2 = ins.get_accel(1);
const Vector3f &gyro2 = ins.get_gyro(1);
const Vector3f &mag2 = compass.get_field(1);
mavlink_msg_scaled_imu2_send(
chan,
hal.scheduler->millis(),
accel2.x * 1000.0f / GRAVITY_MSS,
accel2.y * 1000.0f / GRAVITY_MSS,
accel2.z * 1000.0f / GRAVITY_MSS,
gyro2.x * 1000.0f,
gyro2.y * 1000.0f,
gyro2.z * 1000.0f,
mag2.x,
mag2.y,
mag2.z);
}
static void NOINLINE send_raw_imu2(mavlink_channel_t chan)
{
float pressure = barometer.get_pressure();
mavlink_msg_scaled_pressure_send(
chan,
hal.scheduler->millis(),
pressure*0.01f, // hectopascal
(pressure - barometer.get_ground_pressure())*0.01f, // hectopascal
barometer.get_temperature()*100); // 0.01 degrees C
}
static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
{
// run this message at a much lower rate - otherwise it
// pointlessly wastes quite a lot of bandwidth
static uint8_t counter;
if (counter++ < 10) {
return;
}
counter = 0;
Vector3f mag_offsets = compass.get_offsets();
Vector3f accel_offsets = ins.get_accel_offsets();
Vector3f gyro_offsets = ins.get_gyro_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.get_pressure(),
barometer.get_temperature()*100,
gyro_offsets.x,
gyro_offsets.y,
gyro_offsets.z,
accel_offsets.x,
accel_offsets.y,
accel_offsets.z);
}
static void NOINLINE send_ahrs(mavlink_channel_t chan)
{
Vector3f omega_I = ahrs.get_gyro_drift();
mavlink_msg_ahrs_send(
chan,
omega_I.x,
omega_I.y,
omega_I.z,
0,
0,
ahrs.get_error_rp(),
ahrs.get_error_yaw());
}
static void NOINLINE send_hwstatus(mavlink_channel_t chan)
{
mavlink_msg_hwstatus_send(
@ -296,17 +200,17 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
case MSG_RAW_IMU1:
CHECK_PAYLOAD_SIZE(RAW_IMU);
send_raw_imu1(chan);
gcs[chan-MAVLINK_COMM_0].send_raw_imu(ins, compass);
break;
case MSG_RAW_IMU2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
send_raw_imu2(chan);
gcs[chan-MAVLINK_COMM_0].send_scaled_pressure(barometer);
break;
case MSG_RAW_IMU3:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_raw_imu3(chan);
gcs[chan-MAVLINK_COMM_0].send_sensor_offsets(ins, compass, barometer);
break;
case MSG_NEXT_PARAM:
@ -326,7 +230,7 @@ bool GCS_MAVLINK::try_send_message(enum ap_message id)
case MSG_AHRS:
CHECK_PAYLOAD_SIZE(AHRS);
send_ahrs(chan);
gcs[chan-MAVLINK_COMM_0].send_ahrs(ahrs);
break;
case MSG_SIMSTATE: