Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: add WPNAV log message

Browse Source
This commit is contained in:
Randy Mackay 2013-04-15 21:54:29 +09:00
parent b48864e1ad
commit 8fe3e689f4
4 changed files with 107 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
ArduCopter/Log.pde
View File

@ -771,7 +771,6 @@ struct log_INAV {
float accel_corr_x; float accel_corr_x;
float accel_corr_y; float accel_corr_y;
float accel_corr_z; float accel_corr_z;
float accel_corr_ef_z;
int32_t gps_lat_from_home; int32_t gps_lat_from_home;
int32_t gps_lon_from_home; int32_t gps_lon_from_home;
float inav_lat_from_home; float inav_lat_from_home;
@ -793,13 +792,12 @@ static void Log_Write_INAV()
accel_corr_x : accel_corr.x, // 4 accel correction x-axis accel_corr_x : accel_corr.x, // 4 accel correction x-axis
accel_corr_y : accel_corr.y, // 5 accel correction y-axis accel_corr_y : accel_corr.y, // 5 accel correction y-axis
accel_corr_z : accel_corr.z, // 6 accel correction z-axis accel_corr_z : accel_corr.z, // 6 accel correction z-axis
accel_corr_ef_z : inertial_nav.accel_correction_ef.z, // 7 accel correction earth frame gps_lat_from_home : g_gps->latitude-home.lat, // 7 lat from home
gps_lat_from_home : g_gps->latitude-home.lat, // 8 lat from home gps_lon_from_home : g_gps->longitude-home.lng, // 8 lon from home
gps_lon_from_home : g_gps->longitude-home.lng, // 9 lon from home inav_lat_from_home : inertial_nav.get_latitude_diff(), // 9 accel based lat from home
inav_lat_from_home : inertial_nav.get_latitude_diff(), // 10 accel based lat from home inav_lon_from_home : inertial_nav.get_longitude_diff(), // 10 accel based lon from home
inav_lon_from_home : inertial_nav.get_longitude_diff(), // 11 accel based lon from home inav_lat_speed : inertial_nav.get_latitude_velocity(), // 11 accel based lat velocity
inav_lat_speed : inertial_nav.get_latitude_velocity(), // 12 accel based lat velocity inav_lon_speed : inertial_nav.get_longitude_velocity() // 12 accel based lon velocity
inav_lon_speed : inertial_nav.get_longitude_velocity() // 13 accel based lon velocity
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -810,21 +808,20 @@ static void Log_Read_INAV()
struct log_INAV pkt; struct log_INAV pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt)); DataFlash.ReadPacket(&pkt, sizeof(pkt));
// 1 2 3 4 5 6 7 8 9 10 11 12 13 // 1 2 3 4 5 6 7 8 9 10 11 12
cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %6.4f, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"), cliSerial->printf_P(PSTR("INAV, %d, %d, %d, %6.4f, %6.4f, %6.4f, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"),
(int)pkt.baro_alt, // 1 barometer altitude (int)pkt.baro_alt, // 1 barometer altitude
(int)pkt.inav_alt, // 2 accel + baro filtered altitude (int)pkt.inav_alt, // 2 accel + baro filtered altitude
(int)pkt.inav_climb_rate, // 3 accel + baro based climb rate (int)pkt.inav_climb_rate, // 3 accel + baro based climb rate
(float)pkt.accel_corr_x, // 4 accel correction x-axis (float)pkt.accel_corr_x, // 4 accel correction x-axis
(float)pkt.accel_corr_y, // 5 accel correction y-axis (float)pkt.accel_corr_y, // 5 accel correction y-axis
(float)pkt.accel_corr_z, // 6 accel correction z-axis (float)pkt.accel_corr_z, // 6 accel correction z-axis
(float)pkt.accel_corr_ef_z, // 7 accel correction earth frame (long)pkt.gps_lat_from_home, // 7 lat from home
(long)pkt.gps_lat_from_home, // 8 lat from home (long)pkt.gps_lon_from_home, // 8 lon from home
(long)pkt.gps_lon_from_home, // 9 lon from home (float)pkt.inav_lat_from_home, // 9 accel based lat from home
(float)pkt.inav_lat_from_home, // 10 accel based lat from home (float)pkt.inav_lon_from_home, // 10 accel based lon from home
(float)pkt.inav_lon_from_home, // 11 accel based lon from home (float)pkt.inav_lat_speed, // 11 accel based lat velocity
(float)pkt.inav_lat_speed, // 12 accel based lat velocity (float)pkt.inav_lon_speed); // 12 accel based lon velocity
(float)pkt.inav_lon_speed); // 13 accel based lon velocity
} }
struct log_Mode { struct log_Mode {
@ -1227,6 +1224,61 @@ static void Log_Read_Error()
cliSerial->printf_P(PSTR(", %u\n"),(unsigned)pkt.error_code); cliSerial->printf_P(PSTR(", %u\n"),(unsigned)pkt.error_code);
} }
struct log_WPNAV {
LOG_PACKET_HEADER;
float pos_error_x;
float pos_error_y;
float desired_velocity_x;
float desired_velocity_y;
float velocity_x;
float velocity_y;
float desired_accel_x;
float desired_accel_y;
int32_t desired_roll;
int32_t desired_pitch;
};
// Write an INAV packet. Total length : 52 Bytes
static void Log_Write_WPNAV()
{
Vector3f velocity = inertial_nav.get_velocity();
struct log_WPNAV pkt = {
LOG_PACKET_HEADER_INIT(LOG_WPNAV_MSG),
pos_error_x : wp_nav.dist_error.x,
pos_error_y : wp_nav.dist_error.y,
desired_velocity_x : wp_nav.desired_vel.x,
desired_velocity_y : wp_nav.desired_vel.y,
velocity_x : velocity.x,
velocity_y : velocity.y,
desired_accel_x : wp_nav.desired_accel.x,
desired_accel_y : wp_nav.desired_accel.y,
desired_roll : wp_nav.get_desired_roll(),
desired_pitch : wp_nav.get_desired_pitch()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// Read an WPNAV packet
static void Log_Read_WPNav()
{
struct log_WPNAV pkt;
DataFlash.ReadPacket(&pkt, sizeof(pkt));
// 1 2 3 4 5 6 7 8 9 10
cliSerial->printf_P(PSTR("WPNAV, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %ld, %ld\n"),
(float)pkt.pos_error_x, // 1 position error in cm in lat direction
(float)pkt.pos_error_y, // 2 position error in cm in lon direction
(float)pkt.desired_velocity_x, // 3 desired velocity in cm/s in lat direction
(float)pkt.desired_velocity_y, // 4 desired velocity in cm/s in lon direction
(float)pkt.velocity_x, // 5 velocity in cm/s in lat direction
(float)pkt.velocity_y, // 6 velocity in cm/s in lon direction
(float)pkt.desired_accel_x, // 7 desired acceleration in cm/s/s in lat direction
(float)pkt.desired_accel_y, // 8 desired acceleration in cm/s/s in lon direction
(long)pkt.desired_roll, // 9 accel based lat from home
(long)pkt.desired_pitch); // 12 accel based lon velocity
}
// Read the DataFlash log memory // Read the DataFlash log memory
static void Log_Read(uint8_t log_num, int16_t start_page, int16_t end_page) static void Log_Read(uint8_t log_num, int16_t start_page, int16_t end_page)
{ {
@ -1346,6 +1398,10 @@ static void log_callback(uint8_t msgid)
case LOG_DATA_FLOAT_MSG: case LOG_DATA_FLOAT_MSG:
Log_Read_Float(); Log_Read_Float();
break; break;
case LOG_WPNAV_MSG:
Log_Read_WPNav();
break;
} }
} }

4
ArduCopter/navigation.pde
View File

@ -157,11 +157,15 @@ static void update_nav_mode()
case NAV_LOITER: case NAV_LOITER:
// call loiter controller // call loiter controller
wp_nav.update_loiter(); wp_nav.update_loiter();
// log to dataflash
Log_Write_WPNAV();
break; break;
case NAV_WP: case NAV_WP:
// call waypoint controller // call waypoint controller
wp_nav.update_wpnav(); wp_nav.update_wpnav();
// log to dataflash
Log_Write_WPNAV();
break; break;
} }

34
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -178,7 +178,7 @@ void AC_WPNav::update_loiter()
translate_loiter_target_movements(dt); translate_loiter_target_movements(dt);
// run loiter position controller // run loiter position controller
get_loiter_pos_lat_lon(_target.x, _target.y, dt); get_loiter_position_to_velocity(dt);
} }
/// ///
@ -303,19 +303,17 @@ void AC_WPNav::update_wpnav()
} }
// run loiter position controller // run loiter position controller
get_loiter_pos_lat_lon(_target.x, _target.y, dt); get_loiter_position_to_velocity(dt);
} }
/// ///
/// shared methods /// shared methods
/// ///
// get_loiter_pos_lat_lon - loiter position controller /// get_loiter_position_to_velocity - loiter position controller
// converts desired position provided as distance from home in lat/lon directions to desired velocity /// converts desired position held in _target vector to desired velocity
void AC_WPNav::get_loiter_pos_lat_lon(float target_lat_from_home, float target_lon_from_home, float dt) void AC_WPNav::get_loiter_position_to_velocity(float dt)
{ {
Vector2f dist_error;
Vector2f desired_vel;
Vector3f curr = _inav->get_position(); Vector3f curr = _inav->get_position();
float dist_error_total; float dist_error_total;
@ -325,8 +323,8 @@ void AC_WPNav::get_loiter_pos_lat_lon(float target_lat_from_home, float target_l
float linear_distance; // the distace we swap between linear and sqrt. float linear_distance; // the distace we swap between linear and sqrt.
// calculate distance error // calculate distance error
dist_error.x = target_lat_from_home - curr.x; dist_error.x = _target.x - curr.x;
dist_error.y = target_lon_from_home - curr.y; dist_error.y = _target.y - curr.y;
linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP()); linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP());
_distance_to_target = linear_distance; // for reporting purposes _distance_to_target = linear_distance; // for reporting purposes
@ -347,16 +345,16 @@ void AC_WPNav::get_loiter_pos_lat_lon(float target_lat_from_home, float target_l
desired_vel.y = MAX_LOITER_POS_VELOCITY * desired_vel.y/vel_total; desired_vel.y = MAX_LOITER_POS_VELOCITY * desired_vel.y/vel_total;
} }
get_loiter_vel_lat_lon(desired_vel.x, desired_vel.y, dt); // call velocity to acceleration controller
get_loiter_velocity_to_acceleration(desired_vel.x, desired_vel.y, dt);
} }
// get_loiter_vel_lat_lon - loiter velocity controller /// get_loiter_velocity_to_acceleration - loiter velocity controller
// converts desired velocities in lat/lon frame to accelerations in lat/lon frame /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void AC_WPNav::get_loiter_vel_lat_lon(float vel_lat, float vel_lon, float dt) void AC_WPNav::get_loiter_velocity_to_acceleration(float vel_lat, float vel_lon, float dt)
{ {
Vector3f vel_curr = _inav->get_velocity(); // current velocity in cm/s Vector3f vel_curr = _inav->get_velocity(); // current velocity in cm/s
Vector3f vel_error; // The velocity error in cm/s. Vector3f vel_error; // The velocity error in cm/s.
Vector2f desired_accel; // the resulting desired acceleration
float accel_total; // total acceleration in cm/s/s float accel_total; // total acceleration in cm/s/s
// reset last velocity if this controller has just been engaged or dt is zero // reset last velocity if this controller has just been engaged or dt is zero
@ -389,12 +387,12 @@ void AC_WPNav::get_loiter_vel_lat_lon(float vel_lat, float vel_lon, float dt)
} }
// call accel based controller with desired acceleration // call accel based controller with desired acceleration
get_loiter_accel_lat_lon(desired_accel.x, desired_accel.y); get_loiter_acceleration_to_lean_angles(desired_accel.x, desired_accel.y);
} }
// get_loiter_accel_lat_lon - loiter acceration controller /// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
// converts desired accelerations provided in lat/lon frame to roll/pitch angles /// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_WPNav::get_loiter_accel_lat_lon(float accel_lat, float accel_lon) void AC_WPNav::get_loiter_acceleration_to_lean_angles(float accel_lat, float accel_lon)
{ {
float z_accel_meas = -GRAVITY_MSS * 100; // gravity in cm/s/s float z_accel_meas = -GRAVITY_MSS * 100; // gravity in cm/s/s
float accel_forward; float accel_forward;

24
libraries/AC_WPNav/AC_WPNav.h
View File

@ -155,17 +155,17 @@ protected:
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void translate_loiter_target_movements(float nav_dt); void translate_loiter_target_movements(float nav_dt);
/// get_loiter_pos_lat_lon - loiter position controller /// get_loiter_position_to_velocity - loiter position controller
/// converts desired position provided as distance from home in lat/lon directions to desired velocity /// converts desired position held in _target vector to desired velocity
void get_loiter_pos_lat_lon(float target_lat_from_home, float target_lon_from_home, float dt); void get_loiter_position_to_velocity(float dt);
/// get_loiter_vel_lat_lon - loiter velocity controller /// get_loiter_velocity_to_acceleration - loiter velocity controller
/// converts desired velocities in lat/lon frame to accelerations in lat/lon frame /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void get_loiter_vel_lat_lon(float vel_lat, float vel_lon, float dt); void get_loiter_velocity_to_acceleration(float vel_lat_cms, float vel_lon_cms, float dt);
/// get_loiter_accel_lat_lon - loiter acceration controller /// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles /// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void get_loiter_accel_lat_lon(float accel_lat, float accel_lon); void get_loiter_acceleration_to_lean_angles(float accel_lat_cmss, float accel_lon_cmss);
/// get_bearing_cd - return bearing in centi-degrees between two positions /// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f origin, const Vector3f destination); float get_bearing_cd(const Vector3f origin, const Vector3f destination);
@ -215,9 +215,13 @@ protected:
int16_t _pilot_vel_forward_cms; int16_t _pilot_vel_forward_cms;
int16_t _pilot_vel_right_cms; int16_t _pilot_vel_right_cms;
public:
// for logging purposes
Vector2f dist_error; // distance error calculated by loiter controller
Vector2f desired_vel; // loiter controller desired velocity
Vector2f desired_accel; // the resulting desired acceleration
// To-Do: add split of fast (100hz for accel->angle) and slow (10hz for navigation) // To-Do: add split of fast (100hz for accel->angle) and slow (10hz for navigation)
//float _desired_accel_fwd; // updated from loiter controller at 10hz, consumed by accel->angle controller at 50hz
//float _desired_accel_rgt;
/// update - run the loiter and wpnav controllers - should be called at 100hz /// update - run the loiter and wpnav controllers - should be called at 100hz
//void update_100hz(void); //void update_100hz(void);
/// update - run the loiter and wpnav controllers - should be called at 10hz /// update - run the loiter and wpnav controllers - should be called at 10hz