APM1 hardware and Arduplane code Description and Explanation

As I can see on the forum that many people like to understand the code of Arduplane like me. But there's no appropriate documentation on the web because it's hard to understand the software without undrstanding the hardware, so I decide to find out myself. After a lot of code reading and brainstorming I've come to the draft below. For hardware understanding, please look at APM1 block diagram for more information 
http://diydrones.com/forum/topics/apm-block-diagram?page=1&comm...
 It's still far from complete however I hope it can help those who likes to understand the code.

APM1 hardware and Arduplane code Description and Explanation

ArduPlane Version 2.26
This draft is based partly on APM code outline

https://docs.google.com/document/d/1OCEMtCq7Njr-YeEroSsMPQCZNP0QvMx...

ArduPlane.pde
--------------
Here only normal flight mode's functions are discussed. HIL modes and telemetry are out of the scope.
All the basic system and  flight variables and structures  are declared at the top of this file.  User configurations are in APM_config.h,
g defined as Parameter class entity (Parameter.h)
static Parameters      g;
    // PID controllers
    PID         pidNavRoll;
    PID         pidServoRoll;
    PID         pidServoPitch;
    PID         pidNavPitchAirspeed;
    PID         pidServoRudder;
    PID         pidTeThrottle;
    PID         pidNavPitchAltitude;
From parameters.h it's can be seen that plane's controllers are PID controllers including attitude angle controllers and servo controllers

// All GPS access should be through this pointer.
static GPS         *g_gps;

setup() Basic Initialization
Calls memcheck_init() in libraries/memcheck/memcheck.cpp and calls init_ardupilot() in system.pde to init serial ports, ADC, IMU, servos, RC and telemetry.

loop() Main system loop. Also know as the outside loop. This calls fast_loop and medium_loop.

fast_loop() This loop should execute at 50Hz if possible to ensure servos update frequency. In the table below it is called Floop, medium_loop counter = ... is called
MloopC=...

      -------Floop  -------------------------------------------------MloopC=0  update_GPS()
      |                 read_radio()                                          
      |                 dcm.update_DCM(); calc_bearing_error()
Delta_ms_fast_loop        update_current_flight_mode();                         
      |                 if (control_mode > MANUAL)  stabilize();              
      |                 set_servos();                                         
      -------Floop  -------------------------------------------------MloopC=1  navigate()  nav_bearing = target_bearing;
                                                               |                            update_navigation();  // Change nav_bearing for LOITER, RTL, and GUIDED
                                                               |
                                                               dTnav
                                                               |
                                                               |
       ---------------------------------------------------------------MloopC=2  update_alt(); update_command()
                                                               |
                                                               |
                                                               |
                                                               |
                                                               |
       ---------------------------------------------------------------MloopC=3  Telemetry
                                                               |
                                                               |
                                                               |
                                                               |                                                                       counter=0
                                                               |                                                                      |                        read_control_switch();                         -  --------------------------------------------------------------------MloopC=4  Slow_loop()   | counter=1     update_servo_switches();
                                                               |                                                                      |                        update_aux_servo_function()
                                                               |                                                                        counter=2
                                                               |
                                                               |
                                                               |
       ---------------------------------------------------------------MloopC=0  update_GPS()
                                                               |
                                                               |
                                                               |
                                                               |
                                                               |
       ---------------------------------------------------------------MloopC=1

The control process can be imagined as follow:

ATMEGA 2560 input is PPM with ISR(TIMER4) for reading PPM signal, inputs/outputs are SERIAL0 (console) for loading software and running simulaions, SERIAL1 (GPS data) for communicating with GPS, SERIAL3 (Xbee telemetry) for telemetry, serial port 2 (SPI mode) for communicating with ADC ADS7844 with ISR(TIMER2_OVF_vect) to read ADC data, outputs are pwm outputs from timers to drive servos.

 Outputs from RC receiver are connected to APM1 board inputs (max 8 channel, channel 8 for mode select). From there they are connected to ATMEGA 328 and multiplexer 74LS157. From ATMEGA 328 there's a MUX signal to 74LS157, in case of "HARDWARE MANUAL" mode this signal tells 74LS157 to connect RC receiver's outputs to the servos, otherwise 74LS157 selects signals from ATMEGA 2560. ATMEGA 328 combines 8 PWM channels into 1 PPM output. Due to the fact that PPM channel frequency = sevro update frequency = 50 Hz, so the PPM cycle is 20 ms and the maximum PWM pulsewidth is 1900us = 1.9 ms, so maximum 10 PWM channels can be combined into 1 PPM channel. This PPM channel from ATMEGA 328 is used to interrupt ATMEGA 2560. Each time there's a PPM pulse, an interrupt is generated and APM_RC.cpp ISR(TIMER4) interrupt service routine calculates time intervals between PPM pulses (the ppm pulsewidth) and saves in PPM_RAW[].

 Outputs from IMU sensors are connected to ADC ADS7844 input channels and ISR(TIMER2_OVF_vect) running at timer2 overflow frequency (~ 1000 Hz) reads the converted digital data and accumulates in _sum, then ADS7844::Ch6() calculates gyros' and accelerators' average data.

=============================================================================
read_radio() // radio.pde
InputCh(ch) reads PPM_RAW[ch]. read_radio() then reads all 8 channels and sets pwm arcordingly (radio_in=pwm).
    g.channel_roll.set_pwm(ch1_temp);
    g.channel_pitch.set_pwm(ch2_temp);
    g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3));
    g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4));
    g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
    g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
    g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
    g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
If there's a mode set on channel 8, it will be processed in slow_loop counter=1 case ( read_control_switch();   // control_modes.pde).
   
//update direction cosine matrix
dcm.update_DCM();         // libraries/AP_DCM/AP_DCM.cpp.

( Look at DCMDraft2.pdf for more information

http://code.google.com/p/gentlenav/downloads/detail?name=DCMDraft2....)
ADS7844::Ch6() calculates gyros' and accelerators' average digital data, then AP_IMU_Oilpan::update() calculates gyro and accelerator vectors used in matrix_update()

    matrix_update(delta_t);     // Integrate the DCM matrix
    normalize();            // Normalize the DCM matrix
    drift_correction();        // Perform drift correction
    euler_angles();            // Calculate pitch, roll, yaw for stabilization and navigation

// uses the yaw from the DCM to give more accurate turns
calc_bearing_error();                          // in Navigation.pde.

navigate() (navigation.pde) - called in medium_loop as it needs GPS data, calculates nav_bearing = target_bearing = get_bearing(&current_loc, &next_WP) (however for cases of LOITER, RTL, GUIDED nav_bearing is changed in update_loiter() as called in update_navigation)
Calculating bearing_error = nav_bearing - dcm.yaw_sensor; nav_bearing doesn't change between navigate() calls but dcm.yaw_sensor yes.

// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();                  // (Arduplane.pde)

Most cases (except FBW, STABILIZE, CIRCLE and MANUAL) call calc_nav_roll, calc_nav_pitch, calc_throttle (these are in attitude.pde) to calculate nav_roll, nav_pitch from bearing_error.
e.g. nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler).

In STABILIZE mode, update_current_flight_mode() sets nav_rol = nav_pitch = 0, so if you release stick that means read_radio gives radio_in = radio_trim, so in stabilize() in attitude.pde ch1_inf = ch2_inf = 1, g.channel_roll.pwm_to_angle() = g.channel_pitch.pwm_to_angle() = 0, and
g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler) = g.pidServoRoll.get_pid((0 - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler) (similiarly for g.channel_pitch.servo_out) so the plane will go back to nav_rol = nav_pitch = 0 level state.

In CIRCLE mode it sets nav_roll = g.roll_limit / 3; nav_pitch = 0; So when calling stabilize() the error in get_pid() = g.roll_limit / 3 - dcm.roll_sensor comming to 0 i.e. dcm.roll_sensor is comming to g.roll_limit / 3 = constant, the plane circles.

In FBW modes, nav_roll = g.channel_roll.norm_input() * g.roll_limit and depends only on stick angle. The nav_pitch depends on A or B modes.

In MANUAL mode (SOFTWARE MANUAL), servo_out values are calculated direcly from stick inputs and then sent to servos without calling stabilize().
                g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
                g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
                g.channel_rudder.servo_out = g.channel_rudder.pwm_to_angle();


// apply desired roll, pitch and yaw to the plane
// ----------------------------------------------
if (control_mode > MANUAL) stabilize();                     // in Attitude.pde.

In stabilize() g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler)
If there's an input from sticks in other modes (not FBW), it is mixed with servo_out as ch1_inf, ch2_inf < 1
        g.channel_roll.servo_out *= ch1_inf;
        g.channel_pitch.servo_out *= ch2_inf;

        g.channel_roll.servo_out +=    g.channel_roll.pwm_to_angle();
        g.channel_pitch.servo_out +=    g.channel_pitch.pwm_to_angle();


// write out the servo PWM values
// ------------------------------
set_servos();                                             // in Attitude.pde

Calculates channel_roll.radio_out, channel_pitch.radio_out, channel_throttle.radio_out and channel_rudder.radio_out from channel_...servo_out and mix_mode then send values to the PWM timers for output
----------------------------------------
    APM_RC.OutputCh(CH_1, g.channel_roll.radio_out); // send to Servos
    APM_RC.OutputCh(CH_2, g.channel_pitch.radio_out); // send to Servos
    APM_RC.OutputCh(CH_3, g.channel_throttle.radio_out); // send to Servos
    APM_RC.OutputCh(CH_4, g.channel_rudder.radio_out); // send to Servos
    // Route configurable aux. functions to their respective servos
    g.rc_5.output_ch(CH_5);
    g.rc_6.output_ch(CH_6);
    g.rc_7.output_ch(CH_7);
    g.rc_8.output_ch(CH_8);

Views: 2490

Reply to This

Replies to This Discussion

Great start! But may I suggest you use a format that's easier to understand, such as PDF, along with graphic flow-charts/diagrams?

Thank you Chris. This is only the first draft, I may change after some time

Here's the modified pdf version including hardware block diagram for easier reading and understanding

Attachments:

Minh: Impressive work! But it's for APM 1 and ArduPlane 2.26, both of which are outmoded. Any chance of updating it for APM 2 and ArduPlane 2.40?

As I can see, there's no big change in Arduplane algorithm, so there's not much to update in my description for Arduplane 2.40. May be the obvious change in 2.40 version from 2.26 I can see is that instead of dcm object definition there's ahrs object definition. So in my code description there should be updates:
- In fast_loop instead of dcm.update_DCM() it should be ahrs.update(), the same in description of updaing direction cosine matrix with the file libraries/AP_AHRS_DCM.cpp
- In calc_bearing_error instead of bearing_error = nav_bearing - dcm.yaw_sensor it should be bearing_error = nav_bearing - ahrs.yaw_sensor;
The situation is quite different with APM2 hardware. As this is a completely new hardware, it takes time for me to understand the new hardware. Meanwhile, if someone knows about the new hardware and have time, please share the knowledge.
Chris, may I ask you a question? Recently, I raised a question about coding that in get_pid() calls instead of dTnav it should be delta_ms_fast_loop and I got answer from Andrew Tridgell as below:

yep, you're right. Jon Challinger noticed this recently as well. I've been thinking about how to fix it.

It's easy to just fix the deltat of course, but that will change the meaning of the I and D terms in everyones existing configurations (ie. the HDG2RLL_I and HDG2RLL_D values), scaling them by a factor of 5x. That could change how well a plane flies by quite a lot. I'm always reluctant to break existing tuning, but in this case we may have to, with a warning in the release notes.

I'm also thinking about how to ensure this sort of bug doesn't happen again. I think the best way would be to make the PID object compute the deltat itself, rather than passing it in.

Cheers, Tridge
------------

There is still another issue I don't understand, I even rised an issue in DIY Drones but so far got no answer. In calc_nav_pitch() there is a command
nav_pitch = -g.pidNavPitchAirspeed.get_pid(airspeed_error, dTnav); (2 parameters call)
However, get_pid definition is
PID::get_pid(int32_t error, uint16_t dt, float scaler) (3 parameters definition)
How can it be like this? Can you explain me?

Here is an update to APM 2.5 and Arduplane 2.65

APM 2.5 block diagram is at http://diydrones.com/forum/topics/apm-2-5-block-diagram

and Arduplane 2.65 code description is attached

this is a pdf version of attachment

Attachments:

Thank you very much for spelling things out so nicely.

i can't download pdf that have the matlab simulink code for quadcopter can any one help me please

RSS

© 2014   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service