ardupilot/ArduCopter/UserCode.cpp

#include "Copter.h"

#ifdef USERHOOK_INIT
void Copter::userhook_init()
{
    // put your initialisation code here
    // this will be called once at start-up
}
#endif

#ifdef USERHOOK_FASTLOOP
void Copter::userhook_FastLoop()
{
    // put your 100Hz code here
}
#endif

#ifdef USERHOOK_50HZLOOP
void Copter::userhook_50Hz()
{
    // put your 50Hz code here
}
#endif

#ifdef USERHOOK_MEDIUMLOOP
void Copter::userhook_MediumLoop()
{
    // put your 10Hz code here
}
#endif

#ifdef USERHOOK_SLOWLOOP
void Copter::userhook_SlowLoop()
{
    // put your 3.3Hz code here
}
#endif

#ifdef USERHOOK_SUPERSLOWLOOP
void Copter::userhook_SuperSlowLoop()
{
    // put your 1Hz code here
    static uint8_t cnt;
    cnt +=1;

    if (cnt > 5)
    {
        camera_pos_rotation();
        cnt = 0;
    }
    
    
    
}
#endif

void Copter::camera_pos_rotation()
{
    Matrix3f Tbn;                               // dcm rotation matrix to rotate body frame to north
    Vector3f target_vec_unit_body;
    Vector3f _cam_offset = {0,240.0,130.0};
    Tbn = ahrs.get_rotation_body_to_ned();
    Vector3f cam_pos_ned = Tbn * (_cam_offset);
    Vector3f euler,euler_deg;
    if(ahrs.get_secondary_attitude(euler)){
        euler_deg.x = degrees(euler.x);
        euler_deg.y = degrees(euler.y);
        euler_deg.z = wrap_360(degrees(euler.z));
    }
    // Vector3f cam_pos_delt = Tbn.mul_transpose(_cam_offset);
    gcs().send_text(MAV_SEVERITY_INFO,"org:%.2f,%.2f,%.2f,euler:%.2f,%.2f,%.2f",_cam_offset.x,_cam_offset.y,_cam_offset.z,euler_deg.x,euler_deg.y,euler_deg.z);
    gcs().send_text(MAV_SEVERITY_INFO,"rot:%.2f,%.2f,%.2f",cam_pos_ned.x,cam_pos_ned.y,cam_pos_ned.z);
    // gcs().send_text(MAV_SEVERITY_INFO,"del:%.2f,%.2f,%.2f",cam_pos_delt.x,cam_pos_delt.y,cam_pos_delt.z);


    Vector3f vec_ne;
        const Location &loc = gps.location();
    get_vector_xyz_from_NEU(vec_ne,loc);

    gcs().send_text(MAV_SEVERITY_INFO,"pos:%d,%d,%d",loc.lat,loc.lng,loc.alt);
    gcs().send_text(MAV_SEVERITY_INFO,"xyz:%.2f,%.2f,%.2f",vec_ne.x,vec_ne.y,vec_ne.z);
    Location neu;
    get_NEU_from_xyz(neu,vec_ne);
    gcs().send_text(MAV_SEVERITY_INFO,"neu:%d,%d,%d",neu.lat,neu.lng,neu.alt);

}

bool Copter::get_vector_xyz_from_NEU(Vector3f &vec_ne,Location lla) const
{
    Location ekf_origin;
    if (!AP::ahrs().get_origin(ekf_origin)) {
        return false;
    }
    vec_ne.x = (lla.lat-0) * LATLON_TO_CM;
    vec_ne.y = (lla.lng-0) * LATLON_TO_CM * ekf_origin.longitude_scale();
    vec_ne.z = (lla.alt-0);

    return true;
}

bool Copter::get_NEU_from_xyz(Location &neu,Vector3f vec_ne)
{
    Location ekf_origin;
    if (!AP::ahrs().get_origin(ekf_origin)) {
        return false;
    }
    neu.lat = vec_ne.x/ LATLON_TO_CM;
    neu.lng = vec_ne.y/(LATLON_TO_CM * ekf_origin.longitude_scale());
    neu.alt = vec_ne.z;

    return true;
}

#ifdef USERHOOK_AUXSWITCH
void Copter::userhook_auxSwitch1(uint8_t ch_flag)
{
    // put your aux switch #1 handler here (CHx_OPT = 47)
    switch (ch_flag)
    {
    case 0:
        /* code */
        break;
    case 1:
        /* code */
        break;
    case 2:
        camera_pos_rotation();
        break;
    
    default:
        break;
    }
}

void Copter::userhook_auxSwitch2(uint8_t ch_flag)
{
    // put your aux switch #2 handler here (CHx_OPT = 48)
}

void Copter::userhook_auxSwitch3(uint8_t ch_flag)
{
    // put your aux switch #3 handler here (CHx_OPT = 49)
}
#endif