Revision 74c0e6394dc5a2a6fb703406de719b47a626cb3b authored by ziqingma on 22 October 2023, 21:33:01 UTC, committed by ziqingma on 23 October 2023, 08:42:43 UTC
CG shift due to tilt issue, add takeoff test flightplan in cyberzoo Update guidance_indi.c
1 parent 6b317f2
nps_ivy.c
#include "nps_ivy.h"
#include <stdlib.h>
#include <stdio.h>
#include <sys/types.h>
#include <unistd.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyloop.h>
#include <pthread.h>
#include "generated/airframe.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_double.h"
#include "nps_main.h"
#include "nps_autopilot.h"
#include "nps_fdm.h"
#include "nps_sensors.h"
#include "nps_atmosphere.h"
#include "generated/settings.h"
#include "pprzlink/dl_protocol.h"
#include "modules/datalink/downlink.h"
#if USE_GPS
#include "modules/gps/gps.h"
#endif
#include NPS_SENSORS_PARAMS
pthread_t th_ivy_main; // runs main Ivy loop
static MsgRcvPtr ivyPtr = NULL;
static int seq = 1;
static int ap_launch_index;
/* Gaia Ivy functions */
static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
/* Datalink Ivy functions */
static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[]);
void* ivy_main_loop(void* data __attribute__((unused)));
int find_launch_index(void);
void* ivy_main_loop(void* data __attribute__((unused)))
{
IvyMainLoop();
return NULL;
}
void nps_ivy_init(char *ivy_bus)
{
const char *agent_name = AIRFRAME_NAME"_NPS";
const char *ready_msg = AIRFRAME_NAME"_NPS Ready";
IvyInit(agent_name, ready_msg, NULL, NULL, NULL, NULL);
// bind on a general WORLD_ENV (not a reply to request)
IvyBindMsg(on_WORLD_ENV, NULL, "^(\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)");
// to be able to change datalink_enabled setting back on
IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
#ifdef __APPLE__
const char *default_ivy_bus = "224.255.255.255";
#else
const char *default_ivy_bus = "127.255.255.255";
#endif
if (ivy_bus == NULL) {
IvyStart(default_ivy_bus);
} else {
IvyStart(ivy_bus);
}
nps_ivy_send_world_env = false;
ap_launch_index = find_launch_index();
// Launch separate thread with IvyMainLoop()
pthread_create(&th_ivy_main, NULL, ivy_main_loop, NULL);
}
/*
* Parse WORLD_ENV message from gaia.
*
*/
static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
// wind speed in m/s
struct FloatVect3 wind;
wind.x = atof(argv[1]); //east
wind.y = atof(argv[2]); //north
wind.z = atof(argv[3]); //up
/* set wind speed in NED */
nps_atmosphere_set_wind_ned(wind.y, wind.x, -wind.z);
/* not used so far */
//float ir_contrast = atof(argv[4]);
/* set new time factor */
nps_set_time_factor(atof(argv[5]));
#if USE_GPS
// directly set gps fix in modules/gps/gps_sim_nps.h
gps_has_fix = atoi(argv[6]); // gps_availability
#endif
}
/*
* Send a WORLD_ENV_REQ message
*/
void nps_ivy_send_WORLD_ENV_REQ(void)
{
// First unbind from previous request if needed
if (ivyPtr != NULL) {
IvyUnbindMsg(ivyPtr);
ivyPtr = NULL;
}
int pid = (int)getpid();
// Bind to the reply
ivyPtr = IvyBindMsg(on_WORLD_ENV, NULL, "^%d_%d (\\S*) WORLD_ENV (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", pid, seq);
// Send actual request
struct NpsFdm fdm_ivy;
memcpy(&fdm_ivy, &fdm, sizeof(struct NpsFdm));
IvySendMsg("nps %d_%d WORLD_ENV_REQ %f %f %f %f %f %f",
pid, seq,
DegOfRad(fdm_ivy.lla_pos_pprz.lat),
DegOfRad(fdm_ivy.lla_pos_pprz.lon),
(fdm_ivy.hmsl),
(fdm_ivy.ltpprz_pos.x),
(fdm_ivy.ltpprz_pos.y),
(fdm_ivy.ltpprz_pos.z));
seq++;
nps_ivy_send_world_env = false;
}
int find_launch_index(void)
{
static const char ap_launch[] = "aut_lau"; // short name
char *ap_settings[NB_SETTING] = SETTINGS_NAMES_SHORT;
// list through the settings
// TODO: maybe search for a substring with if(strstr(sent, word) != NULL)
for (uint8_t idx=0;idx<NB_SETTING;idx++) {
if (strcmp(ap_settings[idx],ap_launch) == 0) {
return (int)idx;
}
}
return -1;
}
static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
void *user_data __attribute__((unused)),
int argc __attribute__((unused)), char *argv[])
{
if (atoi(argv[1]) != AC_ID) {
return;
}
/* HACK:
* we actually don't want to allow changing settings if datalink is disabled,
* but since we currently change this variable via settings we have to allow it
* TODO: only allow changing the datalink_enabled setting
*/
uint8_t index = atoi(argv[2]);
float value = atof(argv[3]);
if (!datalink_enabled) {
DlSetting(index, value);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &index, &value);
}
printf("setting %d %f\n", index, value);
/*
* In case of HITL, update nps_autopilot.launch from DL_SETTINGS
* so the plane can be properly launched.
*
* In case of STIL nps_update_launch_from_dl() is an empty function
*/
if ((ap_launch_index >= 0) || (ap_launch_index < NB_SETTING)) {
if (index==ap_launch_index){
nps_update_launch_from_dl(value);
}
}
}
void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data)
{
struct NpsFdm fdm_ivy;
memcpy (&fdm_ivy, fdm_data, sizeof(struct NpsFdm));
struct NpsSensors sensors_ivy;
memcpy (&sensors_ivy, sensors_data, sizeof(struct NpsSensors));
IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
AC_ID,
DegOfRad(fdm_ivy.body_ecef_rotvel.p),
DegOfRad(fdm_ivy.body_ecef_rotvel.q),
DegOfRad(fdm_ivy.body_ecef_rotvel.r),
DegOfRad(fdm_ivy.ltp_to_body_eulers.phi),
DegOfRad(fdm_ivy.ltp_to_body_eulers.theta),
DegOfRad(fdm_ivy.ltp_to_body_eulers.psi));
IvySendMsg("%d NPS_POS_LLH %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm_ivy.lla_pos_pprz.lat),
(fdm_ivy.lla_pos_geod.lat),
(fdm_ivy.lla_pos_geoc.lat),
(fdm_ivy.lla_pos_pprz.lon),
(fdm_ivy.lla_pos_geod.lon),
(fdm_ivy.lla_pos_pprz.alt),
(fdm_ivy.lla_pos_geod.alt),
(fdm_ivy.agl),
(fdm_ivy.hmsl));
IvySendMsg("%d NPS_SPEED_POS %f %f %f %f %f %f %f %f %f",
AC_ID,
(fdm_ivy.ltpprz_ecef_accel.x),
(fdm_ivy.ltpprz_ecef_accel.y),
(fdm_ivy.ltpprz_ecef_accel.z),
(fdm_ivy.ltpprz_ecef_vel.x),
(fdm_ivy.ltpprz_ecef_vel.y),
(fdm_ivy.ltpprz_ecef_vel.z),
(fdm_ivy.ltpprz_pos.x),
(fdm_ivy.ltpprz_pos.y),
(fdm_ivy.ltpprz_pos.z));
IvySendMsg("%d NPS_GYRO_BIAS %f %f %f",
AC_ID,
DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.x) + sensors_ivy.gyro.bias_initial.x),
DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.y) + sensors_ivy.gyro.bias_initial.y),
DegOfRad(RATE_FLOAT_OF_BFP(sensors_ivy.gyro.bias_random_walk_value.z) + sensors_ivy.gyro.bias_initial.z));
/* transform magnetic field to body frame */
struct DoubleVect3 h_body;
double_quat_vmult(&h_body, &fdm_ivy.ltp_to_body_quat, &fdm_ivy.ltp_h);
IvySendMsg("%d NPS_SENSORS_SCALED %f %f %f %f %f %f",
AC_ID,
((sensors_ivy.accel.value.x - sensors_ivy.accel.neutral.x) / NPS_ACCEL_SENSITIVITY_XX),
((sensors_ivy.accel.value.y - sensors_ivy.accel.neutral.y) / NPS_ACCEL_SENSITIVITY_YY),
((sensors_ivy.accel.value.z - sensors_ivy.accel.neutral.z) / NPS_ACCEL_SENSITIVITY_ZZ),
h_body.x,
h_body.y,
h_body.z);
IvySendMsg("%d NPS_WIND %f %f %f",
AC_ID,
fdm_ivy.wind.x,
fdm_ivy.wind.y,
fdm_ivy.wind.z);
if(nps_ivy_send_world_env){
nps_ivy_send_WORLD_ENV_REQ();
}
}
![swh spinner](/static/img/swh-spinner.gif)
Computing file changes ...