Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[rotorcraft] add nav_move_waypoint_lla function

Browse Source 
and use it in MOVE_WP in nps and datalink
This commit is contained in:
Felix Ruess
2013-09-05 19:17:15 +02:00
parent 4eced1b096
commit 1e50aacf20
4 changed files with 38 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/datalink.c
+13 -14
View File
@@ -49,7 +49,7 @@
#include "firmwares/rotorcraft/navigation.h"
#include "math/pprz_geodetic_int.h"
#include "subsystems/ins.h"
#include "state.h"
#define IdOfMsg(x) (x[1])
@@ -97,19 +97,18 @@ void dl_parse_msg(void) {
{
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) break;
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
/* WP_alt is in cm, lla.alt in mm */
lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - ins_impl.ltp_def.hmsl + ins_impl.ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_impl.ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
if (stateIsLocalCoordinateValid()) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
/* WP_alt from message is alt above MSL in cm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(dl_buffer)*10 - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
nav_move_waypoint_lla(wp_id, &lla);
}
}
break;
#endif /* USE_NAVIGATION */
sw/airborne/firmwares/rotorcraft/navigation.c
+17 -4
View File
@@ -44,6 +44,10 @@
#include "math/pprz_algebra_int.h"
#include "subsystems/datalink/downlink.h"
#include "messages.h"
#include "mcu_periph/uart.h"
const uint8_t nb_waypoint = NB_WAYPOINT;
struct EnuCoor_i waypoints[NB_WAYPOINT];
@@ -305,13 +309,22 @@ void nav_periodic_task() {
ground_alt = POS_BFP_OF_REAL((float)ins_impl.ltp_def.hmsl / 1000.);
}
#include "subsystems/datalink/downlink.h"
#include "messages.h"
#include "mcu_periph/uart.h"
void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos) {
if (stateIsLocalCoordinateValid()) {
struct EnuCoor_i enu;
enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
nav_move_waypoint(wp_id, &enu);
}
}
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos) {
if (wp_id < nb_waypoint) {
INT32_VECT3_COPY(waypoints[wp_id],(*new_pos));
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x), &(new_pos->y), &(new_pos->z));
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
&(new_pos->y), &(new_pos->z));
}
}
sw/airborne/firmwares/rotorcraft/navigation.h
+1
View File
@@ -79,6 +79,7 @@ void compute_dist2_to_home(void);
unit_t nav_reset_reference( void ) __attribute__ ((unused));
unit_t nav_reset_alt( void ) __attribute__ ((unused));
void nav_periodic_task(void);
void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i* new_lla_pos);
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
bool_t nav_detect_ground(void);
bool_t nav_is_in_flight(void);
sw/simulator/nps/nps_ivy_rotorcraft.c
+7 -11
View File
@@ -8,8 +8,8 @@
#include "nps_autopilot.h"
#include "nps_fdm.h"
#include "nps_sensors.h"
#include "subsystems/ins.h"
#include "firmwares/rotorcraft/navigation.h"
#include "state.h"
#ifdef RADIO_CONTROL_TYPE_DATALINK
#include "subsystems/radio_control.h"
@@ -31,7 +31,6 @@ void nps_ivy_init(char* ivy_bus) {
}
//TODO use datalink parsing from rotorcraft instead of doing it here explicitly
#include "generated/settings.h"
#include "dl_protocol.h"
@@ -43,16 +42,13 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
uint8_t wp_id = atoi(argv[1]);
struct LlaCoor_i lla;
struct EnuCoor_i enu;
lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
lla.alt = atoi(argv[5])*10 - ins_ltp_def.hmsl + ins_ltp_def.lla.alt;
enu_of_lla_point_i(&enu,&ins_ltp_def,&lla);
enu.x = POS_BFP_OF_REAL(enu.x)/100;
enu.y = POS_BFP_OF_REAL(enu.y)/100;
enu.z = POS_BFP_OF_REAL(enu.z)/100;
VECT3_ASSIGN(waypoints[wp_id], enu.x, enu.y, enu.z);
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &enu.x, &enu.y, &enu.z);
printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, enu.x, enu.y, enu.z);
/* WP_alt from message is alt above MSL in cm
* lla.alt is above ellipsoid in mm
*/
lla.alt = atoi(argv[5]) *10 - state.ned_origin_i.hmsl + state.ned_origin_i.lla.alt;
nav_move_waypoint_lla(wp_id, &lla);
printf("move wp id=%d x=%d y=%d z=%d\n", wp_id, waypoints[wp_id].x, waypoints[wp_id].y, waypoints[wp_id].z);
}
}