void send() {
// attitude states (rad)
float roll = 1;
float pitch = 2;
float yaw = 3;
// body rates
float rollRate = 0.1;
float pitchRate = 0.2;
float yawRate = 0.3;
// position
int32_t lat = 1*m_rad2deg*1e7;
int32_t lon = 2*m_rad2deg*1e7;
int16_t alt = 3*1e3;
int16_t vx = 1*1e2;
int16_t vy = 2*1e2;
int16_t vz = -3*1e2;
int16_t xacc = 1*1e3;
int16_t yacc = 2*1e3;
int16_t zacc = 3*1e3;
mavlink_message_t msg;
mavlink_msg_hil_state_pack(m_system.sysid, m_system.compid, &msg,
get_board()->get_clock()->get_micros()/1000000.0,
roll,pitch,yaw,
rollRate,pitchRate,yawRate,
lat,lon,alt,
vx,vy,vz,
xacc,yacc,zacc);
sendMessage(msg);
}
void send(double * u, uint64_t timeStamp) {
// attitude states (rad)
float roll = u[0];
float pitch = u[1];
float yaw = u[2];
// body rates
float rollRate = u[3];
float pitchRate = u[4];
float yawRate = u[5];
// position
int32_t lat = u[6]*_rad2deg*1e7;
int32_t lon = u[7]*_rad2deg*1e7;
int16_t alt = u[8]*1e3;
int16_t vx = u[9]*1e2;
int16_t vy = u[10]*1e2;
int16_t vz = u[11]*1e2;
int16_t xacc = u[12]*1e3/_g0;
int16_t yacc = u[13]*1e3/_g0;
int16_t zacc = u[14]*1e3/_g0;
mavlink_message_t msg;
mavlink_msg_hil_state_pack(_system.sysid, _system.compid, &msg,
timeStamp,
roll,pitch,yaw,
rollRate,pitchRate,yawRate,
lat,lon,alt,
vx,vy,vz,
xacc,yacc,zacc);
_sendMessage(msg);
}