void
l_global_position(const struct listener *l)
{
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
uint64_t timestamp = global_pos.timestamp;
int32_t lat = global_pos.lat;
int32_t lon = global_pos.lon;
int32_t alt = (int32_t)(global_pos.alt * 1000);
int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f);
int16_t vx = (int16_t)(global_pos.vx * 100.0f);
int16_t vy = (int16_t)(global_pos.vy * 100.0f);
int16_t vz = (int16_t)(global_pos.vz * 100.0f);
/* heading in degrees * 10, from 0 to 36.000) */
uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
timestamp / 1000,
lat,
lon,
alt,
relative_alt,
vx,
vy,
vz,
hdg);
}
void Rover::send_location(mavlink_channel_t chan)
{
uint32_t fix_time;
// if we have a GPS fix, take the time as the last fix time. That
// allows us to correctly calculate velocities and extrapolate
// positions.
// If we don't have a GPS fix then we are dead reckoning, and will
// use the current boot time as the fix time.
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
fix_time = gps.last_fix_time_ms();
} else {
fix_time = millis();
}
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send(
chan,
fix_time,
current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees
current_loc.alt * 10UL, // millimeters above sea level
(current_loc.alt - home.alt) * 10, // millimeters above ground
vel.x * 100, // X speed cm/s (+ve North)
vel.y * 100, // Y speed cm/s (+ve East)
vel.z * -100, // Z speed cm/s (+ve up)
ahrs.yaw_sensor);
}
static void NOINLINE send_location(mavlink_channel_t chan)
{
//Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
1,//millis(),
2,//current_loc.lat, // in 1E7 degrees
3,//current_loc.lng, // in 1E7 degrees
4,//g_gps->altitude * 10, // millimeters above sea level
5,//(current_loc.alt - home.alt) * 10, // millimeters above ground
6,//g_gps->ground_speed * rot.a.x, // X speed cm/s
7,//g_gps->ground_speed * rot.b.x, // Y speed cm/s
8,//g_gps->ground_speed * rot.c.x,
9);//g_gps->ground_course); // course in 1/100 degree
}
void Rover::send_location(mavlink_channel_t chan)
{
const uint32_t now = AP_HAL::millis();
Vector3f vel;
ahrs.get_velocity_NED(vel);
mavlink_msg_global_position_int_send(
chan,
now,
current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees
current_loc.alt * 10UL, // millimeters above sea level
(current_loc.alt - home.alt) * 10, // millimeters above home
vel.x * 100, // X speed cm/s (+ve North)
vel.y * 100, // Y speed cm/s (+ve East)
vel.z * 100, // Z speed cm/s (+ve Down)
ahrs.yaw_sensor);
}
void send(const hrt_abstime t)
{
bool updated = pos_sub->update(t);
updated |= home_sub->update(t);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos->timestamp / 1000,
pos->lat * 1e7,
pos->lon * 1e7,
pos->alt * 1000.0f,
(pos->alt - home->alt) * 1000.0f,
pos->vel_n * 100.0f,
pos->vel_e * 100.0f,
pos->vel_d * 100.0f,
_wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
// send position tracker is using
void GCS_MAVLINK_Tracker::send_global_position_int()
{
if (!tracker.stationary) {
GCS_MAVLINK::send_global_position_int();
return;
}
mavlink_msg_global_position_int_send(
chan,
AP_HAL::millis(),
tracker.current_loc.lat, // in 1E7 degrees
tracker.current_loc.lng, // in 1E7 degrees
tracker.current_loc.alt, // millimeters above ground/sea level
0, // millimeters above home
0, // X speed cm/s (+ve North)
0, // Y speed cm/s (+ve East)
0, // Z speed cm/s (+ve Down)
tracker.ahrs.yaw_sensor); // compass heading in 1/100 degree
}
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
{
float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
if (heading < 0.) {
heading += 360;
}
uint16_t compass_heading = heading * 100;
int32_t relative_alt = stateGetPositionLla_i()->alt - state.ned_origin_f.hmsl;
/// TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs
mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
get_sys_time_msec(),
stateGetPositionLla_i()->lat,
stateGetPositionLla_i()->lon,
stateGetPositionLla_i()->alt,
relative_alt,
stateGetSpeedNed_f()->x * 100,
stateGetSpeedNed_f()->y * 100,
stateGetSpeedNed_f()->z * 100,
compass_heading);
MAVLinkSendMessage();
}
void Tracker::send_location(mavlink_channel_t chan)
{
uint32_t fix_time;
if (gps.status() >= AP_GPS::GPS_OK_FIX_2D) {
fix_time = gps.last_fix_time_ms();
} else {
fix_time = hal.scheduler->millis();
}
const Vector3f &vel = gps.velocity();
mavlink_msg_global_position_int_send(
chan,
fix_time,
current_loc.lat, // in 1E7 degrees
current_loc.lng, // in 1E7 degrees
current_loc.alt * 10, // millimeters above sea level
0,
vel.x * 100, // X speed cm/s (+ve North)
vel.y * 100, // Y speed cm/s (+ve East)
vel.z * -100, // Z speed cm/s (+ve up)
ahrs.yaw_sensor);
}
void send(const hrt_abstime t)
{
struct vehicle_global_position_s pos;
struct home_position_s home;
bool updated = pos_sub->update(&pos_time, &pos);
updated |= home_sub->update(&home_time, &home);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
pos.timestamp / 1000,
pos.lat * 1e7,
pos.lon * 1e7,
pos.alt * 1000.0f,
(pos.alt - home.alt) * 1000.0f,
pos.vel_n * 100.0f,
pos.vel_e * 100.0f,
pos.vel_d * 100.0f,
_wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
