Sub: Added frame type BLUEROV to configure use with the new AP_MotorsBlueROV class.

This commit is contained in:
Rustom Jehangir 2016-01-01 15:31:18 -08:00 committed by Andrew Tridgell
parent e8464d73a9
commit 8b7ea6223f
8 changed files with 53 additions and 7 deletions

View File

@ -21,20 +21,20 @@
// uncomment the lines below to disable features (flash sizes listed are for APM2 boards and will underestimate savings on Pixhawk and other boards)
//#define LOGGING_ENABLED DISABLED // disable dataflash logging to save 11K of flash space
//#define MOUNT DISABLED // disable the camera gimbal to save 8K of flash space
#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
#define AC_FENCE DISABLED // disable fence to save 2k of flash
//#define AUTOTUNE_ENABLED DISABLED // disable the auto tune functionality to save 7k of flash
//#define AC_FENCE DISABLED // disable fence to save 2k of flash
//#define CAMERA DISABLED // disable camera trigger to save 1k of flash
//#define CONFIG_SONAR DISABLED // disable sonar to save 1k of flash
#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally)
#define AC_TERRAIN DISABLED // disable terrain library
#define PARACHUTE DISABLED // disable parachute release to save 1k of flash
//#define POSHOLD_ENABLED DISABLED // disable PosHold flight mode to save 4.5k of flash
//#define AC_RALLY DISABLED // disable rally points library (must also disable terrain which relies on rally)
//#define AC_TERRAIN DISABLED // disable terrain library
//#define PARACHUTE DISABLED // disable parachute release to save 1k of flash
//#define EPM_ENABLED DISABLED // disable epm cargo gripper to save 500bytes of flash
//#define CLI_ENABLED DISABLED // disable the CLI (command-line-interface) to save 21K of flash space
//#define NAV_GUIDED DISABLED // disable external navigation computer ability to control vehicle through MAV_CMD_NAV_GUIDED mission commands
//#define OPTFLOW DISABLED // disable optical flow sensor to save 5K of flash space
//#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry
#define ADSB_ENABLED DISABLED // disable ADSB support
//#define ADSB_ENABLED DISABLED // disable ADSB support
// features below are disabled by default on all boards
//#define SPRAYER ENABLED // enable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)

View File

@ -261,9 +261,12 @@ void Copter::fast_loop()
// run low level rate controllers that only require IMU data
attitude_control.rate_controller_run();
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
motors.set_forward(channel_forward->control_in);
motors.set_strafe(channel_strafe->control_in);
=======
>>>>>>> Changed to ArduCopter as the base code.
#if FRAME_CONFIG == HELI_FRAME
update_heli_control_dynamics();
#endif //HELI_FRAME

View File

@ -162,8 +162,11 @@ private:
RC_Channel *channel_pitch;
RC_Channel *channel_throttle;
RC_Channel *channel_yaw;
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
RC_Channel *channel_forward;
RC_Channel *channel_strafe;
=======
>>>>>>> Changed to ArduCopter as the base code.
// Dataflash
DataFlash_Class DataFlash{FIRMWARE_STRING};
@ -313,8 +316,11 @@ private:
#define MOTOR_CLASS AP_MotorsSingle
#elif FRAME_CONFIG == COAX_FRAME
#define MOTOR_CLASS AP_MotorsCoax
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
#elif FRAME_CONFIG == BLUEROV
#define MOTOR_CLASS AP_MotorsBlueROV
=======
>>>>>>> Changed to ArduCopter as the base code.
#else
#error Unrecognised frame type
#endif
@ -811,8 +817,11 @@ private:
void sport_run();
bool stabilize_init(bool ignore_checks);
void stabilize_run();
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
bool rov_init(bool ignore_checks);
void rov_run();
=======
>>>>>>> Changed to ArduCopter as the base code.
void crash_check();
void parachute_check();
void parachute_release();

View File

@ -92,8 +92,11 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
MAV_TYPE_ROCKET,
#elif (FRAME_CONFIG == COAX_FRAME) //because mavlink did not define a singlecopter, we use a rocket
MAV_TYPE_ROCKET,
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
#elif (FRAME_CONFIG == BLUEROV)
MAV_TYPE_SUBMARINE,
=======
>>>>>>> Changed to ArduCopter as the base code.
#else
#error Unrecognised frame type
#endif

View File

@ -88,8 +88,11 @@
# define FRAME_CONFIG_STRING "SINGLE"
#elif FRAME_CONFIG == COAX_FRAME
# define FRAME_CONFIG_STRING "COAX"
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
#elif FRAME_CONFIG == BLUEROV
# define FRAME_CONFIG_STRING "BLUEROV"
=======
>>>>>>> Changed to ArduCopter as the base code.
#else
# define FRAME_CONFIG_STRING "UNKNOWN"
#endif

View File

@ -79,7 +79,10 @@ enum aux_sw_func {
#define OCTA_QUAD_FRAME 7
#define SINGLE_FRAME 8
#define COAX_FRAME 9
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
#define BLUEROV 10
=======
>>>>>>> Changed to ArduCopter as the base code.
// HIL enumerations
#define HIL_MODE_DISABLED 0

View File

@ -13,12 +13,16 @@ static uint32_t auto_disarm_begin;
// called at 10hz
void Copter::arm_motors_check()
{
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
// Arm motors automatically
if ( !motors.armed() ) {
init_arm_motors(false);
}
/*static int16_t arming_counter;
=======
static int16_t arming_counter;
>>>>>>> Changed to ArduCopter as the base code.
// ensure throttle is down
if (channel_throttle->control_in > 0) {
@ -71,13 +75,21 @@ void Copter::arm_motors_check()
// Yaw is centered so reset arming counter
}else{
arming_counter = 0;
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
}*/
=======
}
>>>>>>> Changed to ArduCopter as the base code.
}
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
void Copter::auto_disarm_check()
{
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
/*uint32_t tnow_ms = millis();
=======
uint32_t tnow_ms = millis();
>>>>>>> Changed to ArduCopter as the base code.
uint32_t disarm_delay_ms = 1000*constrain_int16(g.disarm_delay, 0, 127);
// exit immediately if we are already disarmed, or if auto
@ -122,7 +134,11 @@ void Copter::auto_disarm_check()
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
init_disarm_motors();
auto_disarm_begin = tnow_ms;
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
}*/
=======
}
>>>>>>> Changed to ArduCopter as the base code.
}
// init_arm_motors - performs arming process including initialisation of barometer and gyros

View File

@ -27,22 +27,31 @@ void Copter::init_rc_in()
channel_pitch = RC_Channel::rc_channel(rcmap.pitch()-1);
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
channel_yaw = RC_Channel::rc_channel(rcmap.yaw()-1);
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
channel_forward = RC_Channel::rc_channel(rcmap.forward()-1);
channel_strafe = RC_Channel::rc_channel(rcmap.strafe()-1);
=======
>>>>>>> Changed to ArduCopter as the base code.
// set rc channel ranges
channel_roll->set_angle(ROLL_PITCH_INPUT_MAX);
channel_pitch->set_angle(ROLL_PITCH_INPUT_MAX);
channel_yaw->set_angle(4500);
channel_throttle->set_range(g.throttle_min, THR_MAX);
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
channel_forward->set_angle(4500);
channel_strafe->set_angle(4500);
=======
>>>>>>> Changed to ArduCopter as the base code.
channel_roll->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_pitch->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_yaw->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
<<<<<<< 6dafedb2d1ad5061d859a9c319fa4b69b4ac5dd9
channel_forward->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
channel_strafe->set_type(RC_CHANNEL_TYPE_ANGLE_RAW);
=======
>>>>>>> Changed to ArduCopter as the base code.
//set auxiliary servo ranges
g.rc_5.set_range(0,1000);