void cam_ac_target(void)
{
struct EnuCoor_f ac_pos *ac = acInfoGetPositionEnu_f(cam_target_ac);
cam_target_x = ac->x;
cam_target_y = ac->y;
cam_target_alt = acInfoGetPositionUtm_f()->alt;
cam_target();
}
void cam_waypoint_target( void ) {
if (cam_target_wp < nb_waypoint) {
cam_target_x = waypoints[cam_target_wp].x;
cam_target_y = waypoints[cam_target_wp].y;
}
cam_target_alt = ground_alt;
cam_target();
}
void cam_waypoint_target( void ) {
if (cam_target_wp < nb_waypoint) {
cam_target_x = WaypointX(cam_target_wp);
cam_target_y = WaypointY(cam_target_wp);
}
cam_target_alt = ground_alt;
cam_target();
}
void cam_ac_target( void ) {
#ifdef TRAFFIC_INFO
struct ac_info_ * ac = get_ac_info(cam_target_ac);
cam_target_x = ac->east;
cam_target_y = ac->north;
cam_target_alt = ac->alt;
cam_target();
#endif // TRAFFIC_INFO
}
/** Point straight down */
void cam_nadir( void ) {
#ifdef TEST_CAM
cam_target_x = test_cam_estimator_x;
cam_target_y = test_cam_estimator_y;
#else
cam_target_x = estimator_x;
cam_target_y = estimator_y;
#endif
cam_target_alt = 0;
cam_target();
}
/** Point straight down */
void cam_nadir( void ) {
struct EnuCoor_f* pos = stateGetPositionEnu_f();
#ifdef TEST_CAM
cam_target_x = test_cam_estimator_x;
cam_target_y = test_cam_estimator_y;
#else
cam_target_x = pos->x;
cam_target_y = pos->y;
#endif
cam_target_alt = -10;
cam_target();
}
void cam_periodic( void ) {
ap_state->commands[COMMAND_CAM_SWITCH] = -9600+(pprz_t)cam_switch*19200;
switch (cam_mode) {
case CAM_MODE_OFF:
break;
case CAM_MODE_ANGLES:
cam_angles();
break;
case CAM_MODE_NADIR:
cam_nadir();
break;
case CAM_MODE_XY_TARGET:
cam_target();
break;
case CAM_MODE_WP_TARGET:
cam_waypoint_target();
break;
case CAM_MODE_AC_TARGET:
cam_ac_target();
break;
}
}
void cam_periodic(void)
{
#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
//Position the camera for straight view.
if (pprz_mode == PPRZ_MODE_AUTO2) {
#endif
switch (cam_mode) {
case CAM_MODE_OFF:
cam_pan_c = RadOfDeg(CAM_PAN0);
cam_tilt_c = RadOfDeg(CAM_TILT0);
cam_angles();
break;
case CAM_MODE_ANGLES:
cam_angles();
break;
case CAM_MODE_NADIR:
cam_nadir();
break;
case CAM_MODE_XY_TARGET:
cam_target();
break;
case CAM_MODE_WP_TARGET:
cam_waypoint_target();
break;
case CAM_MODE_AC_TARGET:
cam_ac_target();
break;
// In this mode the target coordinates are calculated continuously from the pan and tilt radio channels.
// The "TARGET" waypoint coordinates are not used.
// If the "-DSHOW_CAM_COORDINATES" is defined then the coordinates of where the camera is looking are calculated.
case CAM_MODE_STABILIZED:
cam_waypoint_target();
break;
// In this mode the angles come from the pan and tilt radio channels.
// The "TARGET" waypoint coordinates are not used but i need to call the "cam_waypoint_target()" function
// in order to calculate the coordinates of where the camera is looking.
case CAM_MODE_RC:
cam_waypoint_target();
break;
default:
break;
}
#if defined(CAM_FIXED_FOR_FPV_IN_AUTO1) && CAM_FIXED_FOR_FPV_IN_AUTO1 == 1
} else if (pprz_mode == PPRZ_MODE_AUTO1) {
//Position the camera for straight view.
#if defined(CAM_TILT_POSITION_FOR_FPV)
cam_tilt_c = RadOfDeg(CAM_TILT_POSITION_FOR_FPV);
#else
cam_tilt_c = RadOfDeg(90);
#endif
#if defined(CAM_PAN_POSITION_FOR_FPV)
cam_pan_c = RadOfDeg(CAM_PAN_POSITION_FOR_FPV);
#else
cam_pan_c = RadOfDeg(0);
#endif
cam_angles();
#ifdef SHOW_CAM_COORDINATES
cam_point_lon = 0;
cam_point_lat = 0;
cam_point_distance_from_home = 0;
#endif
}
#endif
#if defined(COMMAND_CAM_PWR_SW)
if (video_tx_state) { ap_state->commands[COMMAND_CAM_PWR_SW] = MAX_PPRZ; } else { ap_state->commands[COMMAND_CAM_PWR_SW] = MIN_PPRZ; }
#elif defined(VIDEO_TX_SWITCH)
if (video_tx_state) { LED_OFF(VIDEO_TX_SWITCH); } else { LED_ON(VIDEO_TX_SWITCH); }
#endif
}
