void send(const hrt_abstime t)
{
struct vehicle_attitude_s att;
struct vehicle_global_position_s pos;
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
bool updated = att_sub->update(&att_time, &att);
updated |= pos_sub->update(&pos_time, &pos);
updated |= armed_sub->update(&armed_time, &armed);
updated |= act_sub->update(&act_time, &act);
updated |= airspeed_sub->update(&airspeed_time, &airspeed);
if (updated) {
float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
airspeed.true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos.alt,
-pos.vel_d);
}
}
void Rover::send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
gps.ground_speed(),
ahrs.groundspeed(),
(ahrs.yaw_sensor / 100) % 360,
(uint16_t)(100 * fabsf(channel_throttle->norm_output())),
current_loc.alt / 100.0,
0);
}
void Rover::send_vfr_hud(mavlink_channel_t chan)
{
mavlink_msg_vfr_hud_send(
chan,
gps.ground_speed(),
ahrs.groundspeed(),
(ahrs.yaw_sensor / 100) % 360,
g2.motors.get_throttle(),
current_loc.alt / 100.0f,
0);
}
void
l_airspeed(const struct listener *l)
{
struct airspeed_s airspeed;
orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
float alt = global_pos.alt;
float climb = global_pos.vz;
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
}
/**
* Send Metrics typically displayed on a HUD for fixed wing aircraft.
*/
static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
{
/* Current heading in degrees, in compass units (0..360, 0=north) */
int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
/* Current throttle setting in integer percent, 0 to 100 */
// is a 16bit unsigned int but supposed to be from 0 to 100??
uint16_t throttle;
#ifdef COMMAND_THRUST
throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
#elif defined COMMAND_THROTTLE
throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
#endif
mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
stateGetAirspeed_f(),
stateGetHorizontalSpeedNorm_f(), // groundspeed
heading,
throttle,
stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL
stateGetSpeedNed_f()->z); // climb rate
MAVLinkSendMessage();
}
void send(const hrt_abstime t)
{
bool updated = att_sub->update(t);
updated |= pos_sub->update(t);
updated |= armed_sub->update(t);
updated |= act_sub->update(t);
updated |= airspeed_sub->update(t);
if (updated) {
float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
airspeed->true_airspeed_m_s,
groundspeed,
heading,
throttle,
pos->alt,
-pos->vel_d);
}
}
