Hi! Jimit here! I am working with a position controller for my drone, I use EKF for position estimation, and my loop was 50Hz, I designed a controller 1Khz rate loop, 250Hz Angle Controller loop, 100Hz Altitude Hold using baro, and posHold 50Hz loop, I have a question about how I combine roll, pitch setpoint to position controller? I use an "NED" frame, I give you the code to my GPS pos hold. can you guide me, am I doing right? All sensors are properly calibrated, and the controller work awesome, except PosHold, please guide me, the controller does not tune very well, I dought my math was wrong or right, so I increase or decrease the PD parameter but not the tune!
void gpsLOOP()
{
GPS_COUNTER++;
if (GPS_COUNTER == 5)
{
GPS_COUNTER = 0;
prevTimeIMU = currentTimeIMU;
currentTimeIMU = (float)micros();
dtIMU = (currentTimeIMU - prevTimeIMU) / 1000000.0f;
dtIMUFiltered = 0.85 * dtIMUFiltered + 0.15 * dtIMU;
if (estimation.NUM_OF_SAT < 8)
{
digitalWrite(13, !digitalRead(13));
digitalWrite(40, !digitalRead(40));
}
else
{
digitalWrite(13, HIGH);
digitalWrite(40, HIGH);
}
estimation.Update(dtIMUFiltered, ekf.xhat, mAcc);
GPS_WATCHDOG_TIMER = micros();
float pos_X = estimation.state(3) * 100.0f;
float pos_Y = estimation.state(4) * 100.0f;
float vel_Y = estimation.state(1) * 100.0f;
float vel_X = estimation.state(0) * 100.0f;
if (FlightMode.FLGIHT_MODE >= 3 && WAYPOINT_SET == 0)
{
WAYPOINT_SET = 1;
SETPOINT_NORTH = pos_X;
SETPOINT_EAST = pos_Y;
// SETPOINT_NORTH = estimation.state(3);
// SETPOINT_EAST = estimation.state(4);
}
if (FlightMode.FLGIHT_MODE >= 3 && WAYPOINT_SET == 1)
{
// POS_NORTH_ERROR = estimation.state(3) - SETPOINT_NORTH;
// POS_EAST_ERROR = SETPOINT_EAST - estimation.state(4);
POS_NORTH_ERROR = pos_X - SETPOINT_NORTH;
POS_EAST_ERROR = pos_Y - SETPOINT_EAST;
POS_NORTH_TOTAL_AVERAGE -= POS_NORTH_ROTATING_MEM[INDEX];
POS_NORTH_ROTATING_MEM[INDEX] = POS_NORTH_ERROR - POS_NORTH_ERROR_PREVIOUS;
POS_NORTH_TOTAL_AVERAGE += POS_NORTH_ROTATING_MEM[INDEX];
POS_EAST_TOTAL_AVERAGE -= POS_EAST_ROTATING_MEM[INDEX];
POS_EAST_ROTATING_MEM[INDEX] = POS_EAST_ERROR - POS_EAST_ERROR_PREVIOUS;
POS_EAST_TOTAL_AVERAGE += POS_EAST_ROTATING_MEM[INDEX];
INDEX++;
if (INDEX == 35)
INDEX = 0;
POS_NORTH_ERROR_PREVIOUS = POS_NORTH_ERROR;
POS_EAST_ERROR_PREVIOUS = POS_EAST_ERROR;
// GPS_PITCH_ADJUST_NORTH = POS_NORTH_ERROR * GPS_P_GAIN + POS_NORTH_TOTAL_AVERAGE * GPS_D_GAIN;
// GPS_ROLL_ADJUST_NORTH = POS_EAST_ERROR * GPS_P_GAIN - POS_EAST_TOTAL_AVERAGE * GPS_D_GAIN;
GPS_PITCH_ADJUST_NORTH = POS_NORTH_ERROR * GPS_P_GAIN + vel_X * GPS_D_GAIN;
GPS_ROLL_ADJUST_NORTH = POS_EAST_ERROR * GPS_P_GAIN + vel_Y * GPS_D_GAIN;
GPS_PITCH_ADJUST_NORTH *= -1.0f;
GPS_PITCH_ADJUST = GPS_PITCH_ADJUST_NORTH * cos(ANGLE_YAW) - GPS_ROLL_ADJUST_NORTH * sin(ANGLE_YAW);
GPS_ROLL_ADJUST = GPS_PITCH_ADJUST_NORTH * sin(ANGLE_YAW) + GPS_ROLL_ADJUST_NORTH * cos(ANGLE_YAW);
// GPS_ROLL_ADJUST = GPS_ROLL_ADJUST_NORTH * cos(ANGLE_YAW) + GPS_PITCH_ADJUST_NORTH * cos(ANGLE_YAW - (PI / 2));
// GPS_PITCH_ADJUST = GPS_PITCH_ADJUST_NORTH * cos(ANGLE_YAW) + GPS_ROLL_ADJUST_NORTH * cos(ANGLE_YAW + (PI / 2));
GPS_ROLL_ADJUST = constrain(GPS_ROLL_ADJUST, -300.0f, 300.0f);
GPS_PITCH_ADJUST = constrain(GPS_PITCH_ADJUST, -300.0f, 300.0f);
}
}
if (GPS_WATCHDOG_TIMER + 1000000 < micros())
{ // If the watchdog timer is exceeded the GPS signal is missing.
if (FlightMode.FLGIHT_MODE >= 3 && FlightMode.START > 0)
{ // If flight mode is set to 3 (GPS hold).
FlightMode.FLGIHT_MODE = 2; // Set the flight mode to 2.
FlightMode.ERROR = 4; // Output an error.
}
}
if (FlightMode.FLGIHT_MODE < 3 && WAYPOINT_SET > 0)
{ // If the GPS hold mode is disabled and the waypoints are set.
GPS_ROLL_ADJUST = 0.0f; // Reset the gps_roll_adjust variable to disable the correction.
GPS_PITCH_ADJUST = 0.0f; // Reset the gps_pitch_adjust variable to disable the correction.
if (WAYPOINT_SET == 1)
{ // If the waypoints are stored
INDEX = 0; // Set the gps_rotating_mem_location to zero so we can empty the
WAYPOINT_SET = 2; // Set the waypoint_set variable to 2 as an indication that the buffer is not cleared.
}
POS_NORTH_ROTATING_MEM[INDEX] = 0; // Reset the current gps_lon_rotating_mem location.
POS_EAST_ROTATING_MEM[INDEX] = 0; // Reset the current gps_lon_rotating_mem location.
INDEX++; // Increment the gps_rotating_mem_location variable for the next loop.
if (INDEX == 36)
{ // If the gps_rotating_mem_location equals 36, all the buffer locations are cleared.
WAYPOINT_SET = 0; // Reset the waypoint_set variable to 0.
// Reset the variables that are used for the D-controller.
POS_NORTH_ERROR_PREVIOUS = 0.0f;
POS_EAST_ERROR_PREVIOUS = 0.0f;
POS_EAST_TOTAL_AVERAGE = 0.0f;
POS_NORTH_TOTAL_AVERAGE = 0.0f;
INDEX = 0;
}
}
}
here is how I implement GPS PID output.
gpsLOOP();
if (FlightMode.FLGIHT_MODE >= 3 && WAYPOINT_SET == 1)
{
ROLL_SETPOINT_BASE += GPS_ROLL_ADJUST;
PITCH_SETPOINT_BASE += GPS_PITCH_ADJUST;
}
ROLL_SETPOINT_BASE = constrain(ROLL_SETPOINT_BASE, 1000.0f, 2000.0f);
PITCH_SETPOINT_BASE = constrain(PITCH_SETPOINT_BASE, 1000.0f, 2000.0f);
rollSetpoint = (ROLL_SETPOINT_BASE - 1500.0f) / 500.0f;
pitchSetpoint = (PITCH_SETPOINT_BASE - 1500.0f) / 500.0f;
rollSetpoint = constrain(rollSetpoint, -1.0f, 1.0f) * MAX_ROLL_PITCH;
pitchSetpoint = constrain(pitchSetpoint, -1.0f, 1.0f) * MAX_ROLL_PITCH;