Replies to This Discussion

Permalink Reply by Heidi Goeringer on June 10, 2012 at 3:31pm

Pretty awesome! I'm interested to know how to read the heading as well. 

Permalink Reply by Michael Pursifull on June 10, 2012 at 7:32pm

If I remember correctly, there is heading and there is bearing. One is the direction the airframe is pointed, and one is the direction it needs to point in order to get to a place (which, depending on wind and other factors, may or may not be the heading) Or something along those lines. 

If you just want to raw compass heading/orientation, perhaps this helps?

[From ArduCopter/setup.pde]

print_hit_enter();
init_compass();

int _min[3] = {0,0,0};
int _max[3] = {0,0,0};

compass.read();
compass.calculate(0,0); // roll = 0, pitch = 0

while(1){
             delay(50);

             compass.read();
             compass.calculate(0,0); // roll = 0, pitch = 0

             if(compass.mag_x < _min[0]) _min[0] = compass.mag_x;
             if(compass.mag_y < _min[1]) _min[1] = compass.mag_y;
             if(compass.mag_z < _min[2]) _min[2] = compass.mag_z;

             // capture max
             if(compass.mag_x > _max[0]) _max[0] = compass.mag_x;
             if(compass.mag_y > _max[1]) _max[1] = compass.mag_y;
             if(compass.mag_z > _max[2]) _max[2] = compass.mag_z;

             // calculate offsets
            _offsets.x = (float)(_max[0] + _min[0]) / -2;
            _offsets.y = (float)(_max[1] + _min[1]) / -2;
            _offsets.z = (float)(_max[2] + _min[2]) / -2;

            // display all to user
            Serial.printf_P(PSTR("Heading: %u, \t (%d, %d, %d), (%4.4f, %4.4f, %4.4f)\n"),(uint16_t)(wrap_360(ToDeg(compass.heading) * 100)) /100,

                    compass.mag_x,
                    compass.mag_y,
                    compass.mag_z,

                    _offsets.x,
                    _offsets.y,
                    _offsets.z);

                    if(Serial.available() > 1){
                         compass.set_offsets(_offsets);
                        //compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
                         report_compass();
                         return 0;
                      }
 }

However, I don't think that is what you want. 

You might use these values, already accessible to you:

static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
{
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
nav_pitch / 1.0e2,
nav_bearing / 1.0e2, <-----
target_bearing / 1.0e2,
wp_distance / 1.0e2,
altitude_error / 1.0e2,
0,
crosstrack_error); // was 0
}

But I'm not sure that is what you really want either. Don't you want the yaw value relative to the original/intended orientation of your IMU?

So I guess there are a few ways to do it, depending on what you want. All the values are already calculated for you. But you need to think about whether you to get the direction the gimbal is facing, or if you want to know which way and how far it is rotating away from the direction it was oriented (or the direction you want it to be pointed) or if want to calculate which way it should be facing given two AHRS solutions (the gimbal location and attitude and a target location) in which case, you might want something like:

navigation.pde:static int32_t get_bearing(struct Location *loc1, struct Location *loc2)

Permalink Reply by Nicholas J Anderson on June 10, 2012 at 8:58pm

I'm trying to grab the actual heading of the gimbal so that I can hold a desired heading, which can be the heading to face GPS coordinate.  I'm getting accurate, pitch & roll, lat, long, and gps altitude values, but I'm not getting an accurate, geo-referenced yaw value, so where the gimbal is pointed.

target_yaw = get_bearing(current_location, target_location);
target_yaw_rate = yaw_PID.get_pid(wrap_180(target_yaw - yaw), 10.0, 1.0);  <---------- I need "yaw"
target_yaw_rate = constrain(target_yaw_rate, MIN_YAW_RATE, MAX_YAW_RATE);

servo_4_pwm = SERVO_4_CENTER + yaw_rate_PID.get_pid(target_yaw_rate - yaw_rate, 10.0, 1.0); 

I'm trying to calculate "yaw" using this:

static void read_compass(void){
    if(compass.read()){
        compass.calculate(ahrs.get_dcm_matrix());
        yaw = ToDeg(compass.heading);
    }
}

Permalink Reply by Nicholas J Anderson on June 10, 2012 at 9:08pm

I think I keep bouncing around terms incorrectly.  I want to read where the gimbal is pointing, not traveling.  So that I can point it due north, east, etc.

Any advice as to why this incorrect values are being stored in "yaw".  Maybe am I reading a traveling direction?

Maybe it is

yaw = ToDeg(ahrs.yaw);

that I want to be using?

Permalink Reply by Michael Pursifull on June 10, 2012 at 7:03pm

Have you considered looking at the code for the ArduPlane instead? The reason I ask is if all you want is a static heading direction, that is easy enough. However, as your gimbal moves through space, the heading to your point of interest is likely to change. See http://code.google.com/p/ardupilot-mega/wiki/Tracking for an understanding of why I ask. 

If you want to do this, also take a look at the reply to http://diydrones.com/forum/topics/apm1-for-gimbal-control-on-blimp-...

Now, to do this well, you will need to change it to a continuous servo, which means you would need to change the code, rather than setting the servo to a positional value, you'd want to do some quick math, decide which direction the gimbal needs to rotate, and decide how quickly you want it to move until you bring the gimbal to "zero" and then set the yaw servo to the "stop" value. I have been thinking about something similar, and plan to use PIDs to drive the continuous rotation gimbals so I can achieve better tracking. Just something to consider before you get in too deep. Since a lot of this code is in the libraries, it might be accessible to either platform, but I have not looked at if/how it is implemented in ArduCopter. You'll also find examples in the code written for antenna tracking, and that might be an even easier adaptation, since it is intended to point at a specific GPS location. FInd an example created for a mobile antenna and you've got everything you need without writing much code, just some tuning. Maybe. 

Permalink Reply by Nicholas J Anderson on June 10, 2012 at 7:43pm

Yes the gimbal is to be attached to a multirotor.  Down the road: have a couple APM1s that I was going to attach to the subject (moving or stationary), and feed lat/long and alt to the Gimbal APM2.  

I've implemented PIDs that work to maintain a desired yaw rate to servo output, and one to maintain a heading error to a desired yaw rate, the only issue is that my APM doesn't know where it's pointing, because I don't think I've implemented the compass read correctly.

Here's the code that does that if you want to use it for your tracking project:

This is called every 10 milliseconds as similar methods are called in Arducopter 2.5.5, the attitude AHRS is updated every 10 ms as well and the compass is updated every 100 ms.  But the compass is giving false readings if I rotate the heli it changes between 360 and 270 degrees, not 360 to 0 as I would expect. 

static void update_yaw(){
    if(yaw_mode == PASS_THROUGH){

        //this just passes through the stick command
        servo_4_pwm = ch_4_pwm;
    } else if(yaw_mode == RATE){

        //this maps the stick input to a yaw rate, this works very well
        target_yaw_rate = map(ch_4_pwm, MIN_PWM, MAX_PWM, MIN_YAW_RATE, MAX_YAW_RATE);
        target_yaw_rate = constrain(target_yaw_rate, MIN_YAW_RATE, MAX_YAW_RATE);
        servo_4_pwm = SERVO_4_CENTER + yaw_rate_PID.get_pid(target_yaw_rate - yaw_rate, 10.0, 1.0);
    } else if(yaw_mode == HOLD){

        //this mode holds a heading of 0.0
        target_yaw = 0.0;
        target_yaw_rate = yaw_PID.get_pid(wrap_180(target_yaw - yaw), 10.0, 1.0);
        target_yaw_rate = constrain(target_yaw_rate, MIN_YAW_RATE, MAX_YAW_RATE);

servo_4_pwm = SERVO_4_CENTER + yaw_rate_PID.get_pid(target_yaw_rate - yaw_rate, 10.0, 1.0); 

       //servo_4_pwm = SERVO_4_CENTER + alt_yaw_rate_PID.get_pid(target_yaw_rate - yaw_rate, 10.0, 1.0);
     }
}

Permalink Reply by Nicholas J Anderson on June 10, 2012 at 7:56pm

Oh and yes it is a continuous servo for the the yaw.  I know there must be something simple I'm getting wrong for the incorrect heading readings.  I think it might have to do with the angle of the APM2 or me not initializing the compass correctly or my lack of understanding of DCM & Quaternion.

Permalink Reply by Redemptioner on April 26, 2013 at 5:31pm

hows the project coming along?