/* Set guided mode setpoint
* Note: Offset position command in NED frame or body frame will only be implemented if
* local reference frame has been initialised.
* Flag definition:
bit 0: x,y as offset coordinates
bit 1: x,y in body coordinates
bit 2: z as offset coordinates
bit 3: yaw as offset coordinates
bit 4: free
bit 5: x,y as vel
bit 6: z as vel
bit 7: yaw as rate
*/
void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw)
{
/* only update setpoints when in guided mode */
if (autopilot_mode != AP_MODE_GUIDED) {
return;
}
// handle x,y
struct FloatVect2 setpoint = {.x = x, .y = y};
if (bit_is_set(flags, 5)) { // velocity setpoint
if (bit_is_set(flags, 1)) { // set velocity in body frame
guidance_h_set_guided_body_vel(setpoint.x, setpoint.y);
} else {
guidance_h_set_guided_vel(setpoint.x, setpoint.y);
}
} else { // position setpoint
if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint
guidance_h_set_guided_pos(setpoint.x, setpoint.y);
} else {
if (stateIsLocalCoordinateValid()) {
if (bit_is_set(flags, 1)) { // set position as offset in body frame
float psi = stateGetNedToBodyEulers_f()->psi;
setpoint.x = stateGetPositionNed_f()->x + cosf(-psi) * x + sinf(-psi) * y;
setpoint.y = stateGetPositionNed_f()->y - sinf(-psi) * x + cosf(-psi) * y;
} else { // set position as offset in NED frame
setpoint.x += stateGetPositionNed_f()->x;
setpoint.y += stateGetPositionNed_f()->y;
}
guidance_h_set_guided_pos(setpoint.x, setpoint.y);
}
}
}
//handle z
if (bit_is_set(flags, 6)) { // speed set-point
guidance_v_set_guided_vz(z);
} else { // position set-point
if (bit_is_set(flags, 2)) { // set position as offset in NED frame
if (stateIsLocalCoordinateValid()) {
z += stateGetPositionNed_f()->z;
guidance_v_set_guided_z(z);
}
} else {
guidance_v_set_guided_z(z);
}
}
//handle yaw
if (bit_is_set(flags, 7)) { // speed set-point
guidance_h_set_guided_heading_rate(yaw);
} else { // position set-point
if (bit_is_set(flags, 3)) { // set yaw as offset
yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later
}
guidance_h_set_guided_heading(yaw);
}
}
// Utility function: converts lla to local point
bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
// return FALSE if there is no valid local coordinate
// FIXME we should only test if local frame is initialized, not valid
if (!stateIsLocalCoordinateValid()) return FALSE;
// change geoid alt to ellipsoid alt
lla->alt = lla->alt - state.ned_origin_f.hmsl + state.ned_origin_f.lla.alt;
//Compute ENU components from LLA with respect to ltp origin
struct EnuCoor_f tmp_enu_point;
enu_of_lla_point_f(&tmp_enu_point, &state.ned_origin_f, lla);
//Bound the new waypoint with max distance from home
struct EnuCoor_f home;
ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]);
struct FloatVect2 vect_from_home;
VECT2_DIFF(vect_from_home, tmp_enu_point, home);
//Saturate the mission wp not to overflow max_dist_from_home
//including a buffer zone before limits
float dist_to_home = float_vect2_norm(&vect_from_home);
dist_to_home += BUFFER_ZONE_DIST;
if (dist_to_home > MAX_DIST_FROM_HOME) {
VECT2_SMUL(vect_from_home, vect_from_home, (MAX_DIST_FROM_HOME / dist_to_home));
}
// set new point
VECT2_SUM(*point, home, vect_from_home);
point->z = tmp_enu_point.z;
return TRUE;
}
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
{
if (autopilot_mode == AP_MODE_GUIDED && stateIsLocalCoordinateValid()) {
float x = stateGetPositionNed_f()->x + dx;
float y = stateGetPositionNed_f()->y + dy;
float z = stateGetPositionNed_f()->z + dz;
float heading = stateGetNedToBodyEulers_f()->psi + dyaw;
return autopilot_guided_goto_ned(x, y, z, heading);
}
return false;
}
void dl_parse_msg(void)
{
datalink_time = 0;
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING : {
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING : {
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
#if defined USE_NAVIGATION
case DL_BLOCK : {
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP : {
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) { break; }
if (stateIsLocalCoordinateValid()) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
lla.lat = DL_MOVE_WP_lat(dl_buffer);
lla.lon = DL_MOVE_WP_lon(dl_buffer);
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
waypoint_move_lla(wp_id, &lla);
}
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if defined GPS_DATALINK
case DL_REMOTE_GPS :
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
// Parse the GPS
parse_gps_datalink(
DL_REMOTE_GPS_numsv(dl_buffer),
DL_REMOTE_GPS_ecef_x(dl_buffer),
DL_REMOTE_GPS_ecef_y(dl_buffer),
DL_REMOTE_GPS_ecef_z(dl_buffer),
DL_REMOTE_GPS_lat(dl_buffer),
DL_REMOTE_GPS_lon(dl_buffer),
DL_REMOTE_GPS_alt(dl_buffer),
DL_REMOTE_GPS_hmsl(dl_buffer),
DL_REMOTE_GPS_ecef_xd(dl_buffer),
DL_REMOTE_GPS_ecef_yd(dl_buffer),
DL_REMOTE_GPS_ecef_zd(dl_buffer),
DL_REMOTE_GPS_tow(dl_buffer),
DL_REMOTE_GPS_course(dl_buffer));
break;
#endif
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}
void dl_parse_msg(void)
{
uint8_t msg_id = IdOfMsg(dl_buffer);
switch (msg_id) {
case DL_PING: {
DOWNLINK_SEND_PONG(DefaultChannel, DefaultDevice);
}
break;
case DL_SETTING : {
if (DL_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_SETTING_index(dl_buffer);
float var = DL_SETTING_value(dl_buffer);
DlSetting(i, var);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &var);
}
break;
case DL_GET_SETTING : {
if (DL_GET_SETTING_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t i = DL_GET_SETTING_index(dl_buffer);
float val = settings_get_value(i);
DOWNLINK_SEND_DL_VALUE(DefaultChannel, DefaultDevice, &i, &val);
}
break;
#ifdef USE_NAVIGATION
case DL_BLOCK : {
if (DL_BLOCK_ac_id(dl_buffer) != AC_ID) { break; }
nav_goto_block(DL_BLOCK_block_id(dl_buffer));
}
break;
case DL_MOVE_WP : {
uint8_t ac_id = DL_MOVE_WP_ac_id(dl_buffer);
if (ac_id != AC_ID) { break; }
if (stateIsLocalCoordinateValid()) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
lla.lat = DL_MOVE_WP_lat(dl_buffer);
lla.lon = DL_MOVE_WP_lon(dl_buffer);
/* WP_alt from message is alt above MSL in mm
* lla.alt is above ellipsoid in mm
*/
lla.alt = DL_MOVE_WP_alt(dl_buffer) - state.ned_origin_i.hmsl +
state.ned_origin_i.lla.alt;
waypoint_move_lla(wp_id, &lla);
}
}
break;
#endif /* USE_NAVIGATION */
#ifdef RADIO_CONTROL_TYPE_DATALINK
case DL_RC_3CH :
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_3ch_datalink(
DL_RC_3CH_throttle_mode(dl_buffer),
DL_RC_3CH_roll(dl_buffer),
DL_RC_3CH_pitch(dl_buffer));
break;
case DL_RC_4CH :
if (DL_RC_4CH_ac_id(dl_buffer) == AC_ID) {
#ifdef RADIO_CONTROL_DATALINK_LED
LED_TOGGLE(RADIO_CONTROL_DATALINK_LED);
#endif
parse_rc_4ch_datalink(DL_RC_4CH_mode(dl_buffer),
DL_RC_4CH_throttle(dl_buffer),
DL_RC_4CH_roll(dl_buffer),
DL_RC_4CH_pitch(dl_buffer),
DL_RC_4CH_yaw(dl_buffer));
}
break;
#endif // RADIO_CONTROL_TYPE_DATALINK
#if USE_GPS
#ifdef GPS_DATALINK
case DL_REMOTE_GPS_SMALL : {
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_SMALL_ac_id(dl_buffer) != AC_ID) { break; }
parse_gps_datalink_small(
DL_REMOTE_GPS_SMALL_numsv(dl_buffer),
DL_REMOTE_GPS_SMALL_pos_xyz(dl_buffer),
DL_REMOTE_GPS_SMALL_speed_xyz(dl_buffer),
DL_REMOTE_GPS_SMALL_heading(dl_buffer));
}
break;
case DL_REMOTE_GPS : {
// Check if the GPS is for this AC
if (DL_REMOTE_GPS_ac_id(dl_buffer) != AC_ID) { break; }
// Parse the GPS
parse_gps_datalink(
DL_REMOTE_GPS_numsv(dl_buffer),
DL_REMOTE_GPS_ecef_x(dl_buffer),
DL_REMOTE_GPS_ecef_y(dl_buffer),
DL_REMOTE_GPS_ecef_z(dl_buffer),
DL_REMOTE_GPS_lat(dl_buffer),
DL_REMOTE_GPS_lon(dl_buffer),
DL_REMOTE_GPS_alt(dl_buffer),
DL_REMOTE_GPS_hmsl(dl_buffer),
DL_REMOTE_GPS_ecef_xd(dl_buffer),
DL_REMOTE_GPS_ecef_yd(dl_buffer),
DL_REMOTE_GPS_ecef_zd(dl_buffer),
DL_REMOTE_GPS_tow(dl_buffer),
DL_REMOTE_GPS_course(dl_buffer));
}
break;
#endif // GPS_DATALINK
case DL_GPS_INJECT : {
// Check if the GPS is for this AC
if (DL_GPS_INJECT_ac_id(dl_buffer) != AC_ID) { break; }
// GPS parse data
gps_inject_data(
DL_GPS_INJECT_packet_id(dl_buffer),
DL_GPS_INJECT_data_length(dl_buffer),
DL_GPS_INJECT_data(dl_buffer)
);
}
break;
#endif // USE_GPS
case DL_GUIDED_SETPOINT_NED:
if (DL_GUIDED_SETPOINT_NED_ac_id(dl_buffer) != AC_ID) { break; }
uint8_t flags = DL_GUIDED_SETPOINT_NED_flags(dl_buffer);
float x = DL_GUIDED_SETPOINT_NED_x(dl_buffer);
float y = DL_GUIDED_SETPOINT_NED_y(dl_buffer);
float z = DL_GUIDED_SETPOINT_NED_z(dl_buffer);
float yaw = DL_GUIDED_SETPOINT_NED_yaw(dl_buffer);
switch (flags) {
case 0x00:
case 0x02:
/* local NED position setpoints */
autopilot_guided_goto_ned(x, y, z, yaw);
break;
case 0x01:
/* local NED offset position setpoints */
autopilot_guided_goto_ned_relative(x, y, z, yaw);
break;
case 0x03:
/* body NED offset position setpoints */
autopilot_guided_goto_body_relative(x, y, z, yaw);
break;
default:
/* others not handled yet */
break;
}
break;
default:
break;
}
/* Parse modules datalink */
modules_parse_datalink(msg_id);
}