From 8fe3e689f4dcb26a6c2ca7c2653e345facf62bf3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Apr 2013 21:54:29 +0900 Subject: [PATCH] Copter: add WPNAV log message --- ArduCopter/Log.pde | 90 ++++++++++++++++++++++++++------- ArduCopter/navigation.pde | 4 ++ libraries/AC_WPNav/AC_WPNav.cpp | 34 ++++++------- libraries/AC_WPNav/AC_WPNav.h | 24 +++++---- 4 files changed, 107 insertions(+), 45 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 0765354634..2f5cac82bc 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -771,7 +771,6 @@ struct log_INAV { float accel_corr_x; float accel_corr_y; float accel_corr_z; - float accel_corr_ef_z; int32_t gps_lat_from_home; int32_t gps_lon_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_y : accel_corr.y, // 5 accel correction y-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, // 8 lat from home - gps_lon_from_home : g_gps->longitude-home.lng, // 9 lon 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(), // 11 accel based lon from home - inav_lat_speed : inertial_nav.get_latitude_velocity(), // 12 accel based lat velocity - inav_lon_speed : inertial_nav.get_longitude_velocity() // 13 accel based lon velocity + gps_lat_from_home : g_gps->latitude-home.lat, // 7 lat from home + gps_lon_from_home : g_gps->longitude-home.lng, // 8 lon from home + inav_lat_from_home : inertial_nav.get_latitude_diff(), // 9 accel based lat from home + inav_lon_from_home : inertial_nav.get_longitude_diff(), // 10 accel based lon from home + inav_lat_speed : inertial_nav.get_latitude_velocity(), // 11 accel based lat velocity + inav_lon_speed : inertial_nav.get_longitude_velocity() // 12 accel based lon velocity }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } @@ -810,21 +808,20 @@ static void Log_Read_INAV() struct log_INAV pkt; DataFlash.ReadPacket(&pkt, sizeof(pkt)); - // 1 2 3 4 5 6 7 8 9 10 11 12 13 - 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"), + // 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, %ld, %ld, %6.4f, %6.4f, %6.4f, %6.4f\n"), (int)pkt.baro_alt, // 1 barometer altitude (int)pkt.inav_alt, // 2 accel + baro filtered altitude (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_y, // 5 accel correction y-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, // 8 lat from home - (long)pkt.gps_lon_from_home, // 9 lon from home - (float)pkt.inav_lat_from_home, // 10 accel based lat from home - (float)pkt.inav_lon_from_home, // 11 accel based lon from home - (float)pkt.inav_lat_speed, // 12 accel based lat velocity - (float)pkt.inav_lon_speed); // 13 accel based lon velocity + (long)pkt.gps_lat_from_home, // 7 lat from home + (long)pkt.gps_lon_from_home, // 8 lon from home + (float)pkt.inav_lat_from_home, // 9 accel based lat from home + (float)pkt.inav_lon_from_home, // 10 accel based lon from home + (float)pkt.inav_lat_speed, // 11 accel based lat velocity + (float)pkt.inav_lon_speed); // 12 accel based lon velocity } struct log_Mode { @@ -1227,6 +1224,61 @@ static void Log_Read_Error() 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 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: Log_Read_Float(); break; + + case LOG_WPNAV_MSG: + Log_Read_WPNav(); + break; } } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 81813aa576..f9069ba287 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -157,11 +157,15 @@ static void update_nav_mode() case NAV_LOITER: // call loiter controller wp_nav.update_loiter(); + // log to dataflash + Log_Write_WPNAV(); break; case NAV_WP: // call waypoint controller wp_nav.update_wpnav(); + // log to dataflash + Log_Write_WPNAV(); break; } diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 2306477705..e03f8e47f7 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -178,7 +178,7 @@ void AC_WPNav::update_loiter() translate_loiter_target_movements(dt); // 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 - get_loiter_pos_lat_lon(_target.x, _target.y, dt); + get_loiter_position_to_velocity(dt); } /// /// shared methods /// -// get_loiter_pos_lat_lon - loiter position controller -// converts desired position provided as distance from home in lat/lon directions to desired velocity -void AC_WPNav::get_loiter_pos_lat_lon(float target_lat_from_home, float target_lon_from_home, float dt) +/// get_loiter_position_to_velocity - loiter position controller +/// converts desired position held in _target vector to desired velocity +void AC_WPNav::get_loiter_position_to_velocity(float dt) { - Vector2f dist_error; - Vector2f desired_vel; Vector3f curr = _inav->get_position(); 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. // calculate distance error - dist_error.x = target_lat_from_home - curr.x; - dist_error.y = target_lon_from_home - curr.y; + dist_error.x = _target.x - curr.x; + dist_error.y = _target.y - curr.y; linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP()); _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; } - 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 -// converts desired velocities in lat/lon frame to accelerations in lat/lon frame -void AC_WPNav::get_loiter_vel_lat_lon(float vel_lat, float vel_lon, float dt) +/// get_loiter_velocity_to_acceleration - loiter velocity controller +/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame +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_error; // The velocity error in cm/s. - Vector2f desired_accel; // the resulting desired acceleration float accel_total; // total acceleration in cm/s/s // 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 - 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 -// 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) +/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller +/// converts desired accelerations provided in lat/lon frame to roll/pitch angles +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 accel_forward; diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 4db860de91..75803b3860 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -155,17 +155,17 @@ protected: /// translate_loiter_target_movements - consumes adjustments created by move_loiter_target void translate_loiter_target_movements(float nav_dt); - /// get_loiter_pos_lat_lon - loiter position controller - /// converts desired position provided as distance from home in lat/lon directions to desired velocity - void get_loiter_pos_lat_lon(float target_lat_from_home, float target_lon_from_home, float dt); + /// get_loiter_position_to_velocity - loiter position controller + /// converts desired position held in _target vector to desired velocity + void get_loiter_position_to_velocity(float dt); - /// get_loiter_vel_lat_lon - loiter velocity controller - /// converts desired velocities in lat/lon frame to accelerations in lat/lon frame - void get_loiter_vel_lat_lon(float vel_lat, float vel_lon, float dt); + /// get_loiter_velocity_to_acceleration - loiter velocity controller + /// converts desired velocities in lat/lon directions to accelerations in lat/lon frame + 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 - 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 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_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) - //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 //void update_100hz(void); /// update - run the loiter and wpnav controllers - should be called at 10hz