Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 23:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: get example working again

Browse Source
This commit is contained in:
Peter Barker 2018-08-31 12:50:33 +10:00 committed by Randy Mackay
parent c54c2a2930
commit 774b091611
1 changed files with 47 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp
View File

@ -8,11 +8,41 @@
#include <AC_PID/AC_HELI_PID.h>
#include <RC_Channel/RC_Channel.h>
// we need a boardconfig created so that the io processor is available
#if HAL_WITH_IO_MCU || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_IOMCU/AP_IOMCU.h>
AP_BoardConfig BoardConfig;
#endif
void setup();
void loop();
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
class RC_Channel_PIDTest : public RC_Channel
{
};
class RC_Channels_PIDTest : public RC_Channels
{
public:
RC_Channel *channel(uint8_t chan) {
return &obj_channels[chan];
}
RC_Channel_PIDTest obj_channels[NUM_RC_CHANNELS];
private:
int8_t flight_mode_channel_number() const override { return -1; };
};
#define RC_CHANNELS_SUBCLASS RC_Channels_PIDTest
#define RC_CHANNEL_SUBCLASS RC_Channel_PIDTest
#include <RC_Channel/RC_Channels_VarInfo.h>
RC_Channels_PIDTest _rc;
// default PID values
#define TEST_P 1.0f
#define TEST_I 0.01f
@ -25,7 +55,13 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL();
// setup function
void setup()
{
hal.console->printf("ArduPilot Mega AC_PID library test\n");
hal.console->printf("ArduPilot AC_PID library test\n");
#if HAL_WITH_IO_MCU || CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
BoardConfig.init();
#endif
rc().init();
hal.scheduler->delay(1000);
}
@ -36,31 +72,29 @@ void loop()
// setup (unfortunately must be done here as we cannot create a global AC_PID object)
AC_PID pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100, TEST_FILTER, TEST_DT);
AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100, TEST_FILTER, TEST_DT, TEST_INITIAL_FF);
uint16_t radio_in;
uint16_t radio_trim;
int16_t error;
float control_P, control_I, control_D;
// display PID gains
hal.console->printf("P %f I %f D %f imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax());
RC_Channel *ch = rc().channel(0);
if (ch == nullptr) {
hal.console->printf("No channel 0?");
return;
while (true) {
hal.console->printf("No channel 0?");
hal.scheduler->delay(1000);
}
}
// capture radio trim
radio_trim = ch->get_radio_in();
const uint16_t radio_trim = ch->get_radio_in();
while (true) {
rc().read_input(); // poll the radio for new values
radio_in = ch->get_radio_in();
error = radio_in - radio_trim;
const uint16_t radio_in = ch->get_radio_in();
const int16_t error = radio_in - radio_trim;
pid.set_input_filter_all(error);
control_P = pid.get_p();
control_I = pid.get_i();
control_D = pid.get_d();
const float control_P = pid.get_p();
const float control_I = pid.get_i();
const float control_D = pid.get_d();
// display pid results
hal.console->printf("radio: %d\t err: %d\t pid:%4.2f (p:%4.2f i:%4.2f d:%4.2f)\n",