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);
}
/* Command The Camera */
void dc_send_command(uint8_t cmd)
{
switch (cmd) {
case DC_SHOOT:
// Send Photo Position To Camera
dc_shot_msg.data.nr = dc_photo_nr + 1;
dc_shot_msg.data.lat = stateGetPositionLla_i()->lat;
dc_shot_msg.data.lon = stateGetPositionLla_i()->lon;
dc_shot_msg.data.alt = stateGetPositionLla_i()->alt;
dc_shot_msg.data.phi = stateGetNedToBodyEulers_i()->phi;
dc_shot_msg.data.theta = stateGetNedToBodyEulers_i()->theta;
dc_shot_msg.data.psi = stateGetNedToBodyEulers_i()->psi;
dc_shot_msg.data.vground = *stateGetHorizontalSpeedNorm_i();
dc_shot_msg.data.course = *stateGetHorizontalSpeedDir_i();
dc_shot_msg.data.groundalt = POS_BFP_OF_REAL(state.alt_agl_f);
MoraHeader(MORA_SHOOT, MORA_SHOOT_MSG_SIZE);
for (int i = 0; i < (MORA_SHOOT_MSG_SIZE); i++) {
MoraPutUint8(dc_shot_msg.bin[i]);
}
MoraTrailer();
dc_send_shot_position();
break;
case DC_TALLER:
break;
case DC_WIDER:
break;
case DC_ON:
break;
case DC_OFF:
break;
default:
break;
}
}
uint8_t dc_info(void)
{
#ifdef DOWNLINK_SEND_DC_INFO
float course = DegOfRad(stateGetNedToBodyEulers_f()->psi);
int16_t mode = dc_autoshoot;
uint8_t shutter = dc_autoshoot_period * 10;
DOWNLINK_SEND_DC_INFO(DefaultChannel, DefaultDevice,
&mode,
&stateGetPositionLla_i()->lat,
&stateGetPositionLla_i()->lon,
&stateGetPositionLla_i()->alt,
&course,
&dc_photo_nr,
&dc_survey_interval,
&dc_gps_next_dist,
&dc_gps_x,
&dc_gps_y,
&dc_circle_start_angle,
&dc_circle_interval,
&dc_circle_last_block,
&dc_gps_count,
&shutter);
#endif
return 0;
}
/** set waypoint to current horizontal location without modifying altitude */
void waypoint_set_here_2d(uint8_t wp_id)
{
if (wp_id >= nb_waypoint) {
return;
}
if (waypoint_is_global(wp_id)) {
waypoint_set_latlon(wp_id, stateGetPositionLla_i());
} else {
waypoint_set_xy_i(wp_id, stateGetPositionEnu_i()->x, stateGetPositionEnu_i()->y);
}
}
/** set waypoint to current location and altitude */
void waypoint_set_here(uint8_t wp_id)
{
if (wp_id >= nb_waypoint) {
return;
}
if (waypoint_is_global(wp_id)) {
waypoint_set_lla(wp_id, stateGetPositionLla_i());
} else {
waypoint_set_enu_i(wp_id, stateGetPositionEnu_i());
}
}
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 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);
}
