Browse Source

Copter: integrate mount frontend-backend restructure

initialise mount on startup
use Mount::has_pan_control method
Remove calls to unimplemented mount.configure_cmd
Remove call to update_mount_type which will be handled from within mount
lib
mission-4.1.18
Randy Mackay 10 years ago committed by Andrew Tridgell
parent
commit
c51ba8cd03
  1. 8
      ArduCopter/ArduCopter.pde
  2. 10
      ArduCopter/commands_logic.pde
  3. 2
      ArduCopter/control_auto.pde
  4. 3
      ArduCopter/system.pde

8
ArduCopter/ArduCopter.pde

@ -659,7 +659,7 @@ static AP_HAL::AnalogSource* rssi_analog_source;
// -------------------------------------- // --------------------------------------
#if MOUNT == ENABLED #if MOUNT == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only. // current_loc uses the baro/gps soloution for altitude rather than gps only.
static AP_Mount camera_mount(&current_loc, ahrs, 0); static AP_Mount camera_mount(ahrs, current_loc);
#endif #endif
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -1018,7 +1018,7 @@ static void update_mount()
{ {
#if MOUNT == ENABLED #if MOUNT == ENABLED
// update camera mount's position // update camera mount's position
camera_mount.update_mount_position(); camera_mount.update();
#endif #endif
#if CAMERA == ENABLED #if CAMERA == ENABLED
@ -1134,10 +1134,6 @@ static void one_hz_loop()
// update assigned functions and enable auxiliar servos // update assigned functions and enable auxiliar servos
RC_Channel_aux::enable_aux_servos(); RC_Channel_aux::enable_aux_servos();
#if MOUNT == ENABLED
camera_mount.update_mount_type();
#endif
check_usb_mux(); check_usb_mux();
#if AP_TERRAIN_AVAILABLE #if AP_TERRAIN_AVAILABLE

10
ArduCopter/commands_logic.pde

@ -153,16 +153,6 @@ static bool start_command(const AP_Mission::Mission_Command& cmd)
break; break;
#endif #endif
#if MOUNT == ENABLED
case MAV_CMD_DO_MOUNT_CONFIGURE: // Mission command to configure a camera mount |Mount operation mode (see MAV_CONFIGURE_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
camera_mount.configure_cmd();
break;
case MAV_CMD_DO_MOUNT_CONTROL: // Mission command to control a camera mount |pitch(deg*100) or lat, depending on mount mode.| roll(deg*100) or lon depending on mount mode| yaw(deg*100) or alt (in cm) depending on mount mode| Empty| Empty| Empty| Empty|
camera_mount.control_cmd();
break;
#endif
#if PARACHUTE == ENABLED #if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE: // Mission command to configure or release parachute case MAV_CMD_DO_PARACHUTE: // Mission command to configure or release parachute
do_parachute(cmd); do_parachute(cmd);

2
ArduCopter/control_auto.pde

@ -590,7 +590,7 @@ static void set_auto_yaw_roi(const Location &roi_location)
}else{ }else{
#if MOUNT == ENABLED #if MOUNT == ENABLED
// check if mount type requires us to rotate the quad // check if mount type requires us to rotate the quad
if(camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll) { if(!camera_mount.has_pan_control()) {
roi_WP = pv_location_to_vector(roi_location); roi_WP = pv_location_to_vector(roi_location);
set_auto_yaw_mode(AUTO_YAW_ROI); set_auto_yaw_mode(AUTO_YAW_ROI);
} }

3
ArduCopter/system.pde

@ -231,6 +231,9 @@ static void init_ardupilot()
// initialise inertial nav // initialise inertial nav
inertial_nav.init(); inertial_nav.init();
// initialise camera mount
camera_mount.init();
#ifdef USERHOOK_INIT #ifdef USERHOOK_INIT
USERHOOK_INIT USERHOOK_INIT
#endif #endif

Loading…
Cancel
Save