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

Blimp: Fix mode land, manual and fins class

This commit is contained in:
Michelle Rossouw 2021-06-24 00:17:34 +10:00 committed by Andrew Tridgell
parent 3bfd577a49
commit cceb612d0c
4 changed files with 17 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
Blimp/Fins.cpp
View File

@ -34,9 +34,9 @@ Fins::Fins(uint16_t loop_rate) :
void Fins::setup_fins()
{
//amp r f d y,off r f d y right, front, down, yaw
add_fin(0, 0, -1, 0.5, 0, 0, 0, -0.5, 0); //Back(?)
add_fin(1, 0, 1, 0.5, 0, 0, 0, -0.5, 0); //Front(?)
//fin # r f d y, r f d y right, front, down, yaw for amplitude then for offset
add_fin(0, 0, 1, 0.5, 0, 0, 0, 0.5, 0); //Back
add_fin(1, 0, -1, 0.5, 0, 0, 0, 0.5, 0); //Front
add_fin(2, -1, 0, 0, 0.5, 0, 0, 0, 0.5); //Right
add_fin(3, 1, 0, 0, 0.5, 0, 0, 0, -0.5); //Left
@ -78,25 +78,21 @@ void Fins::output()
yaw_out = 0;
}
right_out /= RC_SCALE;
front_out /= RC_SCALE;
down_out /= RC_SCALE;
yaw_out /= RC_SCALE;
_time = AP_HAL::micros() * 1.0e-6;
for (int8_t i=0; i<NUM_FINS; i++) {
_amp[i] = max(0,_right_amp_factor[i]*right_out) + max(0,_front_amp_factor[i]*front_out) +
_amp[i] = fmaxf(0,_right_amp_factor[i]*right_out) + fmaxf(0,_front_amp_factor[i]*front_out) +
fabsf(_down_amp_factor[i]*down_out) + fabsf(_yaw_amp_factor[i]*yaw_out);
_off[i] = _right_off_factor[i]*right_out + _front_off_factor[i]*front_out +
_down_off_factor[i]*down_out + _yaw_off_factor[i]*yaw_out;
_freq[i] = 1;
_num_added = 0;
if (max(0,_right_amp_factor[i]*right_out) > 0.0f) {
if (fmaxf(0,_right_amp_factor[i]*right_out) > 0.0f) {
_num_added++;
}
if (max(0,_front_amp_factor[i]*front_out) > 0.0f) {
if (fmaxf(0,_front_amp_factor[i]*front_out) > 0.0f) {
_num_added++;
}
if (fabsf(_down_amp_factor[i]*down_out) > 0.0f) {

12
Blimp/Fins.h
View File

@ -84,15 +84,6 @@ public:
bool _interlock; // 1 if the motor interlock is enabled (i.e. motors run), 0 if disabled (motors don't run)
bool _initialised_ok; // 1 if initialisation was successful
float max(float one, float two)
{
if (one >= two) {
return one;
} else {
return two;
}
}
void output_min();
void add_fin(int8_t fin_num, float right_amp_fac, float front_amp_fac, float yaw_amp_fac, float down_amp_fac,
@ -104,7 +95,8 @@ public:
float get_throttle()
{
return 0.1f; //TODO
//Only for Mavlink - essentially just an indicator of how hard the fins are working.
return fmaxf(fmaxf(fabsf(down_out),fabsf(front_out)), fmaxf(fabsf(right_out),fabsf(yaw_out)));
}
void rc_write(uint8_t chan, uint16_t pwm);

7
Blimp/mode_land.cpp
View File

@ -6,8 +6,11 @@
// Runs the main land controller
void ModeLand::run()
{
//stop moving
//Stop moving
motors->right_out = 0;
motors->front_out = 0;
motors->yaw_out = 0;
motors->down_out = 0;
}
// set_mode_land_with_pause - sets mode to LAND and triggers 4 second delay before descent starts

8
Blimp/mode_manual.cpp
View File

@ -6,8 +6,8 @@
// Runs the main manual controller
void ModeManual::run()
{
motors->right_out = channel_right->get_control_in();
motors->front_out = channel_front->get_control_in();
motors->yaw_out = channel_yaw->get_control_in();
motors->down_out = channel_down->get_control_in();
motors->right_out = channel_right->get_control_in() / float(RC_SCALE);
motors->front_out = channel_front->get_control_in() / float(RC_SCALE);
motors->yaw_out = channel_yaw->get_control_in() / float(RC_SCALE);
motors->down_out = channel_down->get_control_in() / float(RC_SCALE);
}