Browse Source

Copter: RCMAP Fix, remove RC_Channel references

from AP_Motors objects.  And a few function renaming to follow changes in AP_Motors.  Also add new throttle channel setter functionality.
Remove RC7 object from Tricopter.  Add special Tricopter param handling.
master
Robert Lefebvre 10 years ago committed by Randy Mackay
parent
commit
acdf4a226f
  1. 13
      ArduCopter/ArduCopter.pde
  2. 8
      ArduCopter/Log.pde
  3. 5
      ArduCopter/Parameters.pde
  4. 2
      ArduCopter/land_detector.pde
  5. 2
      ArduCopter/radio.pde

13
ArduCopter/ArduCopter.pde

@ -428,15 +428,15 @@ static struct { @@ -428,15 +428,15 @@ static struct {
#endif
#if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments
static MOTOR_CLASS motors(*channel_roll, *channel_pitch, *channel_throttle, *channel_yaw, g.rc_7, g.rc_8, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE);
static MOTOR_CLASS motors(g.rc_7, g.rc_8, g.heli_servo_1, g.heli_servo_2, g.heli_servo_3, g.heli_servo_4, MAIN_LOOP_RATE);
#elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing
static MOTOR_CLASS motors(*channel_roll, *channel_pitch, *channel_throttle, *channel_yaw, g.rc_7, MAIN_LOOP_RATE);
static MOTOR_CLASS motors(MAIN_LOOP_RATE);
#elif FRAME_CONFIG == SINGLE_FRAME // single constructor requires extra servos for flaps
static MOTOR_CLASS motors(*channel_roll, *channel_pitch, *channel_throttle, *channel_yaw, g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4, MAIN_LOOP_RATE);
static MOTOR_CLASS motors(g.single_servo_1, g.single_servo_2, g.single_servo_3, g.single_servo_4, MAIN_LOOP_RATE);
#elif FRAME_CONFIG == COAX_FRAME // single constructor requires extra servos for flaps
static MOTOR_CLASS motors(*channel_roll, *channel_pitch, *channel_throttle, *channel_yaw, g.single_servo_1, g.single_servo_2, MAIN_LOOP_RATE);
static MOTOR_CLASS motors(g.single_servo_1, g.single_servo_2, MAIN_LOOP_RATE);
#else
static MOTOR_CLASS motors(*channel_roll, *channel_pitch, *channel_throttle, *channel_yaw, MAIN_LOOP_RATE);
static MOTOR_CLASS motors(MAIN_LOOP_RATE);
#endif
////////////////////////////////////////////////////////////////////////////////
@ -1065,6 +1065,9 @@ static void one_hz_loop() @@ -1065,6 +1065,9 @@ static void one_hz_loop()
// check the user hasn't updated the frame orientation
motors.set_frame_orientation(g.frame_orientation);
// set all throttle channel settings
motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);
}
// update assigned functions and enable auxiliar servos

8
ArduCopter/Log.pde

@ -411,16 +411,16 @@ static void Log_Write_Rate() @@ -411,16 +411,16 @@ static void Log_Write_Rate()
time_ms : hal.scheduler->millis(),
control_roll : (float)rate_targets.x,
roll : (float)(ahrs.get_gyro().x * AC_ATTITUDE_CONTROL_DEGX100),
roll_out : (float)(motors.get_roll()),
roll_out : (motors.get_roll()),
control_pitch : (float)rate_targets.y,
pitch : (float)(ahrs.get_gyro().y * AC_ATTITUDE_CONTROL_DEGX100),
pitch_out : (float)(motors.get_pitch()),
pitch_out : (motors.get_pitch()),
control_yaw : (float)rate_targets.z,
yaw : (float)(ahrs.get_gyro().z * AC_ATTITUDE_CONTROL_DEGX100),
yaw_out : (float)(motors.get_yaw()),
yaw_out : (motors.get_yaw()),
control_accel : (float)accel_target.z,
accel : (float)(-(ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f),
accel_out : (float)(motors.get_throttle_out())
accel_out : (motors.get_throttle())
};
DataFlash.WriteBlock(&pkt_rate, sizeof(pkt_rate));
}

5
ArduCopter/Parameters.pde

@ -976,6 +976,11 @@ const AP_Param::Info var_info[] PROGMEM = { @@ -976,6 +976,11 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_Motors/AP_MotorsCoax.cpp
GOBJECT(motors, "MOT_", AP_MotorsCoax),
#elif FRAME_CONFIG == TRI_FRAME
// @Group: MOT_
// @Path: ../libraries/AP_Motors/AP_MotorsTri.cpp
GOBJECT(motors, "MOT_", AP_MotorsTri),
#else
// @Group: MOT_
// @Path: ../libraries/AP_Motors/AP_Motors_Class.cpp

2
ArduCopter/land_detector.pde

@ -46,7 +46,7 @@ static void update_land_detector() @@ -46,7 +46,7 @@ static void update_land_detector()
// we've sensed movement up or down so reset land_detector
land_detector = 0;
// if throttle output is high then clear landing flag
if (motors.get_throttle_out() > get_non_takeoff_throttle()) {
if (motors.get_throttle() > get_non_takeoff_throttle()) {
set_land_complete(false);
}
}

2
ArduCopter/radio.pde

@ -54,7 +54,7 @@ static void init_rc_out() @@ -54,7 +54,7 @@ static void init_rc_out()
motors.set_update_rate(g.rc_speed);
motors.set_frame_orientation(g.frame_orientation);
motors.Init(); // motor initialisation
motors.set_min_throttle(g.throttle_min);
motors.set_throttle_range(g.throttle_min, channel_throttle->radio_min, channel_throttle->radio_max);
for(uint8_t i = 0; i < 5; i++) {
delay(20);

Loading…
Cancel
Save