rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

LP-291 Added MSP_SET_PID command.

Browse Source
This commit is contained in:
Vladimir Zidar 2016-04-20 18:29:44 +02:00 committed by Vladimir Zidar
parent db5807e54c
commit d281d8ce0c
1 changed files with 81 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
flight/modules/UAVOMSPBridge/UAVOMSPBridge.c
View File

@ -224,6 +224,7 @@ struct msp_bridge {
union { union {
uint8_t data[0]; uint8_t data[0];
// Specific packed data structures go here. // Specific packed data structures go here.
msp_pid_t piditems[PID_ITEM_COUNT];
} cmd_data; } cmd_data;
}; };
@ -255,11 +256,16 @@ static void msp_send(struct msp_bridge *m, uint8_t cmd, const uint8_t *data, siz
buf[4] = cmd; buf[4] = cmd;
PIOS_COM_SendBuffer(m->com, buf, sizeof(buf)); PIOS_COM_SendBuffer(m->com, buf, sizeof(buf));
PIOS_COM_SendBuffer(m->com, data, len);
if(len > 0)
{
PIOS_COM_SendBuffer(m->com, data, len);
for (unsigned i = 0; i < len; i++) {
cs ^= data[i];
}
}
for (unsigned i = 0; i < len; i++) {
cs ^= data[i];
}
cs ^= 0; cs ^= 0;
buf[0] = cs; buf[0] = cs;
@ -646,53 +652,101 @@ static void msp_send_boxids(struct msp_bridge *m) { // This is actually sending
msp_send( m, MSP_BOXIDS, msp_boxes, sizeof( msp_boxes ) ); msp_send( m, MSP_BOXIDS, msp_boxes, sizeof( msp_boxes ) );
} }
static void msp_send_pid(struct msp_bridge *m) static StabilizationSettingsFlightModeMapOptions get_current_stabilization_bank()
{ {
uint8_t fm; uint8_t fm;
ManualControlCommandFlightModeSwitchPositionGet(&fm); ManualControlCommandFlightModeSwitchPositionGet(&fm);
if( fm >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM ) if( fm >= FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM )
{ {
return; return STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1;
} }
StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM]; StabilizationSettingsFlightModeMapOptions flightModeMap[STABILIZATIONSETTINGS_FLIGHTMODEMAP_NUMELEM];
StabilizationSettingsFlightModeMapGet(flightModeMap); StabilizationSettingsFlightModeMapGet(flightModeMap);
StabilizationBankData bankData; return flightModeMap[fm];
}
switch(flightModeMap[fm])
static void get_current_stabilizationbankdata( StabilizationBankData *bankData )
{
switch( get_current_stabilization_bank() )
{ {
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1: case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1:
StabilizationSettingsBank1Get( ((StabilizationSettingsBank1Data *) &bankData ) ); StabilizationSettingsBank1Get( (StabilizationSettingsBank1Data *) bankData );
break; break;
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2: case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2:
StabilizationSettingsBank2Get( ((StabilizationSettingsBank2Data *) &bankData ) ); StabilizationSettingsBank2Get( (StabilizationSettingsBank2Data *) bankData );
break; break;
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3: case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3:
StabilizationSettingsBank3Get( ((StabilizationSettingsBank3Data *) &bankData ) ); StabilizationSettingsBank3Get( (StabilizationSettingsBank3Data *) bankData );
break; break;
} }
}
static void set_current_stabilizationbankdata( const StabilizationBankData *bankData )
{
// update just relevant parts. or not.
switch( get_current_stabilization_bank() )
{
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK1:
StabilizationSettingsBank1Set( (const StabilizationSettingsBank1Data *) bankData );
break;
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK2:
StabilizationSettingsBank2Set( (const StabilizationSettingsBank2Data *) bankData );
break;
case STABILIZATIONSETTINGS_FLIGHTMODEMAP_BANK3:
StabilizationSettingsBank3Set( (const StabilizationSettingsBank3Data *) bankData );
break;
}
}
static void pid_native2msp( const float *native, msp_pid_t *piditem )
{
piditem->P = native[0] * 10000;
piditem->I = native[1] * 10000;
piditem->D = native[2] * 10000;
}
static void pid_msp2native( const msp_pid_t *piditem, float *native )
{
native[0] = (float)piditem->P / 10000;
native[1] = (float)piditem->I / 10000;
native[2] = (float)piditem->D / 10000;
}
static void msp_send_pid(struct msp_bridge *m)
{
StabilizationBankData bankData;
get_current_stabilizationbankdata( &bankData );
msp_pid_t piditems[ PID_ITEM_COUNT ]; msp_pid_t piditems[ PID_ITEM_COUNT ];
memset(piditems, 0, sizeof( piditems ) ); memset(piditems, 0, sizeof( piditems ) );
piditems[ PIDROLL ].P = bankData.RollRatePID.Kp * 10000; pid_native2msp( (float *) &bankData.RollRatePID, &piditems[ PIDROLL ] );
piditems[ PIDROLL ].I = bankData.RollRatePID.Ki * 10000; pid_native2msp( (float *) &bankData.PitchRatePID, &piditems[ PIDPITCH ] );
piditems[ PIDROLL ].D = bankData.RollRatePID.Kd * 10000; pid_native2msp( (float *) &bankData.YawRatePID, &piditems[ PIDYAW ] );
piditems[ PIDPITCH ].P = bankData.PitchRatePID.Kp * 10000;
piditems[ PIDPITCH ].I = bankData.PitchRatePID.Ki * 10000;
piditems[ PIDPITCH ].D = bankData.PitchRatePID.Kd * 10000;
piditems[ PIDYAW ].P = bankData.YawRatePID.Kp * 10000;
piditems[ PIDYAW ].I = bankData.YawRatePID.Ki * 10000;
piditems[ PIDYAW ].D = bankData.YawRatePID.Kd * 10000;
msp_send( m, MSP_PID, (const uint8_t *) piditems, sizeof( piditems ) ); msp_send( m, MSP_PID, (const uint8_t *) piditems, sizeof( piditems ) );
} }
static void msp_set_pid(struct msp_bridge *m)
{
StabilizationBankData bankData;
get_current_stabilizationbankdata( &bankData );
pid_msp2native( &m->cmd_data.piditems[ PIDROLL ], (float *) &bankData.RollRatePID );
pid_msp2native( &m->cmd_data.piditems[ PIDPITCH ], (float *) &bankData.PitchRatePID );
pid_msp2native( &m->cmd_data.piditems[ PIDYAW ], (float *) &bankData.YawRatePID );
set_current_stabilizationbankdata( &bankData );
msp_send( m, MSP_SET_PID, 0, 0 ); // send ack.
}
#define ALARM_OK 0 #define ALARM_OK 0
#define ALARM_WARN 1 #define ALARM_WARN 1
#define ALARM_ERROR 2 #define ALARM_ERROR 2
@ -784,6 +838,9 @@ static msp_state msp_state_checksum(struct msp_bridge *m, uint8_t b)
case MSP_PID: case MSP_PID:
msp_send_pid(m); msp_send_pid(m);
break; break;
case MSP_SET_PID:
msp_set_pid(m);
break;
} }
return MSP_IDLE; return MSP_IDLE;