Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsTri: reverse-frame cleanup and fixes 

- fix motor test order for reverse frame
- add frame type string for reverse frame
- fix initialization of _pitch_reversed flag
This commit is contained in:
Bob Long 2023-12-11 11:47:09 +11:00 committed by Andrew Tridgell
parent dc3f2c9724
commit 7e8f9c72fb
3 changed files with 55 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -48,9 +48,7 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
SRV_Channels::set_angle(SRV_Channels::get_motor_function(AP_MOTORS_CH_TRI_YAW), _yaw_servo_angle_max_deg*100);
// check for reverse tricopter
if (frame_type == MOTOR_FRAME_TYPE_PLUSREV) {
_pitch_reversed = true;
}
_pitch_reversed = frame_type == MOTOR_FRAME_TYPE_PLUSREV;
_mav_type = MAV_TYPE_TRICOPTER;
@ -62,11 +60,7 @@ void AP_MotorsTri::init(motor_frame_class frame_class, motor_frame_type frame_ty
void AP_MotorsTri::set_frame_class_and_type(motor_frame_class frame_class, motor_frame_type frame_type)
{
// check for reverse tricopter
if (frame_type == MOTOR_FRAME_TYPE_PLUSREV) {
_pitch_reversed = true;
} else {
_pitch_reversed = false;
}
_pitch_reversed = frame_type == MOTOR_FRAME_TYPE_PLUSREV;
set_initialised_ok((frame_class == MOTOR_FRAME_TRI) && SRV_Channels::function_assigned(SRV_Channel::k_motor7));
}
@ -286,6 +280,7 @@ void AP_MotorsTri::output_armed_stabilizing()
void AP_MotorsTri::_output_test_seq(uint8_t motor_seq, int16_t pwm)
{
// output to motors and servos
if (!_pitch_reversed) {
switch (motor_seq) {
case 1:
// front right motor
@ -307,6 +302,29 @@ void AP_MotorsTri::_output_test_seq(uint8_t motor_seq, int16_t pwm)
// do nothing
break;
}
} else {
switch (motor_seq) {
case 1:
// front motor
rc_write(AP_MOTORS_MOT_4, pwm);
break;
case 2:
// front servo
rc_write(AP_MOTORS_CH_TRI_YAW, pwm);
break;
case 3:
// back right motor
rc_write(AP_MOTORS_MOT_1, pwm);
break;
case 4:
// back left motor
rc_write(AP_MOTORS_MOT_2, pwm);
break;
default:
// do nothing
break;
}
}
}
/*
@ -404,6 +422,7 @@ bool AP_MotorsTri::arming_checks(size_t buflen, char *buffer) const
// Get the testing order for the motors
uint8_t AP_MotorsTri::get_motor_test_order(uint8_t i)
{
if (!_pitch_reversed) {
switch (i) {
case AP_MOTORS_MOT_1: // front right motor
return 1;
@ -416,5 +435,19 @@ uint8_t AP_MotorsTri::get_motor_test_order(uint8_t i)
default:
return 0;
}
} else {
switch (i) {
case AP_MOTORS_MOT_4: // front motor
return 1;
case AP_MOTORS_CH_TRI_YAW: // front servo
return 2;
case AP_MOTORS_MOT_1: // back right motor
return 3;
case AP_MOTORS_MOT_2: // back left motor
return 4;
default:
return 0;
}
}
}
#endif // APM_BUILD_TYPE(APM_BUILD_UNKNOWN)

1
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -65,6 +65,7 @@ protected:
void thrust_compensation(void) override;
const char* _get_frame_string() const override { return "TRI"; }
const char* get_type_string() const override { return _pitch_reversed ? "pitch-reversed" : ""; }
// output_test_seq - spin a motor at the pwm value specified
// motor_seq is the motor's sequence number from 1 to the number of motors on the frame

2
libraries/AP_Motors/examples/AP_Motors_test/AP_Motors_test.cpp
View File

@ -391,7 +391,7 @@ void print_motor_tri(uint8_t frame_class, uint8_t frame_type)
hal.console->printf("\t\t\t\"Class\": %i,\n", frame_class);
hal.console->printf("\t\t\t\"ClassName\": \"%s\",\n", frame_string+2);
hal.console->printf("\t\t\t\"Type\": %i,\n", frame_type);
hal.console->printf("\t\t\t\"TypeName\": \"%s\",\n", (type_string != nullptr) ? type_string + 1 : "?");
hal.console->printf("\t\t\t\"TypeName\": \"%s\",\n", (type_string != nullptr) ? type_string + 1 : "default");
hal.console->printf("\t\t\t\"motors\": [\n");
bool first_motor = true;
for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {