void dc_send_shot_position(void)
{
// angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi*10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta*10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi*10.0f);
// course in decideg
int16_t course = DegOfRad(*stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = (*stateGetHorizontalSpeedNorm_f()) * 10;
int16_t photo_nr = -1;
if (dc_photo_nr < DC_IMAGE_BUFFER) {
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
}
void dc_send_shot_position(void)
{
int16_t phi = DegOfRad(estimator_phi*10.0f);
int16_t theta = DegOfRad(estimator_theta*10.0f);
float gps_z = ((float)gps.hmsl) / 1000.0f;
int16_t course = (DegOfRad(gps.course)/((int32_t)1e6));
int16_t photo_nr = -1;
if (dc_buffer < DC_IMAGE_BUFFER) {
dc_buffer++;
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
DOWNLINK_SEND_DC_SHOT(DefaultChannel,
&photo_nr,
&gps.utm_pos.east,
&gps.utm_pos.north,
&gps_z,
&gps.utm_pos.zone,
&phi,
&theta,
&course,
&gps.gspeed,
&gps.tow);
}
void dc_send_shot_position(void)
{
// angles in decideg
int16_t phi = DegOfRad(stateGetNedToBodyEulers_f()->phi * 10.0f);
int16_t theta = DegOfRad(stateGetNedToBodyEulers_f()->theta * 10.0f);
int16_t psi = DegOfRad(stateGetNedToBodyEulers_f()->psi * 10.0f);
// course in decideg
int16_t course = DegOfRad(stateGetHorizontalSpeedDir_f()) * 10;
// ground speed in cm/s
uint16_t speed = stateGetHorizontalSpeedNorm_f() * 10;
int16_t photo_nr = -1;
if (dc_photo_nr < DC_IMAGE_BUFFER) {
dc_photo_nr++;
photo_nr = dc_photo_nr;
}
#if DC_SHOT_EXTRA_DL
// send a message on second datalink first
// (for instance an embedded CPU)
DOWNLINK_SEND_DC_SHOT(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
#endif
DOWNLINK_SEND_DC_SHOT(DefaultChannel, DefaultDevice,
&photo_nr,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&gps.hmsl,
&phi,
&theta,
&psi,
&course,
&speed,
&gps.tow);
}